def internal_addFootOnly(self): mv = mechVariables() self.struct = mfStructure('leftAnkleOuterFront',[0,0,0],'leftAnkleOuterBack',[1,0,0]) s = self.struct s.defaultDisplayProperties = mv.mainSpanDispProp s.defaultDensity = mv.mainSpanDensity s.collisionMargin = mv.collisionMargin s.add1DJoint('leftAnkleInnerFront','leftAnkleOuterFront',0.5,[0,0,-0.5]) s.add1DJoint('leftAnkleInnerBack','leftAnkleOuterBack',0.5,[1,0,-0.5]) self.internal_addFoot( mv, 'left') #legBoneXWidth, legBoneZWidth,footLength,footWidth,footHeight,footDispProp,actDispProp,isLeftFoot):
def internal_addLegOnly(self): mv = mechVariables() self.struct = mfStructure('leftUpperFrontHipTruss',[0,0,0],'leftUpperBackHipTruss',[mv.hipTrussWidth,0,0]) s = self.struct s.defaultDisplayProperties = mv.mainSpanDispProp s.defaultDensity = mv.mainSpanDensity s.collisionMargin = mv.collisionMargin s.add1DJoint('leftLowerFrontHipTruss','leftUpperFrontHipTruss',mv.hipTrussHeight,[0.0,-1.0,0.0]) s.add1DJoint('leftLowerBackHipTruss','leftUpperBackHipTruss',mv.hipTrussHeight,[mv.hipTrussWidth,-1.0,0.0]) self.internal_addLeg(mv,True)
def test_joint_collisions(): p = True # -- LINES -- #No-check cases if checkForCollisionSurface(True,False,[location(0,0,0),location(1,1,1)],'NULL',[location(0,0,0),location(1,1,1)],'NULL',1.0): print "ERROR on joint collisions test 1" p = False if checkForCollisionSurface(False,True,[location(0,0,0),location(1,1,1)],'NULL',[location(0,0,0),location(1,1,1)],'NULL',1.0): print "ERROR on joint collisions test 2" p = False #Single line on top of itself if not checkForCollisionSurface(True,True,[location(0,0,0),location(1,1,1)],'NULL',[location(0,0,0),location(1,1,1)],'NULL',1.0): print "ERROR on joint collisions test 3" p = False #Short line within a large line if not checkForCollisionSurface(True,True,[location(0,0,0),location(1,0.0,0.0)],'NULL',[location(0.5,0,0),location(0.6,0,0)],'NULL',0.1): print "ERROR on joint collisions test 4" p = False #Short line away from the large one if checkForCollisionSurface(True,True,[location(0,0,0),location(1,0.0,0.0)],'NULL',[location(0.5,1,1),location(0.6,1,0)],'NULL',0.1): print "ERROR on joint collisions test 5" p = False #Line coming in from the end if not checkForCollisionSurface(True,True,[location(0,0,0),location(1,0.0,0.0)],'NULL',[location(1.5,0.01,0.01),location(0.6,0.01,0.01)],'NULL',0.1): print "ERROR on joint collisions test 6" p = False #Line coming in at an angle if not checkForCollisionSurface(True,True,[location(-2,-2,-2),location(2,2,2)],'NULL',[location(0.6,0.5,0.5),location(2,0,0)],'NULL',0.2): print "ERROR on joint collisions test 7" p = False #Line coming in at an angle but the radius is too small if checkForCollisionSurface(True,True,[location(-2,-2,-2),location(2,2,2)],'NULL',[location(0.6,0.5,0.5),location(2,0,0)],'NULL',0.07): print "ERROR on joint collisions test 8" p = False # -- Plane vs. line -- #Simple plane with line through it if not checkForCollisionSurface(True,True,[location(0,0,0),location(0,1,0)],[location(0,0,0),location(1,0,0)],[location(0.5,0.5,-1),location(0.5,0.5,1)],'NULL',0.07): print "ERROR on joint collisions test 9" p = False #Simple plane with line through it but missing if checkForCollisionSurface(True,True,[location(0,0,0),location(0,1,0)],[location(0,0,0),location(1,0,0)],[location(1,1,-1),location(1,1,1)],'NULL',0.07): print "ERROR on joint collisions test 10" p = False #Simple plane with line getting close to it if not checkForCollisionSurface(True,True,[location(0,0,0),location(0,1,0)],[location(0,0,0),location(1,0,0)],[location(0.5,0.5,0.1),location(0.5,0.5,1)],'NULL',0.2): print "ERROR on joint collisions test 11" p = False #Simple plane with line getting close to it but not close enough if checkForCollisionSurface(True,True,[location(0,0,0),location(0,1,0)],[location(0,0,0),location(1,0,0)],[location(0.5,0.5,0.3),location(0.5,0.5,1)],'NULL',0.2): print "ERROR on joint collisions test 12" p = False #Simple plane with angled line from below if not checkForCollisionSurface(True,True,[location(0,0,0),location(0,1,0)],[location(0,0,0),location(1,0,0)],[location(1,1,1),location(0,0,-1)],'NULL',0.2): print "ERROR on joint collisions test 13" p = False #Angled plane with line getting close if checkForCollisionSurface(True,True,[location(100,10,0),location(100,20,-40)],[location(100,10,0),location(100,0,40)],[location(0.5,0.5,0.3),location(0.5,0.5,1)],'NULL',0.2): print "ERROR on joint collisions test 14" p = False #Angled plane with line hitting and edge if not checkForCollisionSurface(True,True,[location(100,10,0),location(100,20,-40)],[location(100,10,0),location(100,0,40)],[location(0.5,0.5,0.3),location(100,0,40)],'NULL',0.2): print "ERROR on joint collisions test 15" p = False #Angled plane with line skimming the center if not checkForCollisionSurface(True,True,[location(100,10,0),location(0,20,-40)],[location(100,10,0),location(100,0,40)],[location(110,5.1,20),location(-10,5.0,20)],'NULL',0.2): print "ERROR on joint collisions test 16" p = False #Angled plane with line skimming the center if checkForCollisionSurface(True,True,[location(100,10,0),location(0,20,-40)],[location(100,10,0),location(100,0,40)],[location(110,5.3,20),location(-10,5.4,20)],'NULL',0.2): print "ERROR on joint collisions test 17" p = False # -- Plane on plane #Angled plane with angled plane larger than it if not checkForCollisionSurface(True,True,[location(100,10,0),location(0,20,-40)],[location(100,10,0),location(100,0,40)],[location(0,0,0),location(0,100,0)],[location(0,0,0),location(1000,0,0)],0.2): print "ERROR on joint collisions test 18" p = False #Angled plane with angled plane that doesn't intersect if checkForCollisionSurface(True,True,[location(100,10,0),location(0,20,-40)],[location(100,10,0),location(100,0,40)],[location(0,20,0),location(0,120,0)],[location(0,20,0),location(1000,20,0)],0.2): print "ERROR on joint collisions test 19" p = False #Angled plane with angled plane that's in range if not checkForCollisionSurface(True,True,[location(0,0,0),location(-100,0,0)],[location(0,0,0),location(0,0,-100)],[location(0,-0.1,0),location(-100,-0.1,0)],[location(0,-0.1,0),location(0,-0.1,-100)],0.2): print "ERROR on joint collisions test 20" p = False #Angled plane with angled plane that's not in range if checkForCollisionSurface(True,True,[location(0,0,0),location(-100,0,0)],[location(0,0,0),location(0,0,-100)],[location(0,-0.1,0),location(-100,-0.1,0)],[location(0,-0.1,0),location(0,-0.1,-100)],0.08): print "ERROR on joint collisions test 21" p = False #Angled plane with angled plane that's barely in range if not checkForCollisionSurface(True,True,[location(0,0,0),location(-100,0,0)],[location(0,0,0),location(0,0,-100)],[location(0,-0.1,0),location(-100,-0.1,0)],[location(0,-0.1,0),location(0,-0.1,-100)],0.1001): print "ERROR on joint collisions test 22" p = False #Angled plane with angled plane that's barely not in range if checkForCollisionSurface(True,True,[location(0,0,0),location(-100,0,0)],[location(0,0,0),location(0,0,-100)],[location(0,-0.1,0),location(-100,-0.1,0)],[location(0,-0.1,0),location(0,-0.1,-100)],0.0999): print "ERROR on joint collisions test 23" p = False # -- Now real objects -- #Object that has no collisions s = mfStructure('a',[0,0,0],'b',[1,0,0]) s.collisionMargin = 0.01 s.add2DJoint('c','a',1,'b',math.sqrt(2),[0,1,0],'f',cd=True) s.addJoint('d','a',1,'c',math.sqrt(2),'b',math.sqrt(2),cd=True) s.computeLocations() if s.checkForCollisions(): print "ERROR on joint collisions test 24" p = False s.addJoint('e','a',math.sqrt(2),'d',math.sqrt(3),'c',1,cd=True) s.computeLocations() if s.checkForCollisions(): print "ERROR on joint collisions test 25" p = False s.add1DJoint('f','a',10,'d') s.add2DJoint('g','f',1,'c',10,'a','f') s.add1DJoint('h','c',11,'g') s.addJoint('j','h',math.sqrt(11*11+1),'g',math.sqrt(10*10+1),'f',math.sqrt(10*10+1+1),cd=True) s.computeLocations() if not s.checkForCollisions(): print "ERROR on joint collisions test 26" p = False #if not j.internal_checkForCollisionEdge([location(0,0,0),location(1,1,1)],[location(0.0,0.0,0.0),location(1.0,1.0,1.0)],1.0): #if not j.internal_checkForCollisionEdge([location(0,0,0),location(1,1,1)],[location(0.0,0.0,0.0),location(1.0,1.0,1.0)],1.0): # print "ERROR on joint collisions test 4" # p = False #Check for various combinations of central collisions #Check for combinations of edge collisions #j.internal_checkForCollisionSurfaces(True,True,lineA,lineB,otherLineA,otherLineB,minDistance): #def internal_checkForCollisionPlane(self,lineA,lineB,colLine,minDistance,debugPrint=False): #def internal_checkForCollisionEdge(self,line,otherLine,minDistance): return p
def __init__(self): #Leg attributes actThighInit = 0.5 actCalfInit = 0.5 actRightThighInit = 0.5 actRightCalfInit = 0.5 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([200,200,255],2) rightLegDispProp = leftLegDispProp rightFootDispProp = displayProperties([100,100,255],2) leftFootDispProp = rightFootDispProp actDispProp = displayProperties([255,255,100],4) hipDispProp = displayProperties([255,100,100],2) #BUILD IT! self.struct = mfStructure('leftHipBack',[0,0,0],'leftHipActuator',[actHipDis,0,0]) s = self.struct #LEFT LEG s.add2DJoint('leftThighActuator','leftHipBack',actThighDis,'leftHipActuator',actThighInit,[0,1,0],'f',dpA=leftLegDispProp,dpB=actDispProp) self.leftThighActuator = s.getSpan('leftThighActuator','leftHipActuator') s.add1DJoint('leftKneeBack','leftHipBack',thighLen,'leftThighActuator',dpA=leftLegDispProp) s.add1DJoint('leftHipFront','leftHipActuator',legBoneDis+actHipDis,'leftHipBack',dpA=leftLegDispProp) s.add2DJoint('leftKneeFront','leftHipFront',thighLen,'leftKneeBack',legBoneDis,'leftHipBack','f',dp=leftLegDispProp) s.add2DJoint('leftKneeBrace','leftKneeBack',math.sqrt(1.0+legBoneDis*legBoneDis),'leftKneeFront',1.0,'leftHipBack','n',dp=leftLegDispProp) s.add2DJoint('leftKneeActuator','leftKneeBrace',math.sqrt(1.0+actKneeDis*actKneeDis),'leftKneeFront',actKneeDis,'leftKneeBack','f',dp=leftLegDispProp) s.add2DJoint('leftCalfActuator','leftKneeActuator',actCalfInit,'leftKneeFront',actCalfDis,'leftKneeBrace','f',dpA=actDispProp,dpB=leftLegDispProp) self.leftCalfActuator = s.getSpan('leftCalfActuator','leftKneeActuator') s.add1DJoint('leftAnkleFront','leftKneeFront',calfLen,'leftCalfActuator',dp=leftLegDispProp) s.add2DJoint('leftAnkleBack','leftAnkleFront',legBoneDis,'leftKneeBack',calfLen,'leftKneeActuator','f',dp=leftLegDispProp) #LEFT FOOT footHeight = 1.0 footThickness = 0.6 footPadWidth = 1.0 footWidth = 6.0 footLength = 8.0 s.add2DJoint('leftFootPivotBackTop','leftAnkleFront',sqrt(sq(legBoneDis)+sq(footHeight)),'leftAnkleBack',footHeight,'leftKneeFront','f',dpA=leftFootDispProp,dpB=actDispProp) self.leftAnkleActuator = s.getSpan('leftAnkleBack','leftKneeFront') s.add2DJoint('leftFootPivotFrontTop','leftAnkleFront',sqrt(sq(legBoneDis)+sq(footHeight)),'leftFootPivotBackTop',legBoneDis*2,'leftAnkleBack','f',dp=leftFootDispProp) s.add2DJoint('leftFootPivotFrontBottom','leftFootPivotFrontTop',footThickness,'leftFootPivotBackTop',sqrt(sq(footThickness)+sq(legBoneDis*2)),'leftAnkleFront','f',dp=leftFootDispProp) s.add2DJoint('leftFootPivotBackBottom','leftFootPivotBackTop',footThickness,'leftFootPivotFrontTop',sqrt(sq(footThickness)+sq(legBoneDis*2)),'leftAnkleBack','f',dp=leftFootDispProp) s.add1DJoint('leftFootToeFrontTop','leftFootPivotBackTop',footLength/2.0+legBoneDis,'leftFootPivotFrontTop',dp=leftFootDispProp) s.add1DJoint('leftFootToeFrontBottom','leftFootPivotBackBottom',footLength/2.0+legBoneDis,'leftFootPivotFrontBottom',dp=leftFootDispProp) s.add1DJoint('leftFootToeFrontBottomNear','leftFootPivotBackBottom',footLength/2.0-footPadWidth+legBoneDis,'leftFootPivotFrontBottom',dp=leftFootDispProp) s.addJoint('leftFootPointyToeFront','leftFootToeFrontBottomNear',sqrt(sq(footPadWidth)+sq(footWidth)),'leftFootToeFrontBottom',footWidth,'leftFootToeFrontTop',sqrt(sq(footWidth)+sq(footThickness)),dp=leftFootDispProp) s.add1DJoint('leftFootToeBackTop','leftFootPivotFrontTop',footLength/2.0+legBoneDis,'leftFootPivotBackTop',dp=leftFootDispProp) s.add1DJoint('leftFootToeBackBottom','leftFootPivotFrontBottom',footLength/2.0+legBoneDis,'leftFootPivotBackBottom',dp=leftFootDispProp) s.add1DJoint('leftFootToeBackBottomNear','leftFootPivotFrontBottom',footLength/2.0-footPadWidth+legBoneDis,'leftFootPivotBackBottom',dp=leftFootDispProp) s.addJoint('leftFootPointyToeBack','leftFootToeBackBottom',footWidth,'leftFootToeBackBottomNear',sqrt(sq(footPadWidth)+sq(footWidth)),'leftFootToeBackTop',sqrt(sq(footWidth)+sq(footThickness)),dp=leftFootDispProp) #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 s.computeLocations() s.addJoint('leftHipSupportBack','leftHipBack',legHipSupportLen,'leftHipActuator', sqrt( sq(legHipSupportLen) + sq(actHipDis)),'leftThighActuator', sqrt(sq(actThighDis)+sq(legHipSupportLen)),dp=leftLegDispProp) s.add1DJoint('leftSupportAttachFront','leftHipFront',actThighDis,'leftKneeFront',dp=leftLegDispProp) s.addJoint('leftHipSupportFront','leftHipFront',legHipSupportLen,'leftHipBack', sqrt(sq(legBoneDis)+sq(legHipSupportLen)),'leftSupportAttachFront', sqrt(sq(actThighDis)+sq(legHipSupportLen)),dp=leftLegDispProp) # Hip post s.addJoint('leftHipWingBack','leftThighActuator',sqrt(sq(actThighDis)+sq(legHipWingLen)),'leftHipBack',legHipWingLen,'leftHipFront',sqrt(sq(legBoneDis)+sq(legHipWingLen)),dp=leftLegDispProp) s.addJoint('leftHipPostUpper','leftHipBack',hipPostUpperPoint,'leftHipFront',sqrt(sq(legBoneDis)+sq(hipPostUpperPoint)),'leftHipWingBack',sqrt(sq(legHipWingLen)+sq(hipPostUpperPoint)),dp=leftLegDispProp) s.addJoint('leftHipActuatorPost','leftHipBack',sqrt(sq(hipPostUpperPoint)+sq(actHipDis)),'leftHipActuator',hipPostUpperPoint,'leftHipPostUpper',actHipDis,dp=leftLegDispProp) # Hip center s.addJoint('leftHipCockpitUpper','leftHipPostUpper',hipCockpitDis,'leftHipActuatorPost',actHipInit,'leftHipBack',sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),dpA=actDispProp,dpB=hipDispProp,dpC=hipDispProp) self.leftHipActuator = s.getSpan('leftHipCockpitUpper','leftHipPostUpper') s.addJoint('leftHipCockpitLower','leftHipCockpitUpper',hipPostUpperPoint,'leftHipBack',hipCockpitDis,'leftHipPostUpper',sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),dp=hipDispProp) s.add1DJoint('rightHipCockpitUpper','leftHipPostUpper',cockpitWidth+hipCockpitDis,'leftHipCockpitUpper',dp=hipDispProp) s.add1DJoint('rightHipCockpitLower','leftHipBack',cockpitWidth+hipCockpitDis,'leftHipCockpitLower',dp=hipDispProp) s.add2DJoint('rightHipPostUpper','rightHipCockpitUpper',hipCockpitDis,'rightHipCockpitLower',sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),'leftHipCockpitLower','f',dp=hipDispProp) s.add2DJoint('rightHipBack','rightHipCockpitLower',hipCockpitDis,'rightHipCockpitUpper',sqrt(sq(hipCockpitDis)+sq(hipPostUpperPoint)),'leftHipCockpitUpper','f',dp=hipDispProp) # Hip post s.addJoint('rightHipActuatorPost','rightHipBack',sqrt(sq(actHipDis)+sq(hipPostUpperPoint)),'rightHipPostUpper',actHipDis,'rightHipCockpitUpper',actHipInit,dpA=rightLegDispProp,dpB=rightLegDispProp,dpC=actDispProp) self.rightHipActuator = s.getSpan('rightHipCockpitUpper','leftHipPostUpper') s.addJoint('rightHipActuator','rightHipActuatorPost',hipPostUpperPoint,'rightHipBack',actHipDis,'rightHipPostUpper',sqrt(sq(hipPostUpperPoint)+sq(actHipDis)),dp=rightLegDispProp) #RIGHT LEG s.addJoint('rightHipWingBack','rightHipPostUpper',sqrt(sq(hipPostUpperPoint)+sq(legHipWingLen)),'rightHipActuator',sqrt(sq(legHipWingLen)+sq(actHipDis)),'rightHipBack',legHipWingLen,dp=rightLegDispProp) s.addJoint('rightThighActuator','rightHipActuator',actRightThighInit,'rightHipBack',actThighDis,'rightHipWingBack',sqrt(sq(legHipWingLen)+sq(actThighDis)),dpB=rightLegDispProp,dpA=actDispProp,dpC=rightLegDispProp) self.rightThighActuator = s.getSpan('rightThighActuator','rightHipActuator') s.add1DJoint('rightKneeBack','rightHipBack',thighLen,'rightThighActuator',dpA=rightLegDispProp) s.add1DJoint('rightHipFront','rightHipActuator',legBoneDis+actHipDis,'rightHipBack',dpA=rightLegDispProp) s.add2DJoint('rightKneeFront','rightHipFront',thighLen,'rightKneeBack',legBoneDis,'rightHipBack','f',dp=rightLegDispProp) s.add2DJoint('rightKneeBrace','rightKneeBack',math.sqrt(1.0+legBoneDis*legBoneDis),'rightKneeFront',1.0,'rightHipBack','n',dp=rightLegDispProp) s.add2DJoint('rightKneeActuator','rightKneeBrace',math.sqrt(1.0+actKneeDis*actKneeDis),'rightKneeFront',actKneeDis,'rightKneeBack','f',dp=rightLegDispProp) s.add2DJoint('rightCalfActuator','rightKneeActuator',actRightCalfInit,'rightKneeFront',actCalfDis,'rightKneeBrace','f',dpA=actDispProp,dpB=rightLegDispProp) self.rightCalfActuator = s.getSpan('rightCalfActuator','rightKneeActuator') s.add1DJoint('rightAnkleFront','rightKneeFront',calfLen,'rightCalfActuator',dp=rightLegDispProp) s.add2DJoint('rightAnkleBack','rightAnkleFront',legBoneDis,'rightKneeBack',calfLen,'rightKneeActuator','f',dp=rightLegDispProp) s.add1DJoint('rightSupportAttachFront','rightHipFront',actThighDis,'rightKneeFront',dp=rightLegDispProp) s.addJoint('rightHipSupportFront','rightHipBack',sqrt(sq(legBoneDis)+sq(legHipSupportLen)),'rightHipFront',legHipSupportLen,'rightSupportAttachFront',sqrt(sq(actThighDis)+sq(legHipSupportLen)),dp=rightLegDispProp) s.addJoint('rightHipSupportBack','rightHipActuator',sqrt(sq(actHipDis)+sq(legHipSupportLen)),'rightHipBack',legHipSupportLen,'rightThighActuator',sqrt(sq(actThighDis)+sq(legHipSupportLen)),dp=rightLegDispProp) #RIGHT FOOT s.add2DJoint('rightFootPivotBackTop','rightAnkleFront',sqrt(sq(legBoneDis)+sq(footHeight)),'rightAnkleBack',footHeight,'rightKneeFront','f',dpA=rightFootDispProp,dpB=actDispProp) self.rightAnkleActuator = s.getSpan('rightFootPivotBackTop','rightAnkleBack') s.add2DJoint('rightFootPivotFrontTop','rightAnkleFront',sqrt(sq(legBoneDis)+sq(footHeight)),'rightFootPivotBackTop',legBoneDis*2,'rightAnkleBack','f',dp=rightFootDispProp) s.add2DJoint('rightFootPivotFrontBottom','rightFootPivotFrontTop',footThickness,'rightFootPivotBackTop',sqrt(sq(footThickness)+sq(legBoneDis*2)),'rightAnkleFront','f',dp=rightFootDispProp) s.add2DJoint('rightFootPivotBackBottom','rightFootPivotBackTop',footThickness,'rightFootPivotFrontTop',sqrt(sq(footThickness)+sq(legBoneDis*2)),'rightAnkleBack','f',dp=rightFootDispProp) s.add1DJoint('rightFootToeFrontTop','rightFootPivotBackTop',footLength/2.0+legBoneDis,'rightFootPivotFrontTop',dp=rightFootDispProp) s.add1DJoint('rightFootToeFrontBottom','rightFootPivotBackBottom',footLength/2.0+legBoneDis,'rightFootPivotFrontBottom',dp=rightFootDispProp) s.add1DJoint('rightFootToeFrontBottomNear','rightFootPivotBackBottom',footLength/2.0-footPadWidth+legBoneDis,'rightFootPivotFrontBottom',dp=rightFootDispProp) s.addJoint('rightFootPointyToeFront','rightFootToeFrontBottom',footWidth,'rightFootToeFrontBottomNear',sqrt(sq(footPadWidth)+sq(footWidth)),'rightFootToeFrontTop',sqrt(sq(footWidth)+sq(footThickness)),dp=rightFootDispProp) s.add1DJoint('rightFootToeBackTop','rightFootPivotFrontTop',footLength/2.0+legBoneDis,'rightFootPivotBackTop',dp=rightFootDispProp) s.add1DJoint('rightFootToeBackBottom','rightFootPivotFrontBottom',footLength/2.0+legBoneDis,'rightFootPivotBackBottom',dp=rightFootDispProp) s.add1DJoint('rightFootToeBackBottomNear','rightFootPivotFrontBottom',footLength/2.0-footPadWidth+legBoneDis,'rightFootPivotBackBottom',dp=rightFootDispProp) s.addJoint('rightFootPointyToeBack','rightFootToeBackBottomNear',sqrt(sq(footPadWidth)+sq(footWidth)),'rightFootToeBackBottom',footWidth,'rightFootToeBackTop',sqrt(sq(footWidth)+sq(footThickness)),dp=rightFootDispProp)
def test_joint_locationComputation(): p = True #Test case: Exception in joint naming: not found t = False s = mfStructure('a',[0,0,0],'b',[0,0,1]) try: s.add2DJoint('c','a',1,'x',1,[0,0,10],'f') except Exception_UnknownJoint: t = True if t == False: print "ERROR on test case -1: Joint naming" p = False #Test case: Exception in joint naming: Already exists t = False s = mfStructure('a',[0,0,0],'b',[0,0,1]) try: s.add2DJoint('b','a',1,'b',1,[0,0,10],'f') except Exception_DuplicateNamedJoint: t = True if t == False: print "ERROR on test case 0: Joint naming" p = False #Test case: Rotation l1 = location(0,1.0/math.sqrt(2),1.0/math.sqrt(2)) #Y=1, Z=1 l2 = location(0,1.0/math.sqrt(2),-1.0/math.sqrt(2)) inv = jgh.rotateXToXYPlane(l1,[l2],yIsPos=True) if(l2.z < -1.1 or l2.z > -0.9 or l2.y > 0.1 or l2.y < -0.1 or l2.x > 0.1 or l2.x < -0.1): p = False print "ERROR on test case 1a: %s"%l2.toString() jgh.rotateXToXYPlane_reverse(l2,inv) if(l2.z < -0.8 or l2.z > -0.7 or l2.y > 0.8 or l2.y < 0.7): p = False print "ERROR on test case 1b: %s"%l2.toString() inv = jgh.rotateXToXYPlane(l1,[l2],yIsPos=False) if(l2.z < 0.9 or l2.z > 1.1 or l2.y > 0.1 or l2.y < -0.1 or l2.x > 0.1 or l2.x < -0.1): p = False print "ERROR on test case 1c: %s"%l2.toString() #Test case: Rotation l1 = location(0,100,-1) l2 = location(-1,-100,0) inv = jgh.rotateYToXYPlane(l1,[l2]) if(l2.z > -0.9 or l2.z < -1.1 or l2.x > 0.1 or l2. x < -0.1): p = False print "ERROR on test case 2: %s"%l2.toString() #Test case: Rotation l1 = location(-1.0,-1.0,0.0) l2 = location(1.0,-1.0,0) inv = jgh.rotateZToXZPlane(l1,[l2]) if(l2.z < -0.1 or l2.z > 0.1 or l2.x < -0.1 or l2.x > 0.1 or l2.y > math.sqrt(2)+0.01 or l2.y < math.sqrt(2)-0.01): p = False print "ERROR on test case 3: %s"%l2.toString() #Test case: Line j = joint() j.jointA = location(0,4,0) j.jointB = location(1,1,1) j.spanA = span( 2.0) j.computeLocation() if(j.x > 0.61 or j.x < 0.60 or j.y > 2.2 or j.y < 2.1 or j.z > 0.61 or j.z < 0.60): p = False print "ERROR on test case 4: %s"%j.toString() #Test case: 2D joint j = joint() j.jointA = location(1.0,1.0,0.0) j.jointB = location(0.0,1.0,0.0) j.jointC = location(0.0,1.0,0.0) j.spanA = span(math.sqrt(2.0)) j.spanB = span(1.0) j.farOrNear = 'f' j.computeLocation() if(j.x > 0.01 or j.x < -0.01 or j.y > 1.01 or j.y < -0.99 or j.z > 1.01 or j.z < 0.99): p = False print "ERROR on test case 5: %s"%j.toString() #Test case: 2D joint j = joint() j.jointA = location(0.0,1.0,0.0) j.jointB = location(4.0,1.0,0.0) j.jointC = location(0.5,6.0,-2.0) j.spanA = span(4.0) j.spanB = span(4.0) j.farOrNear = 'f' j.computeLocation() if(j.x > 2.01 or j.x < 1.99 or j.y < -2.22 or j.y > -2.21 or j.z > 1.29 or j.z < 1.285): p = False print "ERROR on test case 6: %s"%j.toString() #Test case: 2D joint j = joint() j.jointA = location(0.0,1.0,0.0) j.jointB = location(4.0,1.0,0.0) j.jointC = location(0.5,6.0,-2.0) j.spanA = span(4.0) j.spanB = span(4.0) j.farOrNear = 'n' j.computeLocation() if(j.x > 2.01 or j.x < 1.99 or j.y > 4.22 or j.y < 4.21 or j.z < -1.29 or j.z > -1.285): p = False print "ERROR on test case 7: %s"%j.toString() #Test case: 3D joint j = joint() j.jointA = location(1.0,0,0) j.jointB = location(0.0,1.0,0.0) j.jointC = location(0.0,0.0,1.0) j.spanA = span(1.0) j.spanB = span(math.sqrt(1+1+1)) j.spanC = span(1.0) j.computeLocation() if(j.x > 1.01 or j.x < 0.99 or j.y > 0.01 or j.y < -0.01 or j.z > 1.01 or j.z < 0.99): p = False print "ERROR on test case 8: %s"%j.toString() #Test case: 3D joint j = joint() j.jointA = location(0.0,2.0,2.0) j.jointB = location(-1.0,1.0,0.0) j.jointC = location(4.0,0.0,0.0) j.spanA = span(4.0) j.spanB = span(3.0) j.spanC = span(3.0) j.computeLocation() if(j.x > 1.198 or j.x < 1.19 or j.y > -1.013 or j.y < -1.014 or j.z > -0.34 or j.z < -0.35): p = False print "ERROR on test case 9: %s"%j.toString() #Test case: 3D joint j = joint() j.jointA = location(0,0,0) j.jointB = location(1,0,0) j.jointC = location(.5,-math.sqrt(3)/2.0,0.0) j.spanA = span(1.0) j.spanB = span(math.sqrt(2.0)) j.spanC = span(math.sqrt(2.0)) j.computeLocation() if(j.x > 0.01 or j.x < -0.01 or j.y > 0.01 or j.y < -0.01 or j.z > -0.99 or j.z < -1.01): p = False print "ERROR on test case 10: %s"%j.toString() return p
t += sq(arg) return sqrt(t) def sqrt(x): return math.sqrt(x) def sq(x): return math.pow(x,2.0) if __name__ == '__main__': #The simple struct totalLen = 30.0 mountLen = 10.0 refPt = [-1.0,0.0,0.0] subLen = totalLen/3.0 stretch = 1.001448 struct = mfStructure('lowerMount',[0,0,0],'upperMount',[0,mountLen,0]) struct.defaultDisplayProperties = displayProperties([255,100,255],2) struct.add2DJoint('lowerLoad','upperMount',stretch*diag(totalLen,mountLen),'lowerMount',totalLen,refPt,'f') struct.add2DJoint('upperLoad','upperMount',stretch*totalLen,'lowerLoad',mountLen,refPt,'f') struct.computeLocations() print "structure 1: upperLoad: %s"%struct.getJoint('upperLoad').toString() print "structure 1: lowerLoad: %s"%struct.getJoint('lowerLoad').toString() struct.defaultDisplayProperties = displayProperties([255,255,100],2) sectb = 3.0 struct.add2DJoint('lowerLoad1','upperMount',stretch*diag(totalLen/sectb,mountLen),'lowerMount',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad1','upperMount',stretch*totalLen/sectb,'lowerLoad1',mountLen,refPt,'f') struct.add2DJoint('lowerLoad2','upperLoad1',stretch*diag(totalLen/sectb,mountLen),'lowerLoad1',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad2','upperLoad1',stretch*totalLen/sectb,'lowerLoad2',mountLen,refPt,'f') struct.add2DJoint('lowerLoad3','upperLoad2',stretch*diag(totalLen/sectb,mountLen),'lowerLoad2',totalLen/sectb,refPt,'f') struct.add2DJoint('upperLoad3','upperLoad2',stretch*totalLen/sectb,'lowerLoad3',mountLen,refPt,'f')