mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
add converted humanoid from DeepMimic (https://github.com/xbpeng/DeepMimic,
thanks to Jason Peng), and motion capture playback example. See also https://www.youtube.com/watch?v=vw3EKnKrgqw
This commit is contained in:
erwincoumans
parent
0b2142414d
commit
a06b5de7b6
116
examples/pybullet/examples/humanoidMotionCapture.py
Normal file
116
examples/pybullet/examples/humanoidMotionCapture.py
Normal file
@ -0,0 +1,116 @@
|
||||
import pybullet as p
|
||||
import json
|
||||
|
||||
p.connect(p.GUI)
|
||||
#p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
|
||||
|
||||
|
||||
import pybullet_data
|
||||
|
||||
useMotionCapture=True
|
||||
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
print("path = ", path)
|
||||
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)
|
||||
|
||||
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
|
||||
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
|
||||
|
||||
humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)
|
||||
|
||||
for j in range (p.getNumJoints(humanoid)):
|
||||
ji = p.getJointInfo(humanoid,j)
|
||||
#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,14,rgbaColor=[1,0,0,1])
|
||||
chest=1
|
||||
neck=2
|
||||
rightShoulder=3
|
||||
rightElbow=4
|
||||
leftShoulder=6
|
||||
leftElbow = 7
|
||||
rightHip = 9
|
||||
rightKnee=10
|
||||
rightAnkle=11
|
||||
leftHip = 12
|
||||
leftKnee=13
|
||||
leftAnkle=14
|
||||
|
||||
import time
|
||||
p.setRealTimeSimulation(0)
|
||||
while (1):
|
||||
p.getCameraImage(320,200)
|
||||
frame = int(p.readUserDebugParameter(frameId))
|
||||
frameData = motion_dict['Frames'][frame]
|
||||
#print("duration=",frameData[0])
|
||||
#print(pos=[frameData])
|
||||
basePos1 = [frameData[1],frameData[2],frameData[3]]
|
||||
baseOrn1 = [frameData[5],frameData[6], frameData[7],frameData[4]]
|
||||
#pre-rotate to make z-up
|
||||
y2zPos=[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]]
|
||||
#print("chestRot=",chestRot)
|
||||
if (useMotionCapture):
|
||||
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)
|
||||
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)
|
||||
284
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
Normal file
284
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
Normal file
@ -0,0 +1,284 @@
|
||||
<robot name="dumpUrdf">
|
||||
<link name="base" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0 0 0" />
|
||||
<mass value = "0.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="link0" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.280000 0.000000" />
|
||||
<mass value = "6.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.280000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint0" type="fixed" >
|
||||
<parent link = "base" />
|
||||
<child link="link0" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
</joint>
|
||||
<link name="link1" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.480000 0.000000" />
|
||||
<mass value = "14.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.480000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.440000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint1" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link1" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.944604 0.000000" />
|
||||
</joint>
|
||||
<link name="link2" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.700000 0.000000" />
|
||||
<mass value = "2.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.700000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.410000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint2" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link2" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.895576 0.000000" />
|
||||
</joint>
|
||||
<link name="link3" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<mass value = "4.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.200000" radius="0.220000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint3" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link3" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.339548" />
|
||||
</joint>
|
||||
<link name="link4" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<mass value = "3.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.240000" radius="0.200000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint4" type="continuous" >
|
||||
<parent link="link3" />
|
||||
<child link="link4" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link5" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<geometry>
|
||||
<box size="0.708000 0.220000 0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint5" type="spherical" >
|
||||
<parent link="link4" />
|
||||
<child link="link5" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
|
||||
</joint>
|
||||
<link name="link6" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<mass value = "1.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.720000" radius="0.180000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint6" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link6" />
|
||||
<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 0.732440" />
|
||||
</joint>
|
||||
<link name="link7" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.540000" radius="0.160000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint7" type="continuous" >
|
||||
<parent link="link6" />
|
||||
<child link="link7" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link8" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<mass value = "0.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.160000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint8" type="fixed" >
|
||||
<parent link="link7" />
|
||||
<child link="link8" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
|
||||
</joint>
|
||||
<link name="link9" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<mass value = "4.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.200000" radius="0.220000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint9" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link9" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 -0.339548" />
|
||||
</joint>
|
||||
<link name="link10" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<mass value = "3.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.240000" radius="0.200000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint10" type="continuous" >
|
||||
<parent link="link9" />
|
||||
<child link="link10" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link11" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<geometry>
|
||||
<box size="0.708000 0.220000 0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint11" type="spherical" >
|
||||
<parent link="link10" />
|
||||
<child link="link11" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
|
||||
</joint>
|
||||
<link name="link12" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<mass value = "1.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.720000" radius="0.180000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint12" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link12" />
|
||||
<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 -0.732440" />
|
||||
</joint>
|
||||
<link name="link13" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.540000" radius="0.160000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint13" type="continuous" >
|
||||
<parent link="link12" />
|
||||
<child link="link13" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link14" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<mass value = "0.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.160000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint14" type="fixed" >
|
||||
<parent link="link13" />
|
||||
<child link="link14" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user