revert to original humanoidMotionCapture.py example. Add a showJointMotorTorques variable (false by default)
This commit is contained in:
erwincoumans
@@ -10,14 +10,15 @@ else:
|
||||
|
||||
useZUp = False
|
||||
useYUp = not useZUp
|
||||
showJointMotorTorques = False
|
||||
|
||||
if useYUp:
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
|
||||
|
||||
|
||||
from pdControllerExplicit import PDControllerExplicitMultiDof
|
||||
from pdControllerStable import PDControllerStableMultiDof
|
||||
|
||||
|
||||
explicitPD = PDControllerExplicitMultiDof(p)
|
||||
stablePD = PDControllerStableMultiDof(p)
|
||||
|
||||
p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
|
||||
@@ -26,7 +27,7 @@ import pybullet_data
|
||||
p.setTimeOut(10000)
|
||||
useMotionCapture=False
|
||||
useMotionCaptureReset=False#not useMotionCapture
|
||||
|
||||
useExplicitPD = True
|
||||
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.setPhysicsEngineParameter(numSolverIterations=30)
|
||||
@@ -76,8 +77,14 @@ p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,s
|
||||
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
|
||||
|
||||
humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
|
||||
humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
|
||||
humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
|
||||
humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
|
||||
|
||||
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
|
||||
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
|
||||
humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
|
||||
humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
|
||||
|
||||
|
||||
startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
|
||||
@@ -90,6 +97,10 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-
|
||||
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
|
||||
|
||||
p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
|
||||
p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
|
||||
p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
|
||||
p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
|
||||
|
||||
|
||||
|
||||
index0=7
|
||||
@@ -104,6 +115,7 @@ for j in range (p.getNumJoints(humanoid)):
|
||||
print("spherical position: ",targetPosition)
|
||||
print("spherical velocity: ",targetVel)
|
||||
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
|
||||
p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
|
||||
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
|
||||
targetPosition=[startPose[index0]]
|
||||
targetVel = [startVel[index0]]
|
||||
@@ -111,6 +123,9 @@ for j in range (p.getNumJoints(humanoid)):
|
||||
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)):
|
||||
@@ -120,9 +135,13 @@ for j in range (p.getNumJoints(humanoid)):
|
||||
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]])
|
||||
@@ -181,6 +200,7 @@ p.getCameraImage(320,200)
|
||||
|
||||
|
||||
|
||||
|
||||
while (p.isConnected()):
|
||||
|
||||
if useGUI:
|
||||
@@ -202,6 +222,11 @@ while (p.isConnected()):
|
||||
frameNext = frame
|
||||
|
||||
frameFraction = frameReal - frame
|
||||
#print("frameFraction=",frameFraction)
|
||||
#print("frame=",frame)
|
||||
#print("frameNext=", frameNext)
|
||||
|
||||
#getQuaternionSlerp
|
||||
|
||||
frameData = motion_dict['Frames'][frame]
|
||||
frameDataNext = motion_dict['Frames'][frameNext]
|
||||
@@ -218,41 +243,17 @@ while (p.isConnected()):
|
||||
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
|
||||
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
|
||||
#pre-rotate to make z-up
|
||||
|
||||
|
||||
if (useZUp):
|
||||
y2zPos=[0,0,0.0]
|
||||
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
|
||||
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
|
||||
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
|
||||
|
||||
|
||||
once=False
|
||||
#print("chestRot=",chestRot)
|
||||
p.setGravity(0,0,0)
|
||||
|
||||
kp=kpMotor
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
basePos1Start = [frameData[1],frameData[2],frameData[3]]
|
||||
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
|
||||
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
|
||||
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
|
||||
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
|
||||
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
|
||||
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
|
||||
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
|
||||
#pre-rotate to make z-up
|
||||
if (useZUp):
|
||||
y2zPos=[0,0,0.0]
|
||||
y2zPos=[0,2,0.0]
|
||||
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
|
||||
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
|
||||
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
|
||||
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
|
||||
|
||||
|
||||
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
|
||||
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
|
||||
@@ -301,12 +302,45 @@ while (p.isConnected()):
|
||||
leftElbowRotEnd = [frameDataNext[43]]
|
||||
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
|
||||
|
||||
if (0):#if (once):
|
||||
p.resetJointStateMultiDof(humanoid,chest,chestRot)
|
||||
p.resetJointStateMultiDof(humanoid,neck,neckRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
|
||||
once=False
|
||||
|
||||
|
||||
#print("chestRot=",chestRot)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
kp=kpMotor
|
||||
if (useExplicitPD):
|
||||
jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
|
||||
#[x,y,z] base position and [x,y,z,w] base orientation!
|
||||
totalDofs =7
|
||||
for dof in jointDofCounts:
|
||||
totalDofs += dof
|
||||
|
||||
jointIndicesAll = [
|
||||
chest,
|
||||
neck,
|
||||
rightHip,
|
||||
rightKnee,
|
||||
rightAnkle,
|
||||
rightShoulder,
|
||||
rightElbow,
|
||||
leftHip,
|
||||
leftKnee,
|
||||
leftAnkle,
|
||||
leftShoulder,
|
||||
leftElbow
|
||||
]
|
||||
basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
|
||||
pose = [ basePos[0],basePos[1],basePos[2],
|
||||
baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
|
||||
@@ -323,27 +357,12 @@ while (p.isConnected()):
|
||||
leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
|
||||
leftElbowRot[0] ]
|
||||
|
||||
jointIndicesAll = [
|
||||
chest,
|
||||
neck,
|
||||
rightHip,
|
||||
rightKnee,
|
||||
rightAnkle,
|
||||
rightShoulder,
|
||||
rightElbow,
|
||||
leftHip,
|
||||
leftKnee,
|
||||
leftAnkle,
|
||||
leftShoulder,
|
||||
leftElbow
|
||||
]
|
||||
dofIndex=7
|
||||
jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
|
||||
totalDofs = sum(jointDofCounts)+7
|
||||
print("total=",totalDofs)
|
||||
|
||||
useStablePD=False
|
||||
if (useStablePD):
|
||||
#print("pose=")
|
||||
#for po in pose:
|
||||
# print(po)
|
||||
|
||||
|
||||
taus = stablePD.computePD(bodyUniqueId=humanoid,
|
||||
jointIndices = jointIndicesAll,
|
||||
desiredPositions = pose,
|
||||
@@ -353,39 +372,68 @@ while (p.isConnected()):
|
||||
maxForces = [maxForce]*totalDofs,
|
||||
timeStep=timeStep)
|
||||
|
||||
taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
|
||||
jointIndices = jointIndicesAll,
|
||||
desiredPositions = pose,
|
||||
desiredVelocities = [0]*totalDofs,
|
||||
kps = kpOrg,
|
||||
kds = kdOrg,
|
||||
maxForces = [maxForce*0.05]*totalDofs,
|
||||
timeStep=timeStep)
|
||||
|
||||
#taus=[0]*43
|
||||
|
||||
dofIndex=7
|
||||
for index in range (len(jointIndicesAll)):
|
||||
jointIndex = jointIndicesAll[index]
|
||||
if jointDofCounts[index]==4:
|
||||
|
||||
p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
|
||||
p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]])
|
||||
|
||||
if jointDofCounts[index]==1:
|
||||
|
||||
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
|
||||
p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]])
|
||||
|
||||
dofIndex+=jointDofCounts[index]
|
||||
else:
|
||||
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])
|
||||
|
||||
#print("len(taus)=",len(taus))
|
||||
#print("taus=",taus)
|
||||
|
||||
|
||||
p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
|
||||
p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
|
||||
|
||||
kinematicHumanoid4 = True
|
||||
if (kinematicHumanoid4):
|
||||
p.resetJointStateMultiDof(humanoid4,chest,chestRot)
|
||||
p.resetJointStateMultiDof(humanoid4,neck,neckRot)
|
||||
p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot)
|
||||
p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot)
|
||||
p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot)
|
||||
p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
|
||||
p.stepSimulation()
|
||||
for j in range(p.getNumJoints(humanoid)):
|
||||
jointState = p.getJointStateMultiDof(humanoid,j)
|
||||
|
||||
if showJointMotorTorques:
|
||||
for j in range(p.getNumJoints(humanoid2)):
|
||||
jointState = p.getJointStateMultiDof(humanoid2,j)
|
||||
print("jointStateMultiDof[",j,"].pos=",jointState[0])
|
||||
print("jointStateMultiDof[",j,"].vel=",jointState[1])
|
||||
print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
|
||||
|
||||
#j=4
|
||||
#jointState = p.getJointStateMultiDof(humanoid,j)
|
||||
#print("jointStateMultiDof[",j,"].pos=",jointState[0])
|
||||
#print("jointStateMultiDof[",j,"].vel=",jointState[1])
|
||||
#print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
|
||||
time.sleep(timeStep)
|
||||
|
||||
Reference in New Issue
Block a user