I had to work a few more bugs out of the location computing and rendering systems. I think the image is still flipped but I'll deal with that later.
For now, I have the legs and hip sections modeled. Not just their current location, but even their kinematics are modeled in this rendering system: If I adjust the actuators the entire structure will move and react. And all in about 70 lines of code, including constants and comments.
#Leg attributes
actThighInit = 1.0
actCalfInit = 1.0
actHipDis = 1.0
actThighDis = 1.0
actKneeDis = 1.0
actCalfDis = 1.0
thighLen = 5.0
calfLen = 5.0
legBoneDis = 2.0
hipWidth = 8.0
#DISPLAY PROPS
leftLegDispProp = displayProperties([100,100,255],2)
rightLegDispProp = leftLegDispProp
actDispProp = displayProperties([255,255,100],4)
hipDispProp = displayProperties([255,100,100],2)
leftHipBack = location(0,0,0)
leftHipActuator = location(actHipDis,0,0)
self.struct = structureSeed(leftHipBack,leftHipActuator)
s = self.struct
#LEFT LEG
leftThighActuator = s.addJoint(leftHipBack,actThighDis,leftHipActuator,actThighInit,location(0,1,0),'f',dispPropA=leftLegDispProp,dispPropB=actDispProp)
leftKneeBack = s.addJoint(leftHipBack,thighLen,leftThighActuator,dispPropA=leftLegDispProp)
leftHipFront = s.addJoint(leftHipActuator,legBoneDis+actHipDis,leftHipBack,dispPropA=leftLegDispProp)
leftKneeFront = s.addJoint(leftHipFront,thighLen,leftKneeBack,legBoneDis,leftHipBack,'f',dispProp=leftLegDispProp)
leftKneeBrace = s.addJoint(leftKneeBack,math.sqrt(1.0+legBoneDis*legBoneDis),leftKneeFront,1.0,leftHipBack,'n',dispProp=leftLegDispProp)
leftKneeActuator = s.addJoint(leftKneeBrace,math.sqrt(1.0+actKneeDis*actKneeDis),leftKneeFront,actKneeDis,leftKneeBack,'f',dispProp=leftLegDispProp)
leftCalfActuator = s.addJoint(leftKneeActuator,actCalfInit,leftKneeFront,actCalfDis,leftKneeBrace,'f',dispPropA=actDispProp,dispPropB=leftLegDispProp)
leftAnkleFront = s.addJoint(leftKneeFront,calfLen,leftCalfActuator,dispProp=leftLegDispProp)
leftAnkleBack = s.addJoint(leftAnkleFront,legBoneDis,leftKneeBack,calfLen,leftKneeActuator,'f',dispProp=leftLegDispProp)
#HIP
legHipSupportLen = 1.0
legHipWingLen = 0.5
hipPostLowerPoint = 0.1
hipPostUpperPoint = 1.0
hipCockpitDis = 1.0
cockpitWidth = 6.0
actHipInit = sqrt(sq(hipCockpitDis)+sq(actHipDis))
# Main leg supports
leftHipSupportBack = s.addJoint(leftHipBack,legHipSupportLen,leftHipActuator, sqrt( sq(legHipSupportLen) + sq(actHipDis)),leftThighActuator, sqrt(sq(actThighDis)+sq(legHipSupportLen)),dispProp=leftLegDispProp)
leftSupportAttachFront = s.addJoint(leftHipFront,actThighDis,leftKneeFront,dispProp=leftLegDispProp)
leftHipSupportFront = s.addJoint(leftHipFront,legHipSupportLen,leftHipBack, sqrt(sq(legBoneDis)+sq(legHipSupportLen)),leftSupportAttachFront, sqrt(sq(actThighDis)+sq(legHipSupportLen)),dispProp=leftLegDispProp)
# Hip post
leftHipWingBack = s.addJoint(leftThighActuator,sqrt(sq(actThighDis)+sq(legHipWingLen)),leftHipBack,legHipWingLen,leftHipFront,sqrt(sq(legBoneDis)+sq(legHipWingLen)),dispProp=leftLegDispProp)
leftHipPostUpper = s.addJoint(leftHipBack,hipPostUpperPoint,leftHipFront,sqrt(sq(legBoneDis)+sq(hipPostUpperPoint)),leftHipWingBack,sqrt(sq(legHipWingLen)+sq(hipPostUpperPoint)),dispProp=leftLegDispProp)
leftHipActuatorPost = s.addJoint(leftHipBack,sqrt(sq(hipPostUpperPoint)+sq(actHipDis)),leftHipActuator,hipPostUpperPoint,leftHipPostUpper,actHipDis,dispProp=leftLegDispProp)
# Hip center
leftHipCockpitUpper = s.addJoint(leftHipPostUpper,hipCockpitDis,leftHipActuatorPost,actHipInit,leftHipBack,sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),dispPropA=actDispProp,dispPropB=hipDispProp,dispPropC=hipDispProp)
leftHipCockpitLower = s.addJoint(leftHipCockpitUpper,hipPostUpperPoint,leftHipBack,hipCockpitDis,leftHipPostUpper,sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),dispProp=hipDispProp)
rightHipCockpitUpper = s.addJoint(leftHipPostUpper,cockpitWidth+hipCockpitDis,leftHipCockpitUpper,dispProp=hipDispProp)
rightHipCockpitLower = s.addJoint(leftHipBack,cockpitWidth+hipCockpitDis, leftHipCockpitLower,dispProp=hipDispProp)
rightHipPostUpper = s.addJoint(rightHipCockpitUpper,hipCockpitDis,rightHipCockpitLower,sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),leftHipCockpitLower,'f',dispProp=hipDispProp)
rightHipBack = s.addJoint(rightHipCockpitLower,hipCockpitDis,rightHipCockpitUpper,sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),leftHipCockpitUpper,'f',dispProp=hipDispProp)
# Hip post
rightHipActuatorPost = s.addJoint(rightHipBack,sqrt(sq(actHipDis)+sq(hipPostUpperPoint)),rightHipPostUpper,actHipDis,rightHipCockpitUpper,actHipInit,dispPropA=rightLegDispProp,dispPropB=rightLegDispProp,dispPropC=actDispProp)
rightHipActuator = s.addJoint(rightHipActuatorPost,hipPostUpperPoint,rightHipBack,actHipDis,rightHipPostUpper,sqrt(sq(hipPostUpperPoint)+sq(actHipDis)),dispProp=rightLegDispProp)
#RIGHT LEG
rightHipWingBack = s.addJoint(rightHipPostUpper,sqrt(sq(hipPostUpperPoint)+sq(legHipWingLen)),rightHipActuator,sqrt(sq(legHipWingLen)+sq(actHipDis)),rightHipBack,legHipWingLen,dispProp=rightLegDispProp)
rightThighActuator = s.addJoint(rightHipActuator,actThighInit,rightHipBack,actThighDis,rightHipWingBack,sqrt(sq(legHipWingLen)+sq(actThighDis)),dispPropB=rightLegDispProp,dispPropA=actDispProp,dispPropC=rightLegDispProp)
rightKneeBack = s.addJoint(rightHipBack,thighLen,rightThighActuator,dispPropA=rightLegDispProp)
rightHipFront = s.addJoint(rightHipActuator,legBoneDis+actHipDis,rightHipBack,dispPropA=rightLegDispProp)
rightKneeFront = s.addJoint(rightHipFront,thighLen,rightKneeBack,legBoneDis,rightHipBack,'f',dispProp=rightLegDispProp)
rightKneeBrace = s.addJoint(rightKneeBack,math.sqrt(1.0+legBoneDis*legBoneDis),rightKneeFront,1.0,rightHipBack,'n',dispProp=rightLegDispProp)
rightKneeActuator = s.addJoint(rightKneeBrace,math.sqrt(1.0+actKneeDis*actKneeDis),rightKneeFront,actKneeDis,rightKneeBack,'f',dispProp=rightLegDispProp)
rightCalfActuator = s.addJoint(rightKneeActuator,actCalfInit,rightKneeFront,actCalfDis,rightKneeBrace,'f',dispPropA=actDispProp,dispPropB=rightLegDispProp)
rightAnkleFront = s.addJoint(rightKneeFront,calfLen,rightCalfActuator,dispProp=rightLegDispProp)
rightAnkleBack = s.addJoint(rightAnkleFront,legBoneDis,rightKneeBack,calfLen,rightKneeActuator,'f',dispProp=rightLegDispProp)
rightSupportAttachFront = s.addJoint(rightHipFront,actThighDis,rightKneeFront,dispProp=rightLegDispProp)
rightHipSupportFront = s.addJoint(rightHipBack,sqrt(sq(legBoneDis)+sq(legHipSupportLen)),rightHipFront,legHipSupportLen,rightSupportAttachFront,sqrt(sq(actThighDis)+sq(legHipSupportLen)),dispProp=rightLegDispProp)
rightHipSupportBack = s.addJoint(rightHipActuator,sqrt(sq(actHipDis)+sq(legHipSupportLen)),rightHipBack,legHipSupportLen,rightThighActuator,sqrt(sq(actThighDis)+sq(legHipSupportLen)),dispProp=rightLegDispProp)
No comments:
Post a Comment