add yapf style and apply yapf to format all Python files
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
@@ -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)
|
||||
numFrames = len(motion_dict['Frames'])
|
||||
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)
|
||||
|
||||
kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
|
||||
forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000)
|
||||
|
||||
erpId = p.addUserDebugParameter("erp",0,1,0.2)
|
||||
jointTypes = [
|
||||
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
|
||||
]
|
||||
|
||||
kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
|
||||
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
|
||||
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])
|
||||
|
||||
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
|
||||
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
|
||||
|
||||
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
|
||||
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
|
||||
]
|
||||
|
||||
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])
|
||||
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
|
||||
]
|
||||
|
||||
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)
|
||||
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])
|
||||
|
||||
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])
|
||||
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])
|
||||
|
||||
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]
|
||||
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)
|
||||
|
||||
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]
|
||||
#print(ji)
|
||||
print("joint[", j, "].type=", jointTypes[ji[2]])
|
||||
print("joint[", j, "].name=", ji[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])
|
||||
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)
|
||||
|
||||
|
||||
|
||||
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
|
||||
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
|
||||
]
|
||||
|
||||
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)
|
||||
|
||||
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
|
||||
|
||||
while (p.isConnected()):
|
||||
frameFraction = frameReal - frame
|
||||
#print("frameFraction=",frameFraction)
|
||||
#print("frame=",frame)
|
||||
#print("frameNext=", frameNext)
|
||||
|
||||
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
|
||||
#getQuaternionSlerp
|
||||
|
||||
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)
|
||||
frameData = motion_dict['Frames'][frame]
|
||||
frameDataNext = motion_dict['Frames'][frameNext]
|
||||
|
||||
#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]])
|
||||
#print("duration=",frameData[0])
|
||||
#print(pos=[frameData])
|
||||
|
||||
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)
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user