def addUniversalJoint(self,
body1,
body2,
anchor,
axis1,
axis2,
loStop1=-ode.Infinity,
hiStop1=ode.Infinity,
loStop2=-ode.Infinity,
hiStop2=ode.Infinity):
anchor = add3(anchor, self.offset)
joint = ode.UniversalJoint(self.sim.world)
joint.attach(body1, body2)
joint.setAnchor(anchor)
joint.setAxis1(axis1)
joint.setAxis2(axis2)
joint.setParam(ode.ParamLoStop, loStop1)
joint.setParam(ode.ParamHiStop, hiStop1)
joint.setParam(ode.ParamLoStop2, loStop2)
joint.setParam(ode.ParamHiStop2, hiStop2)
joint.style = "univ"
self.joints.append(joint)
return joint
def create_universal(self,key,pos,bod_keys,axis1 = [1,0,0],axis2 = [0,0,1],
loStop1 = -ode.Infinity, hiStop1 = ode.Infinity,
loStop2 = -ode.Infinity, hiStop2 = ode.Infinity,
fmax = -1, fmax2 = -1, world_joint=0):
""" Create a universal joint.
Arguments:
@param key : number id to assign the hinge
@param pos : position of the joint
@param bod_keys : list of two bodies to attach the joint to
@param axis1 : first axis of hinge
@param axis2 : second axis of hinge
@param loStop1 : low limit of hinge 1
@param hiStop1 : high limit of hinge 1
@param loStop2 : low limit of hinge 2
@param hiStop2 : high limit of hinge 2
@param fmax : max output potential for first hinge
@param fmax2 : max output potential for second hinge
@param world_joint: if the body is to be connected to the world
"""
# Auto label the joint key or not.
key = len(self.joints) if key == -1 else key
if(pos == "None"):
anchor = self.bodies[bod_keys[0]].getRelPointPos(
(0.,-self.bodies[bod_keys[0]].length/2.,0.))
else:
anchor = pos
j = ode.UniversalJoint(self.world)
if(world_joint==0):
j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]])
else:
j.attach(self.bodies[bod_keys[0]], ode.environment)
j.setAnchor((anchor))
j.setAxis1((axis1))
j.setAxis2((axis2))
j.setParam(ode.ParamLoStop, loStop1-(self.joint_cushion*ANG_TO_RAD))
j.setParam(ode.ParamHiStop, hiStop1+(self.joint_cushion*ANG_TO_RAD))
j.setParam(ode.ParamLoStop2, loStop2-(self.joint_cushion*ANG_TO_RAD))
j.setParam(ode.ParamHiStop2, hiStop2+(self.joint_cushion*ANG_TO_RAD))
j.setParam(ode.ParamFudgeFactor,.5)
j.setParam(ode.ParamFudgeFactor2,.5)
if fmax == -1:
j.setParam(ode.ParamFMax, 10.)
else:
j.setParam(ode.ParamFMax, fmax)
if fmax2 == -1:
j.setParam(ode.ParamFMax2, 10.)
else:
j.setParam(ode.ParamFMax2, fmax2)
j.style = "universal"
self.joints[key] = j
return key
def create_flexible_universal(self,key,pos,bod_keys,axis1 = [1,0,0],axis2 = [0,0,1],
erp1 = 0.1, cfm1=0.1, erp2 = 0.1, cfm2 = 0.2):
""" Create a universal joint.
Arguments:
@param key : number id to assign the hinge
@param pos : position of the joint
@param bod_keys : list of two bodies to attach the joint to
@param axis1 : first axis of hinge
@param axis2 : second axis of hinge
@param erp1 : erp hinge 1
@param cfm1 : cfm hinge 1
@param erp2 : erp hinge 2
@param cfm2 : cfm hinge 2
"""
# Auto label the joint key or not.
key = len(self.joints) if key == -1 else key
if(pos == "None"):
anchor = self.bodies[bod_keys[0]].getRelPointPos(
(0.,-self.bodies[bod_keys[0]].length/2.,0.))
else:
anchor = pos
j = ode.UniversalJoint(self.world)
j.attach(self.bodies[bod_keys[0]], self.bodies[bod_keys[1]])
j.setAnchor((anchor))
j.setAxis1((axis1))
j.setAxis2((axis2))
j.setParam(ode.ParamLoStop, 0)
j.setParam(ode.ParamHiStop, 0)
j.setParam(ode.ParamLoStop2, 0)
j.setParam(ode.ParamHiStop2, 0)
j.setParam(ode.ParamStopERP, erp1)
j.setParam(ode.ParamStopCFM, cfm1)
j.setParam(ode.ParamStopERP2, erp2)
j.setParam(ode.ParamStopCFM2, cfm2)
j.setParam(ode.ParamFudgeFactor,.5)
j.setParam(ode.ParamFudgeFactor2,.5)
j.setParam(ode.ParamFMax, 10.)
j.setParam(ode.ParamFMax2, 10.)
j.style = "universal_flex"
self.joints[key] = j
return key
def end(name):
if (name == 'universal'):
joint = ode.UniversalJoint(world, self._jg)
joint.attach(link1, link2)
if (anchor[0] is not None):
joint.setAnchor(anchor[0])
if (len(axes) != 2):
raise errors.InvalidError('Wrong number of axes for '
' universal joint.')
joint.setAxis1(self._parser.parseVector(axes[0]))
self._applyAxisParams(joint, 0, axes[0])
joint.setAxis2(self._parser.parseVector(axes[1]))
self._applyAxisParams(joint, 1, axes[1])
self.setODEObject(joint)
self._parser.pop()
# create a space object ( a collision space) space = ode.Space() # create a plane.py geom for the floor (infinite plane.py) floor = ode.GeomPlane(space, (0, 1, 0), 0) # a list with ODE bodies ( a body is a rigid body ) bodies = [] # a joint group for the contact joints that are generated whenever two bodies # collide contactgroup = ode.JointGroup() # crreate a joint between the ball and the stick j1 = ode.UniversalJoint(world) # simulation loop variables fps = 50 dt = 1.0 / fps running = True lasttime = time.time() state = 0 counter = 0 theta2 = 0 e1 = 0 e2 = 0 e3 = 0 P_const = 40.0 I_const = 0.0 D_const = 0.0
def _createJoint(self, joint, parentT, posture):
P = mm.TransVToSE3(joint.offset)
T = numpy.dot(parentT, P)
# R = mm.SO3ToSE3(posture.localRMap[joint.name])
R = mm.SO3ToSE3(posture.localRs[posture.skeleton.getElementIndex(
joint.name)])
T = numpy.dot(T, R)
temp_joint = joint
nodeExistParentJoint = None
while True:
if temp_joint.parent == None:
nodeExistParentJoint = None
break
elif temp_joint.parent.name in self.nodes:
nodeExistParentJoint = temp_joint.parent
break
else:
temp_joint = temp_joint.parent
# if joint.parent and len(joint.children)>0:
if nodeExistParentJoint and len(joint.children) > 0:
if joint.name in self.config.nodes:
p = mm.SE3ToTransV(T)
node = self.nodes[joint.name]
cfgNode = self.config.getNode(joint.name)
if cfgNode.jointAxes != None:
if len(cfgNode.jointAxes) == 0:
node.joint = ode.BallJoint(self.world)
node.motor = ode.AMotor(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
# node.motor.attach(self.nodes[joint.parent.name].body, node.body)
node.motor.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.motor.setNumAxes(3)
node.motor.setAxis(0, 0, (1, 0, 0))
node.motor.setAxis(1, 0, (0, 1, 0))
node.motor.setAxis(2, 0, (0, 0, 1))
node.motor.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
node.motor.setParam(ode.ParamLoStop2,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop2,
cfgNode.jointHiStop)
node.motor.setParam(ode.ParamLoStop3,
cfgNode.jointLoStop)
node.motor.setParam(ode.ParamHiStop3,
cfgNode.jointHiStop)
elif len(cfgNode.jointAxes) == 1:
node.joint = ode.HingeJoint(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
newR = mm.SE3ToSO3(self.newTs[joint.name])
node.joint.setAxis(
numpy.dot(newR, cfgNode.jointAxes[0]))
node.joint.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
elif len(cfgNode.jointAxes) == 2:
pass
node.joint = ode.UniversalJoint(self.world)
#
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body,
node.body)
node.joint.setAnchor(p)
newR = mm.SE3ToSO3(self.newTs[joint.name])
node.joint.setAxis1(
numpy.dot(newR, cfgNode.jointAxes[0]))
node.joint.setAxis2(
numpy.dot(newR, cfgNode.jointAxes[1]))
node.joint.setParam(ode.ParamLoStop,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop,
cfgNode.jointHiStop)
node.joint.setParam(ode.ParamLoStop2,
cfgNode.jointLoStop)
node.joint.setParam(ode.ParamHiStop2,
cfgNode.jointHiStop)
else:
node.joint = ode.FixedJoint(self.world)
# node.joint.attach(self.nodes[joint.parent.name].body, node.body)
node.joint.attach(
self.nodes[nodeExistParentJoint.name].body, node.body)
# node.joint.setFixed()
node.Kp = cfgNode.Kp
node.Kd = cfgNode.Kd
else:
pass
for childJoint in joint.children:
self._createJoint(childJoint, T, posture)
def _createODEjoint(self):
# Create the ODE joint
self.odejoint = ode.UniversalJoint(self.odedynamics.world)