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

Implement btMultiBodySphericalJointMotor, able to track a quaternion position target.

Browse Source 
Expose this btMultiBodySphericalJointMotor through PyBullet.setJointMotorControlMultiDof
Expose PyBullet.getQuaternionSlerp
Improve PyBullet.setJointMotorControlMultiDof
Improve humanoidMotionCapture.py with slerp and using setJointMotorControlMultiDof
Expose  btMultiBody::spatialTransform
Fix btMultiBody::setupPlanar from DeepMimic codebase
Add support for multidof joints in btMultiBody::compTreeLinkVelocities, thanks to DeepMimic codebase @xbpeng
This commit is contained in:
erwincoumans
2018-11-13 14:32:18 -08:00
parent a06b5de7b6
commit 7dd524075c
13 changed files with 778 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,11 +8,15 @@ p.connect(p.GUI)
import pybullet_data
useMotionCapture=True
useMotionCaptureReset=not useMotionCapture
p.setAdditionalSearchPath(pybullet_data.getDataPath())
path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
p.setPhysicsEngineParameter(numSolverIterations=200)
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
p.loadURDF("plane.urdf")
p.loadURDF("plane.urdf",[0,0,-1])
print("path = ", path)
with open(path, 'r') as f:
motion_dict = json.load(f)
@@ -31,6 +35,11 @@ humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)
for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j)
targetPosition=[0]
if (ji[2] == p.JOINT_SPHERICAL):
targetPosition=[0,0,0,1]
#p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, force=0)
#print(ji)
print("joint[",j,"].type=",jointTypes[ji[2]])
print("joint[",j,"].name=",ji[1])
@@ -51,7 +60,7 @@ for j in range (p.getNumJoints(humanoid)):
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
print("jointName=",jointName, "at ", j)
p.changeVisualShape(humanoid,14,rgbaColor=[1,0,0,1])
p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
chest=1
neck=2
rightShoulder=3
@@ -66,34 +75,115 @@ leftKnee=13
leftAnkle=14
import time
p.setRealTimeSimulation(0)
while (1):
p.getCameraImage(320,200)
frame = int(p.readUserDebugParameter(frameId))
once=True
p.getCameraImage(320,200)
maxForce=1000
while (p.isConnected()):
frameReal = p.readUserDebugParameter(frameId)
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])
basePos1 = [frameData[1],frameData[2],frameData[3]]
baseOrn1 = [frameData[5],frameData[6], frameData[7],frameData[4]]
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
y2zPos=[0,0,0]
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)
chestRot = [frameData[9],frameData[10],frameData[11],frameData[8]]
neckRot = [frameData[13],frameData[14],frameData[15],frameData[12]]
rightHipRot = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightKneeRot = [frameData[20]]
rightAnkleRot = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightShoulderRot = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightElbowRot = [frameData[29]]
leftHipRot = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftKneeRot = [frameData[34]]
leftAnkleRot = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftShoulderRot = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftElbowRot = [frameData[43]]
# once=False
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])]
#print("chestRot=",chestRot)
p.setGravity(0,0,0)
kp=1
if (useMotionCapture):
p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=maxForce)
p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=maxForce)
if (useMotionCaptureReset):
p.resetJointStateMultiDof(humanoid,chest,chestRot)
p.resetJointStateMultiDof(humanoid,neck,neckRot)
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
@@ -106,11 +196,5 @@ while (1):
p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
else:
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,jointIds[i],
p.POSITION_CONTROL,targetPos, force=5*240.)
time.sleep(0.1)
p.stepSimulation()
#time.sleep(1./240.)