def initializeJoints(self):
'''
Change control mode of joints to allow for torque control.
'''
self.sphericalJoints = []
self.revoluteJoints = []
for joint in range(self.numJoints):
jointType = p.getJointInfo(self.humanoidAgent, joint)[2]
if jointType == 2:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.humanoidAgent,
jointIndex=joint,
controlMode=p.POSITION_CONTROL,
targetVelocity=[0,0,0],
force=[1,1,1]
)
self.sphericalJoints.append(joint)
elif jointType == 0:
p.setJointMotorControl2(
bodyUniqueId=self.humanoidAgent,
jointIndex=joint,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=1
)
self.revoluteJoints.append(joint)
def loosenModel(self):
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
tireIndices = [
self.linkNameToID[name]
for name in ['frtire', 'fltire', 'brtire', 'bltire']
]
for i in range(nJoints):
if len(p.getJointStateMultiDof(bodyUniqueId=self.cliffordID,jointIndex=i,physicsClientId=self.physicsClientId)[0]) == 4 \
and not self.params["fixedSuspension"]:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=[0, 0, 0, 1],
positionGain=0,
velocityGain=0,
force=[0, 0, 0],
physicsClientId=self.physicsClientId)
dynamicsData = p.getDynamicsInfo(
self.cliffordID, i, physicsClientId=self.physicsClientId)
massScale = self.params["massScale"]
if i in tireIndices:
massScale = massScale * self.params['tireMassScale']
newMass = dynamicsData[0] * massScale
newInertia = [
dynamicsData[2][j] * massScale
for j in range(len(dynamicsData[2]))
]
p.changeDynamics(self.cliffordID,
i,
mass=newMass,
localInertiaDiagonal=newInertia,
linearDamping=0.2,
angularDamping=0.2,
restitution=0,
physicsClientId=self.physicsClientId)
springJointNames = [
'brslower2upper', 'blslower2upper', 'frslower2upper',
'flslower2upper'
]
springConstant = 0
springDamping = 0
maxSpringForce = 0
for name in springJointNames:
#JointInfo = p.getJointInfo(self.cliffordID,self.jointNameToID[name],physicsClientId=self.physicsClientId)
#p.setJointMotorControlArray(bodyUniqueId=self.cliffordID,
# jointIndices=[self.jointNameToID[name]],
# controlMode=p.POSITION_CONTROL,
# targetPositions=[0],
# positionGains=[springConstant],
# velocityGains=[springDamping],
# forces=[maxSpringForce],physicsClientId=self.physicsClientId)
if not self.params["fixedSuspension"]:
p.changeDynamics(self.cliffordID,
self.jointNameToID[name],
jointLowerLimit=self.params["susLowerLimit"],
jointUpperLimit=self.params["susUpperLimit"],
physicsClientId=self.physicsClientId)
def disable_body_motors(body_id):
for j in range(p.getNumJoints(body_id)):
ji = p.getJointInfo(body_id, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition = [0, 0, 0, 1]
p.setJointMotorControlMultiDof(body_id,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
p.setJointMotorControl2(body_id,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
def load_urdf(self):
"""Load the URDF of the blockstacker into the environment
The blockstacker URDF comes with its own dimensions and
textures, collidables.
"""
self.robot = p.loadURDF(Utilities.gen_urdf_path("blockstacker/urdf/blockstacker.urdf"),
[0, 0, 0.05], [0, 0, 0.9999383, 0.0111104], useFixedBase=False)
p.setJointMotorControlMultiDof(self.robot,
self.caster_link,
p.POSITION_CONTROL,
[0, 0, 0],
targetVelocity=[100000, 100000, 100000],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlArray(self.robot, self.flywheel_links, p.VELOCITY_CONTROL,
targetVelocities=[-2, 2],
forces=[1, 1])
def loosenModel(self):
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
for i in range(nJoints):
if len(
p.getJointStateMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
physicsClientId=self.physicsClientId)[0]) == 4:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=[0, 0, 0, 1],
positionGain=0,
velocityGain=0,
force=[0, 0, 0],
physicsClientId=self.physicsClientId)
springJointNames = [
'brslower2upper', 'blslower2upper', 'frslower2upper',
'flslower2upper'
]
springConstant = 0
springDamping = 0
maxSpringForce = 0
for name in springJointNames:
JointInfo = p.getJointInfo(self.cliffordID,
self.jointNameToID[name],
physicsClientId=self.physicsClientId)
p.setJointMotorControlArray(
bodyUniqueId=self.cliffordID,
jointIndices=[self.jointNameToID[name]],
controlMode=p.POSITION_CONTROL,
targetPositions=[0],
positionGains=[springConstant],
velocityGains=[springDamping],
forces=[maxSpringForce],
physicsClientId=self.physicsClientId)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
#print("chestRot=",chestRot)
p.setGravity(0,0,0)
kp=1
if (useMotionCapture):
p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=maxForce)
if (useMotionCaptureReset):
p.resetJointStateMultiDof(humanoid,chest,chestRot)
p.resetJointStateMultiDof(humanoid,neck,neckRot)
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
start_orn,
useFixedBase=True,
flags=humanoid_flag)
for i in range(-1, NLINK):
p.changeVisualShape(humanoid_id, i, rgbaColor=[1, 1, 1, 0.7])
nframe = sample_data['nframe']
frame_id = p.addUserDebugParameter("frame", 0, nframe, 0)
njoint = p.getNumJoints(humanoid_id)
for i in range(njoint):
p.setJointMotorControlMultiDof(humanoid_id,
i,
p.POSITION_CONTROL, [0, 0, 0, 1],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
joint_list = ['base', 'Rhip', 'Rknee', 'Rankle', 'Lhip', 'Lknee', 'Lankle']
spherical_ind = [1, 3, 4, 6]
revolute_ind = [2, 5]
target_orn = [[] for _ in range(NLINK)]
while (p.isConnected()):
target_frame = p.readUserDebugParameter(frame_id)
cur_frame = int(np.minimum(target_frame, nframe - 1))
next_frame = int(np.minimum(cur_frame + 1, nframe - 1))
frac = target_frame - cur_frame
controlMode=p.VELOCITY_CONTROL,
force=0)
p.setJointMotorControl2(linkage,
jointInfoList['right_eye_yolk_joint'],
controlMode=p.VELOCITY_CONTROL,
force=0)
p.setJointMotorControl2(linkage,
jointInfoList['right_eye_joint'],
controlMode=p.VELOCITY_CONTROL,
force=0)
# Spherical joint control
p.setJointMotorControlMultiDof(linkage,
jointInfoList['dogbone_joint_far_left'],
controlMode=p.POSITION_CONTROL,
targetPosition=[0],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlMultiDof(linkage,
jointInfoList['dogbone_joint_mid_left'],
controlMode=p.POSITION_CONTROL,
targetPosition=[0],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlMultiDof(linkage,
jointInfoList['dogbone_joint_mid_right'],
controlMode=p.POSITION_CONTROL,
targetPosition=[0],
def set_joint_motor_control_multi_dof(self, *args, **kwargs):
return pb.setJointMotorControlMultiDof(*args,
**kwargs,
physicsClientId=self.bullet_cid)
targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2,
j,
targetValue=targetPosition,
targetVelocity=targetVel)
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition = [0, 0, 0, 1]
p.setJointMotorControlMultiDof(humanoid,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlMultiDof(humanoid3,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[31, 31, 31])
p.setJointMotorControlMultiDof(humanoid4,
j,
p.POSITION_CONTROL,
targetPosition,
index0+=1
print("revolute:", targetPosition)
print("revolute velocity:", targetVel)
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j)
targetPosition=[0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition=[0,0,0,1]
p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0])
p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31])
p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1])
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
#print(ji)
print("joint[",j,"].type=",jointTypes[ji[2]])
print("joint[",j,"].name=",ji[1])
jointIds=[]
info = p.getJointInfo(robot, num)
if info[16] != -1:
p.setCollisionFilterPair(robot, robot, info[0], info[16], 0)
for j in range(p.getNumJoints(robot)):
ji = p.getJointInfo(robot, j)
print(ji[1], "=", j, " # type=", jointTypes[ji[2]])
targetPosition = [0]
jointType = ji[2]
if jointType == p.JOINT_SPHERICAL:
targetPosition = [0, 0, 0, 1]
p.setJointMotorControlMultiDof(robot,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[1, 1, 1])
if jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE:
p.setJointMotorControl2(robot,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=10)
# print("joint[", j, "].name=", ji[1], "joint[", j, "].type=", jointTypes[ji[2]])
RightUpLeg = 0 # type= JOINT_SPHERICAL
RightLeg = 1 # type= JOINT_SPHERICAL
RightFoot = 2 # type= JOINT_SPHERICAL
"../data/human_adult_scan.urdf",
#useFixedBase = 1,
flags=p.URDF_MAINTAIN_LINK_ORDER
| p.URDF_USE_SELF_COLLISION) #| means and here
#disable motors
for j in range(p.getNumJoints(human_adult_ID)):
ji = p.getJointInfo(human_adult_ID, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition = [0, 0, 0, 1]
p.setJointMotorControlMultiDof(human_adult_ID,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
p.setJointMotorControl2(human_adult_ID,
j,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=0)
p.resetBasePositionAndOrientation(
human_adult_ID, [0, 0, 1.3], p.getQuaternionFromEuler([math.pi / 2, 0, 0]))
#for j in range (p.getNumJoints(human_adult_ID)):
# info = p.getDynamicsInfo(human_adult_ID, j)
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrns,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
###############################################################################
# Next we define the frictional force between the joints of wrecking ball.
friction_vec = [joint_friction] * 3 # same all axis
control_mode = p.POSITION_CONTROL # set pos control mode
for j in range(p.getNumJoints(rope)):
p.setJointMotorControlMultiDof(rope,
j,
control_mode,
targetPosition=[0, 0, 0, 1],
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=friction_vec)
###############################################################################
# We add the following constraint to keep the cubical hinge fixed.
root_robe_c = p.createConstraint(rope, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 2])
box_actor = actor.box(centers=np.array([[0, 0, 0]]),
directions=np.array([[0, 0, 0]]),
scales=(0.02, 0.02, 0.02),
colors=np.array([[1, 0, 0]]))
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7
taus = exPD.computePD(pole, [0, 1], [
basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
desiredPosCart, desiredPosPole
], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
[0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
[0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole,
j,
controlMode=p.TORQUE_CONTROL,
force=[taus[j + baseDof]])
#p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
if (pd >= 0):
link = 0
p.setJointMotorControl2(bodyUniqueId=pole2,
jointIndex=link,
controlMode=p.PD_CONTROL,
targetPosition=desiredPosCart,
targetVelocity=desiredVelCart,
force=maxForceCart,
positionGain=kpCart,
velocityGain=kdCart)
link = 1
p.setJointMotorControl2(bodyUniqueId=pole2,
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7
taus = exPD.computePD(pole, [0, 1], [
basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2],
baseOrn[3], desiredPosCart, desiredPosPole
], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole],
[0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
[0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
[0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole],
timeStep)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole,
j,
controlMode=p.TORQUE_CONTROL,
force=[taus[j + baseDof]])
#p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
if (pd >= 0):
link = 0
p.setJointMotorControl2(bodyUniqueId=pole2,
jointIndex=link,
controlMode=p.PD_CONTROL,
targetPosition=desiredPosCart,
targetVelocity=desiredVelCart,
force=maxForceCart,
positionGain=kpCart,
velocityGain=kdCart)
link = 1
p.setJointMotorControl2(bodyUniqueId=pole2,
gravzId = p.addUserDebugParameter("grav_y",-20,20,-10)
index= 0
spherical_index = -1
for j in range(p.getNumJoints(humanoid)):
if index<7:
ji = p.getJointInfo(humanoid, j)
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
index+=4
p.resetJointStateMultiDof(humanoid, j, targetValue=[0,0,0,1], targetVelocity=[0,0,0])
#p.changeDynamics(humanoid,j,angularDamping=0, linearDamping=0)
spherical_index = j
p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL,
targetPosition=[0,0,0,1], positionGain=0.2,
targetVelocity=[0,0,0], velocityGain=0,
force=[0,0,0])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
index+=1
p.resetJointStateMultiDof(humanoid, j, targetValue=[0], targetVelocity=[0])
p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL,
targetPosition=[0], targetVelocity=[0], force=[0])
p.loadURDF("plane.urdf")
p.setRealTimeSimulation(1)
while p.isConnected():
gravX = p.readUserDebugParameter(gravxId)
gravY = p.readUserDebugParameter(gravyId)
gravZ = p.readUserDebugParameter(gravzId)
