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')
