Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

revert to original humanoidMotionCapture.py example. Add a showJointMotorTorques variable (false by default)

Browse Source
This commit is contained in:
erwincoumans
2019-02-27 10:01:54 -08:00
parent 8e1c1448ab
commit 2ae03f50b1
1 changed files with 332 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

616
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -1,86 +1,93 @@
import pybullet as p import pybullet as p
import json import json
import time import time
useGUI = True useGUI = True
if useGUI: if useGUI:
p.connect(p.GUI) p.connect(p.GUI)
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
useZUp = False useZUp = False
useYUp = not useZUp useYUp = not useZUp
showJointMotorTorques = False
if useYUp: if useYUp:
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
from pdControllerStable import PDControllerStableMultiDof
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p) stablePD = PDControllerStableMultiDof(p)
p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
import pybullet_data import pybullet_data
p.setTimeOut(10000) p.setTimeOut(10000)
useMotionCapture=False useMotionCapture=False
useMotionCaptureReset=False#not useMotionCapture useMotionCaptureReset=False#not useMotionCapture
useExplicitPD = True
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30) p.setPhysicsEngineParameter(numSolverIterations=30)
#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
#explicit PD control requires small timestep #explicit PD control requires small timestep
timeStep = 1./600. timeStep = 1./600.
#timeStep = 1./240. #timeStep = 1./240.
p.setPhysicsEngineParameter(fixedTimeStep=timeStep) p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt" path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03]) #p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path) print("path = ", path)
with open(path, 'r') as f: with open(path, 'r') as f:
motion_dict = json.load(f) motion_dict = json.load(f)
#print("motion_dict = ", motion_dict) #print("motion_dict = ", motion_dict)
print("len motion=", len(motion_dict)) print("len motion=", len(motion_dict))
print(motion_dict['Loop']) print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames']) numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames) print("#frames = ", numFrames)
frameId= p.addUserDebugParameter("frame",0,numFrames-1,0) frameId= p.addUserDebugParameter("frame",0,numFrames-1,0)
erpId = p.addUserDebugParameter("erp",0,1,0.2) erpId = p.addUserDebugParameter("erp",0,1,0.2)
kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2) kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000) forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]] startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0])
p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0])
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) 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) 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]) 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, startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153,
0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495,
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
@@ -89,62 +96,74 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-
-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] -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(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 index0=7
for j in range (p.getNumJoints(humanoid)): for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j) ji = p.getJointInfo(humanoid,j)
targetPosition=[0] targetPosition=[0]
jointType = ji[2] jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL): if (jointType == p.JOINT_SPHERICAL):
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]]
index0+=4 index0+=4
print("spherical position: ",targetPosition) print("spherical position: ",targetPosition)
print("spherical velocity: ",targetVel) print("spherical velocity: ",targetVel)
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
targetPosition=[startPose[index0]] if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
targetVel = [startVel[index0]] targetPosition=[startPose[index0]]
index0+=1 targetVel = [startVel[index0]]
print("revolute:", targetPosition) index0+=1
print("revolute velocity:", targetVel) print("revolute:", targetPosition)
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel) 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])
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): for j in range (p.getNumJoints(humanoid)):
p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0) ji = p.getJointInfo(humanoid,j)
targetPosition=[0]
#print(ji) jointType = ji[2]
print("joint[",j,"].type=",jointTypes[ji[2]]) if (jointType == p.JOINT_SPHERICAL):
print("joint[",j,"].name=",ji[1]) 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=[] jointIds=[]
paramIds=[] paramIds=[]
for j in range (p.getNumJoints(humanoid)): for j in range (p.getNumJoints(humanoid)):
#p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1]) p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
info = p.getJointInfo(humanoid,j) info = p.getJointInfo(humanoid,j)
#print(info) #print(info)
if (not useMotionCapture): if (not useMotionCapture):
jointName = info[1] jointName = info[1]
jointType = info[2] jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j) jointIds.append(j)
#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
#print("jointName=",jointName, "at ", j) #print("jointName=",jointName, "at ", j)
p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1]) p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
chest=1 chest=1
neck=2 neck=2
@@ -163,7 +182,7 @@ leftElbow=13
#rightElbow=4 #rightElbow=4
#leftShoulder=6 #leftShoulder=6
#leftElbow = 7 #leftElbow = 7
#rightHip = 9 #rightHip = 9
#rightKnee=10 #rightKnee=10
#rightAnkle=11 #rightAnkle=11
#leftHip = 12 #leftHip = 12
@@ -173,219 +192,248 @@ leftElbow=13
import time import time
kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300] kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30] kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
once=True once=True
p.getCameraImage(320,200) p.getCameraImage(320,200)
while (p.isConnected()):
if useGUI: while (p.isConnected()):
erp = p.readUserDebugParameter(erpId)
kpMotor = p.readUserDebugParameter(kpMotorId)
maxForce=p.readUserDebugParameter(forceMotorId)
frameReal = p.readUserDebugParameter(frameId)
else:
erp = 0.2
kpMotor = 0.2
maxForce=1000
frameReal = 0
kp=kpMotor if useGUI:
erp = p.readUserDebugParameter(erpId)
frame = int(frameReal) kpMotor = p.readUserDebugParameter(kpMotorId)
frameNext = frame+1 maxForce=p.readUserDebugParameter(forceMotorId)
if (frameNext >= numFrames): frameReal = p.readUserDebugParameter(frameId)
frameNext = frame else:
erp = 0.2
frameFraction = frameReal - frame kpMotor = 0.2
maxForce=1000
frameData = motion_dict['Frames'][frame] frameReal = 0
frameDataNext = motion_dict['Frames'][frameNext]
#print("duration=",frameData[0])
#print(pos=[frameData])
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]
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]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
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])]
once=False
kp=kpMotor
basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
pose = [ basePos[0],basePos[1],basePos[2],
baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
chestRot[0],chestRot[1],chestRot[2],chestRot[3],
neckRot[0],neckRot[1],neckRot[2],neckRot[3],
rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
rightKneeRot[0],
rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
rightElbowRot[0],
leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
leftKneeRot[0],
leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
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):
taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices = jointIndicesAll,
desiredPositions = pose,
desiredVelocities = [0]*totalDofs,
kps = kpOrg,
kds = kdOrg,
maxForces = [maxForce]*totalDofs,
timeStep=timeStep)
#taus=[0]*43
for index in range (len(jointIndicesAll)): kp=kpMotor
jointIndex = jointIndicesAll[index]
if jointDofCounts[index]==4: frame = int(frameReal)
p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]]) frameNext = frame+1
if jointDofCounts[index]==1: if (frameNext >= numFrames):
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]]) frameNext = frame
dofIndex+=jointDofCounts[index]
else: frameFraction = frameReal - frame
p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce]) #print("frameFraction=",frameFraction)
p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce]) #print("frame=",frame)
p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce]) #print("frameNext=", frameNext)
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]) #getQuaternionSlerp
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]) frameData = motion_dict['Frames'][frame]
p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce]) frameDataNext = motion_dict['Frames'][frameNext]
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]) #print("duration=",frameData[0])
p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce]) #print(pos=[frameData])
p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
p.stepSimulation() basePos1Start = [frameData[1],frameData[2],frameData[3]]
for j in range(p.getNumJoints(humanoid)): basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
jointState = p.getJointStateMultiDof(humanoid,j) basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
print("jointStateMultiDof[",j,"].pos=",jointState[0]) basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
print("jointStateMultiDof[",j,"].vel=",jointState[1]) basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
print("jointStateMultiDof[",j,"].jointForces=",jointState[3]) baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
#j=4 baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
#jointState = p.getJointStateMultiDof(humanoid,j) #pre-rotate to make z-up
#print("jointStateMultiDof[",j,"].pos=",jointState[0]) if (useZUp):
#print("jointStateMultiDof[",j,"].vel=",jointState[1]) y2zPos=[0,0,0.0]
#print("jointStateMultiDof[",j,"].jointForces=",jointState[3]) y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
time.sleep(timeStep) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zPos=[0,2,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
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])]
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],
chestRot[0],chestRot[1],chestRot[2],chestRot[3],
neckRot[0],neckRot[1],neckRot[2],neckRot[3],
rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
rightKneeRot[0],
rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
rightElbowRot[0],
leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
leftKneeRot[0],
leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
leftElbowRot[0] ]
#print("pose=")
#for po in pose:
# print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices = jointIndicesAll,
desiredPositions = pose,
desiredVelocities = [0]*totalDofs,
kps = kpOrg,
kds = kdOrg,
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]
#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()
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])
time.sleep(timeStep)