diff options
Diffstat (limited to 'examples/pybullet/examples/humanoidMotionCapture.py')
| -rw-r--r-- | examples/pybullet/examples/humanoidMotionCapture.py | 898 |
1 files changed, 510 insertions, 388 deletions
diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index 852fcda4f..fc3fa9a7c 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -1,182 +1,233 @@ -import pybullet as p +import pybullet as p import json import time useGUI = True if useGUI: - p.connect(p.GUI) + p.connect(p.GUI) else: - p.connect(p.DIRECT) + p.connect(p.DIRECT) useZUp = False useYUp = not useZUp -showJointMotorTorques = False +showJointMotorTorques = False if useYUp: - p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) + p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1) -from pdControllerExplicit import PDControllerExplicitMultiDof -from pdControllerStable import PDControllerStableMultiDof +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]) +p.resetDebugVisualizerCamera(cameraDistance=7, + cameraYaw=-94, + cameraPitch=-14, + cameraTargetPosition=[0.31, 0.03, -1.16]) import pybullet_data p.setTimeOut(10000) -useMotionCapture=False -useMotionCaptureReset=False#not useMotionCapture -useExplicitPD = True +useMotionCapture = False +useMotionCaptureReset = False #not useMotionCapture +useExplicitPD = True p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setPhysicsEngineParameter(numSolverIterations=30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #explicit PD control requires small timestep -timeStep = 1./600. +timeStep = 1. / 600. #timeStep = 1./240. 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_walk.txt" - #p.loadURDF("plane.urdf",[0,0,-1.03]) print("path = ", path) -with open(path, 'r') as f: - motion_dict = json.load(f) +with open(path, 'r') as f: + motion_dict = json.load(f) #print("motion_dict = ", motion_dict) print("len motion=", len(motion_dict)) print(motion_dict['Loop']) -numFrames = len(motion_dict['Frames']) -print("#frames = ", numFrames) - - -frameId= p.addUserDebugParameter("frame",0,numFrames-1,0) - - -erpId = p.addUserDebugParameter("erp",0,1,0.2) - -kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2) -forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000) - - - - -jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", - "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] - -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("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("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, -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.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] - -startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952, --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] - -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 -for j in range (p.getNumJoints(humanoid)): - ji = p.getJointInfo(humanoid,j) - targetPosition=[0] - jointType = ji[2] - if (jointType == p.JOINT_SPHERICAL): - targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] - targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] - index0+=4 - 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]] - 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=[] -paramIds=[] -for j in range (p.getNumJoints(humanoid)): - #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) - p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1]) - info = p.getJointInfo(humanoid,j) - #print(info) - if (not useMotionCapture): - jointName = info[1] - jointType = info[2] - if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): - jointIds.append(j) - #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) - #print("jointName=",jointName, "at ", j) - -p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1]) -chest=1 -neck=2 -rightHip=3 -rightKnee=4 -rightAnkle=5 -rightShoulder=6 -rightElbow=7 -leftHip=9 -leftKnee=10 -leftAnkle=11 -leftShoulder=12 -leftElbow=13 +numFrames = len(motion_dict['Frames']) +print("#frames = ", numFrames) + +frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0) + +erpId = p.addUserDebugParameter("erp", 0, 1, 0.2) + +kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2) +forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000) + +jointTypes = [ + "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED" +] + +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("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("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, 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.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188, + 0.105854014, 0.581348 +] + +startVel = [ + 1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0, + 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952, + -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 +] + +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 +for j in range(p.getNumJoints(humanoid)): + ji = p.getJointInfo(humanoid, j) + targetPosition = [0] + jointType = ji[2] + if (jointType == p.JOINT_SPHERICAL): + targetPosition = [ + startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0] + ] + targetVel = [startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2]] + index0 += 4 + 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]] + 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 = [] +paramIds = [] +for j in range(p.getNumJoints(humanoid)): + #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) + p.changeVisualShape(humanoid, j, rgbaColor=[1, 1, 1, 1]) + info = p.getJointInfo(humanoid, j) + #print(info) + if (not useMotionCapture): + jointName = info[1] + jointType = info[2] + if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): + jointIds.append(j) + #paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) + #print("jointName=",jointName, "at ", j) + +p.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1]) +chest = 1 +neck = 2 +rightHip = 3 +rightKnee = 4 +rightAnkle = 5 +rightShoulder = 6 +rightElbow = 7 +leftHip = 9 +leftKnee = 10 +leftAnkle = 11 +leftShoulder = 12 +leftElbow = 13 #rightShoulder=3 #rightElbow=4 @@ -191,249 +242,320 @@ leftElbow=13 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] -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 -p.getCameraImage(320,200) - - - - -while (p.isConnected()): - - if useGUI: - 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 - - frame = int(frameReal) - frameNext = frame+1 - if (frameNext >= numFrames): - 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] - - #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) - - 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) +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 +] + +once = True +p.getCameraImage(320, 200) + +while (p.isConnected()): + + if useGUI: + 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 + + frame = int(frameReal) + frameNext = frame + 1 + if (frameNext >= numFrames): + 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] + + #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) + + 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) |