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

Merge pull request #2103 from erwincoumans/master

Browse Source 
PyBullet deep_mimic backflip re-using original DeepMimic policy from Jason Peng
This commit is contained in:
erwincoumans
2019-02-11 05:45:21 -08:00
committed by GitHub
parent b574a360f5 d4292fdac3
commit 12e6478689
30 changed files with 829 additions and 1290 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.data-00000-of-00001 Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.index Normal file
View File

Binary file not shown.

BIN
examples/pybullet/gym/pybullet_data/domino/domino.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 292 KiB

11
examples/pybullet/gym/pybullet_data/domino/domino.mtl Normal file
View File

@@ -0,0 +1,11 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
map_Kd domino.jpg

152
examples/pybullet/gym/pybullet_data/domino/domino.obj Normal file
View File

@@ -0,0 +1,152 @@
# Blender v2.79 (sub 0) OBJ File: ''
# www.blender.org
mtllib domino.mtl
o Cube_Cube.001
v -0.012700 0.003175 -0.025400
v -0.012700 0.003175 0.025400
v 0.012700 0.003175 -0.025400
v 0.012700 0.003175 0.025400
v -0.012700 -0.003175 -0.025400
v -0.012700 -0.003175 0.025400
v 0.012700 -0.003175 -0.025400
v 0.012700 -0.003175 0.025400
v 0.000000 0.003175 -0.025400
v -0.012700 0.003175 0.000000
v 0.000000 0.003175 0.025400
v 0.012700 0.003175 0.000000
v 0.012700 0.000000 -0.025400
v 0.012700 0.000000 0.025400
v 0.012700 -0.003175 0.000000
v 0.000000 -0.003175 -0.025400
v 0.000000 -0.003175 0.025400
v -0.012700 -0.003175 0.000000
v -0.012700 0.000000 -0.025400
v -0.012700 0.000000 0.025400
v 0.000000 0.000000 0.025400
v 0.000000 0.000000 -0.025400
v -0.012700 0.000000 0.000000
v 0.000000 -0.003175 0.000000
v 0.012700 0.000000 0.000000
v 0.000000 0.003175 0.000000
vt 0.126001 0.870497
vt 0.124465 0.872033
vt 0.124465 0.870497
vt 0.237060 0.851119
vt 0.218639 0.869540
vt 0.218639 0.851119
vt 0.245670 0.816240
vt 0.187635 0.874274
vt 0.187635 0.816240
vt 0.128048 0.875036
vt 0.125628 0.877456
vt 0.125628 0.875036
vt 0.234999 0.850344
vt 0.218252 0.867091
vt 0.218252 0.850344
vt 0.126159 0.872047
vt 0.122877 0.875330
vt 0.122877 0.872047
vt 0.218639 0.852670
vt 0.203807 0.867502
vt 0.203807 0.852670
vt 0.218639 0.837837
vt 0.203807 0.837837
vt 0.126159 0.868764
vt 0.122877 0.868764
vt 0.126351 0.873597
vt 0.123659 0.876290
vt 0.123659 0.873597
vt 0.126351 0.870905
vt 0.123659 0.870905
vt 0.234999 0.833597
vt 0.218252 0.833597
vt 0.123208 0.877456
vt 0.123208 0.875036
vt 0.215927 0.837296
vt 0.204429 0.848794
vt 0.204429 0.837296
vt 0.227424 0.837296
vt 0.215927 0.848794
vt 0.129601 0.874274
vt 0.129601 0.816240
vt 0.187635 0.758206
vt 0.129601 0.758206
vt 0.245670 0.758206
vt 0.200219 0.869540
vt 0.200219 0.851119
vt 0.125240 0.873601
vt 0.123516 0.875326
vt 0.123516 0.873601
vt 0.126964 0.873601
vt 0.125240 0.875326
vt 0.122929 0.872033
vt 0.122929 0.870497
vt 0.124465 0.868961
vt 0.122929 0.868961
vt 0.126001 0.868961
vt 0.126001 0.872033
vt 0.237060 0.869540
vt 0.245670 0.874274
vt 0.128048 0.877456
vt 0.234999 0.867091
vt 0.126159 0.875330
vt 0.218639 0.867502
vt 0.126351 0.876290
vt 0.227424 0.848794
vt 0.126964 0.875326
vn 0.0000 1.0000 0.0000
vn 1.0000 0.0000 0.0000
vn 0.0000 -1.0000 0.0000
vn -1.0000 0.0000 0.0000
vn 0.0000 0.0000 -1.0000
vn 0.0000 0.0000 1.0000
usemtl None
s off
f 11/1/1 12/2/1 26/3/1
f 14/4/2 15/5/2 25/6/2
f 17/7/3 18/8/3 24/9/3
f 20/10/4 10/11/4 23/12/4
f 16/13/5 19/14/5 22/15/5
f 11/16/6 20/17/6 21/18/6
f 21/19/6 6/20/6 17/21/6
f 14/22/6 17/21/6 8/23/6
f 4/24/6 21/18/6 14/25/6
f 22/26/5 1/27/5 9/28/5
f 13/29/5 9/28/5 3/30/5
f 7/31/5 22/15/5 13/32/5
f 23/12/4 1/33/4 19/34/4
f 18/35/4 19/36/4 5/37/4
f 6/38/4 23/39/4 18/35/4
f 24/9/3 5/40/3 16/41/3
f 15/42/3 16/41/3 7/43/3
f 8/44/3 24/9/3 15/42/3
f 25/6/2 7/45/2 13/46/2
f 12/47/2 13/48/2 3/49/2
f 4/50/2 25/51/2 12/47/2
f 26/3/1 3/52/1 9/53/1
f 10/54/1 9/53/1 1/55/1
f 2/56/1 26/3/1 10/54/1
f 11/1/1 4/57/1 12/2/1
f 14/4/2 8/58/2 15/5/2
f 17/7/3 6/59/3 18/8/3
f 20/10/4 2/60/4 10/11/4
f 16/13/5 5/61/5 19/14/5
f 11/16/6 2/62/6 20/17/6
f 21/19/6 20/63/6 6/20/6
f 14/22/6 21/19/6 17/21/6
f 4/24/6 11/16/6 21/18/6
f 22/26/5 19/64/5 1/27/5
f 13/29/5 22/26/5 9/28/5
f 7/31/5 16/13/5 22/15/5
f 23/12/4 10/11/4 1/33/4
f 18/35/4 23/39/4 19/36/4
f 6/38/4 20/65/4 23/39/4
f 24/9/3 18/8/3 5/40/3
f 15/42/3 24/9/3 16/41/3
f 8/44/3 17/7/3 24/9/3
f 25/6/2 15/5/2 7/45/2
f 12/47/2 25/51/2 13/48/2
f 4/50/2 14/66/2 25/51/2
f 26/3/1 12/2/1 3/52/1
f 10/54/1 26/3/1 9/53/1
f 2/56/1 11/1/1 26/3/1

29
examples/pybullet/gym/pybullet_data/domino/domino.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="0.5"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.025"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 1.57" xyz="0 0 0"/>
<geometry>
<mesh filename="domino.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.00635 0.0254 0.0508"/>
</geometry>
</collision>
</link>
</robot>

10
examples/pybullet/gym/pybullet_data/domino/license.txt Normal file
View File

@@ -0,0 +1,10 @@
URDF model created by Erwin Coumans. Mesh / texture created in Blender.
If you use the model, add a citation to PyBullet:
@MISC{coumans2018,
author = {Erwin Coumans and Yunfei Bai},
title = {PyBullet, a Python module for physics simulation for games, robotics and machine learning},
howpublished = {\url{http://pybullet.org}},
year = {2016--2019}
}

4
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
View File

@@ -2,8 +2,8 @@
<link name="base" >
<inertial>
<origin rpy = "0 0 0" xyz = "0 0 0" />
<mass value = "0.00100" />
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
<mass value = "0.0001" />
<inertia ixx = "0.0001" ixy = "0" ixz = "0" iyy = "0.0001" iyz = "0" izz = "0.0001" />
</inertial>
</link>
<link name="root" >

31
examples/pybullet/gym/pybullet_data/plane_implicit.urdf Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0" ?>
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="0.9"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane100.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<!--<origin rpy="0 0 0" xyz="0 0 -5"/>-->
<geometry>
<plane normal="0 0 1"/>
<!--<box size="100 100 10"/>-->
</geometry>
</collision>
</link>
</robot>

14
examples/pybullet/gym/pybullet_data/plane_transparent.mtl Normal file
View File

@@ -0,0 +1,14 @@
newmtl Material
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Kd checker_blue.png

18
examples/pybullet/gym/pybullet_data/plane_transparent.obj Normal file
View File

@@ -0,0 +1,18 @@
# Blender v2.66 (sub 1) OBJ File: ''
# www.blender.org
mtllib plane_transparent.mtl
o Plane
v 15.000000 -15.000000 0.000000
v 15.000000 15.000000 0.000000
v -15.000000 15.000000 0.000000
v -15.000000 -15.000000 0.000000
vt 15.000000 0.000000
vt 15.000000 15.000000
vt 0.000000 15.000000
vt 0.000000 0.000000
usemtl Material
s off
f 1/1 2/2 3/3
f 1/1 3/3 4/4

29
examples/pybullet/gym/pybullet_data/plane_transparent.urdf Normal file
View File

@@ -0,0 +1,29 @@
<?xml version="1.0" ?>
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="1"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane_transparent.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 .7"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -5"/>
<geometry>
<box size="30 30 10"/>
</geometry>
</collision>
</link>
</robot>

2
examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py
View File

@@ -1 +1 @@
from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv

90
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py vendored
View File

@@ -1,45 +1,51 @@
from pybullet_envs.minitaur.envs import bullet_client
from pybullet_utils import bullet_client
import math
class HumanoidPoseInterpolator(object):
def __init__(self):
pass
def Reset(self):
def Reset(self,basePos=[0,0,0], baseOrn=[0,0,0,1],chestRot=[0,0,0,1], neckRot=[0,0,0,1],rightHipRot= [0,0,0,1], rightKneeRot=[0],rightAnkleRot = [0,0,0,1],
rightShoulderRot = [0,0,0,1],rightElbowRot = [0], leftHipRot = [0,0,0,1], leftKneeRot = [0],leftAnkleRot = [0,0,0,1],
leftShoulderRot = [0,0,0,1] ,leftElbowRot = [0],
baseLinVel = [0,0,0],baseAngVel = [0,0,0], chestVel = [0,0,0],neckVel = [0,0,0],rightHipVel = [0,0,0],rightKneeVel = [0],
rightAnkleVel = [0,0,0],rightShoulderVel = [0,0,0],rightElbowVel = [0],leftHipVel = [0,0,0],leftKneeVel = [0],leftAnkleVel = [0,0,0],leftShoulderVel = [0,0,0],leftElbowVel = [0]
):
self._basePos = [0,0,0]
self._baseLinVel = [0,0,0]
self._baseOrn = [0,0,0,1]
self._baseAngVel = [0,0,0]
self._basePos = basePos
self._baseLinVel = baseLinVel
#print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
self._baseOrn =baseOrn
self._baseAngVel = baseAngVel
self._chestRot = [0,0,0,1]
self._chestVel = [0,0,0]
self._neckRot = [0,0,0,1]
self._neckVel = [0,0,0]
self._chestRot = chestRot
self._chestVel =chestVel
self._neckRot = neckRot
self._neckVel = neckVel
self._rightHipRot = [0,0,0,1]
self._rightHipVel = [0,0,0]
self._rightKneeRot = [0]
self._rightKneeVel = [0]
self._rightAnkleRot = [0,0,0,1]
self._rightAnkleVel = [0,0,0]
self._rightHipRot = rightHipRot
self._rightHipVel = rightHipVel
self._rightKneeRot =rightKneeRot
self._rightKneeVel = rightKneeVel
self._rightAnkleRot = rightAnkleRot
self._rightAnkleVel = rightAnkleVel
self._rightShoulderRot = [0,0,0,1]
self._rightShoulderVel = [0,0,0]
self._rightElbowRot = [0]
self._rightElbowVel = [0]
self._rightShoulderRot =rightShoulderRot
self._rightShoulderVel = rightShoulderVel
self._rightElbowRot = rightElbowRot
self._rightElbowVel = rightElbowVel
self._leftHipRot = [0,0,0,1]
self._leftHipVel = [0,0,0]
self._leftKneeRot = [0]
self._leftKneeVel = [0]
self._leftAnkleRot = [0,0,0,1]
self._leftAnkleVel = [0,0,0]
self._leftHipRot = leftHipRot
self._leftHipVel = leftHipVel
self._leftKneeRot = leftKneeRot
self._leftKneeVel = leftKneeVel
self._leftAnkleRot =leftAnkleRot
self._leftAnkleVel = leftAnkleVel
self._leftShoulderRot = [0,0,0,1]
self._leftShoulderVel = [0,0,0]
self._leftElbowRot = [0]
self._leftElbowVel = [0]
self._leftShoulderRot = leftShoulderRot
self._leftShoulderVel = leftShoulderVel
self._leftElbowRot =leftElbowRot
self._leftElbowVel = leftElbowVel
def ComputeLinVel(self,posStart, posEnd, deltaTime):
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
@@ -50,6 +56,14 @@ class HumanoidPoseInterpolator(object):
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def ComputeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def NormalizeVector(self, vec):
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]
@@ -121,17 +135,17 @@ class HumanoidPoseInterpolator(object):
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client)
self._chestVel = self.ComputeAngVelRel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client)
self._neckVel = self.ComputeAngVelRel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
self._rightHipVel = self.ComputeAngVelRel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
@@ -141,12 +155,12 @@ class HumanoidPoseInterpolator(object):
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client)
self._rightAnkleVel = self.ComputeAngVelRel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
self._rightShoulderVel = self.ComputeAngVelRel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
@@ -156,7 +170,7 @@ class HumanoidPoseInterpolator(object):
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
self._leftHipVel = self.ComputeAngVelRel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
@@ -166,12 +180,12 @@ class HumanoidPoseInterpolator(object):
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
self._leftAnkleVel = self.ComputeAngVelRel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
self._leftShoulderVel = self.ComputeAngVelRel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]

43
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py vendored
View File

@@ -1,5 +1,5 @@
from pybullet_envs.deep_mimic.env import pd_controller_stable
from pybullet_utils import pd_controller_stable
from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator
import math
@@ -24,10 +24,10 @@ class HumanoidStablePD(object):
print("LOADING humanoid!")
self._sim_model = self._pybullet_client.loadURDF(
"humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
"humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
self._kin_model = self._pybullet_client.loadURDF(
"humanoid/humanoid.urdf", [0,2.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
"humanoid/humanoid.urdf", [0,12.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9)
for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
@@ -64,7 +64,7 @@ class HumanoidStablePD(object):
for dof in self._jointDofCounts:
self._totalDofs += dof
self.setSimTime(0)
self._maxForce = 6000
self.resetPose()
def resetPose(self):
@@ -146,7 +146,9 @@ class HumanoidStablePD(object):
def computePose(self, frameFraction):
frameData = self._mocap_data._motion_data['Frames'][self._frame]
frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext]
pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
#print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
return pose
@@ -154,27 +156,34 @@ class HumanoidStablePD(object):
pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
return pose
def computePDForces(self, desiredPositions, desiredVelocities = None):
def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
if desiredVelocities==None:
desiredVelocities = [0]*self._totalDofs
taus = self._stablePD.computePD(bodyUniqueId=self._sim_model,
jointIndices = self._jointIndicesAll,
desiredPositions = desiredPositions,
desiredVelocities = desiredVelocities,
kps = self._kpOrg,
kds = self._kdOrg,
maxForces = [self._maxForce]*self._totalDofs,
maxForces = maxForces,
timeStep=self._timeStep)
return taus
def applyPDForces(self, taus):
dofIndex=7
scaling = 1
for index in range (len(self._jointIndicesAll)):
jointIndex = self._jointIndicesAll[index]
if self._jointDofCounts[index]==4:
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
force=[scaling*taus[dofIndex+0],scaling*taus[dofIndex+1],scaling*taus[dofIndex+2]]
#print("force[", jointIndex,"]=",force)
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=force)
if self._jointDofCounts[index]==1:
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=[taus[dofIndex]])
force=[scaling*taus[dofIndex]]
#print("force[", jointIndex,"]=",force)
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=force)
dofIndex+=self._jointDofCounts[index]
@@ -257,6 +266,13 @@ class HumanoidStablePD(object):
for l in linkPosLocal:
stateVector.append(l)
#re-order the quaternion, DeepMimic uses w,x,y,z
if (linkOrnLocal[3]<0):
linkOrnLocal[0]*=-1
linkOrnLocal[1]*=-1
linkOrnLocal[2]*=-1
linkOrnLocal[3]*=-1
stateVector.append(linkOrnLocal[3])
stateVector.append(linkOrnLocal[0])
stateVector.append(linkOrnLocal[1])
@@ -268,9 +284,13 @@ class HumanoidStablePD(object):
ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
linkLinVel = ls[6]
linkAngVel = ls[7]
for l in linkLinVel:
linkLinVelLocal , unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkLinVel,[0,0,0,1])
#linkLinVelLocal=[linkLinVelLocal[0]-rootPosRel[0],linkLinVelLocal[1]-rootPosRel[1],linkLinVelLocal[2]-rootPosRel[2]]
linkAngVelLocal ,unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkAngVel,[0,0,0,1])
for l in linkLinVelLocal:
stateVector.append(l)
for l in linkAngVel:
for l in linkAngVelLocal:
stateVector.append(l)
#print("stateVector len=",len(stateVector))
@@ -284,6 +304,9 @@ class HumanoidStablePD(object):
pts = self._pybullet_client.getContactPoints()
for p in pts:
part = -1
#ignore self-collision
if (p[1]==p[2]):
continue
if (p[1]==self._sim_model):
part=p[3]
if (p[2]==self._sim_model):

144
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py vendored
View File

@@ -1,144 +0,0 @@
import numpy as np
class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
#q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
#qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
qError = [0,0,0, 0,0,0,0]
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
q = np.array(q1)
qdot=np.array(qdot1)
qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired-qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
#np.savetxt("pb_d.csv", d, delimiter=",")
#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
M2 = np.array(M1)
#np.savetxt("M2.csv", M2, delimiter=",")
M = (M2 + Kd * timeStep)
#np.savetxt("pbM_tKd.csv",M, delimiter=",")
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
c = np.array(c1)
#np.savetxt("pbC.csv",c, delimiter=",")
A = M
#p = [0]*43
b = p + d -c
#np.savetxt("pb_acc.csv",b, delimiter=",")
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
return forces
class PDControllerStable(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
#print("c=",c)
return tau

77
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py vendored
View File

@@ -2,9 +2,9 @@ import numpy as np
import math
from pybullet_envs.deep_mimic.env.env import Env
from pybullet_envs.deep_mimic.env.action_space import ActionSpace
from pybullet_envs.minitaur.envs import bullet_client
from pybullet_utils import bullet_client
import time
import motion_capture_data
from pybullet_envs.deep_mimic.env import motion_capture_data
from pybullet_envs.deep_mimic.env import humanoid_stable_pd
import pybullet_data
import pybullet as p1
@@ -20,7 +20,10 @@ class PyBulletDeepMimicEnv(Env):
self.reset()
def reset(self):
self.t = 0
startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime()
self.t = startTime
if not self._isInitialized:
if self.enable_draw:
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
@@ -29,7 +32,7 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
self._planeId = self._pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
#print("planeId=",self._planeId)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
self._pybullet_client.setGravity(0,-9.8,0)
@@ -38,8 +41,8 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
self._mocapData = motion_capture_data.MotionCaptureData()
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1./600
useFixedBase=False
@@ -48,7 +51,7 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
selfCheck = False
if (selfCheck):
@@ -65,16 +68,17 @@ class PyBulletDeepMimicEnv(Env):
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
rnrange = 1000
rn = random.randint(0,rnrange)
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
self._humanoid.setSimTime(startTime)
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
self._pybullet_client.stepSimulation()
#self._pybullet_client.stepSimulation()
self._humanoid.resetPose()
self.needs_update_time = self.t-1#force update
def get_num_agents(self):
return self._num_agents
@@ -198,12 +202,14 @@ class PyBulletDeepMimicEnv(Env):
self._mode = mode
def need_new_action(self, agent_id):
return True
if self.t>=self.needs_update_time:
self.needs_update_time = self.t + 1./30.
return True
return False
def record_state(self, agent_id):
state = self._humanoid.getState()
state[1]=state[1]+0.008
#print("record_state=",state)
return np.array(state)
@@ -216,24 +222,43 @@ class PyBulletDeepMimicEnv(Env):
return reward
def set_action(self, agent_id, action):
#print("action=",)
#for a in action:
# print(a)
np.savetxt("pb_action.csv", action, delimiter=",")
self.desiredPose = self._humanoid.convertActionToPose(action)
#we need the target root positon and orientation to be zero, to be compatible with deep mimic
self.desiredPose[0] = 0
self.desiredPose[1] = 0
self.desiredPose[2] = 0
self.desiredPose[3] = 0
self.desiredPose[4] = 0
self.desiredPose[5] = 0
self.desiredPose[6] = 0
target_pose = np.array(self.desiredPose)
np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
#print("set_action: desiredPose=", self.desiredPose)
def log_val(self, agent_id, val):
pass
def update(self, timeStep):
for i in range(10):
#print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
for i in range(1):
self.t += timeStep
self._humanoid.setSimTime(self.t)
if self.desiredPose:
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0],pos[1]+2,pos[2]],orn)
#kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
#self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
#pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
#self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
#print("desiredPositions=",self.desiredPose)
taus = self._humanoid.computePDForces(self.desiredPose)
maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200, 150, 90, 90, 90, 90, 100,100,100,100, 60]
taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
self._humanoid.applyPDForces(taus)
self._pybullet_client.stepSimulation()
@@ -255,3 +280,13 @@ class PyBulletDeepMimicEnv(Env):
def check_valid_episode(self):
#could check if limbs exceed velocity threshold
return true
def getKeyboardEvents(self):
return self._pybullet_client.getKeyboardEvents()
def isKeyTriggered(self, keys, key):
o = ord(key)
#print("ord=",o)
if o in keys:
return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
return False

122
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py vendored Normal file
View File

@@ -0,0 +1,122 @@
from pybullet_utils import bullet_client
import time
import math
import motion_capture_data
from pybullet_envs.deep_mimic.env import humanoid_stable_pd
import pybullet_data
import pybullet as p1
import humanoid_pose_interpolator
import numpy as np
pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
z2y = pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
#planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
planeId= pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
#print("planeId=",planeId)
pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
pybullet_client.setGravity(0,-9.8,0)
pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
mocapData = motion_capture_data.MotionCaptureData()
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
mocapData.Load(motionPath)
timeStep = 1./600
useFixedBase=False
humanoid = humanoid_stable_pd.HumanoidStablePD(pybullet_client, mocapData, timeStep, useFixedBase)
isInitialized = True
pybullet_client.setTimeStep(timeStep)
pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
#startPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
#startPose.Reset(basePos=[-0.000000,0.889540,0.000000],baseOrn=[0.029215,-0.000525,-0.017963,0.999412],chestRot=[0.000432,0.000572,0.005500,0.999985],
# neckRot=[0.001660,-0.011168,-0.140597,0.990003],rightHipRot=[-0.024450,-0.000839,-0.038869,0.998945],rightKneeRot=[-0.014186],rightAnkleRot=[0.010757,-0.105223,0.035405,0.993760],
# rightShoulderRot=[-0.003003,-0.124234,0.073280,0.989539],rightElbowRot=[0.240463],leftHipRot=[-0.020920,-0.012925,-0.006300,0.999678],leftKneeRot=[-0.027859],
# leftAnkleRot=[-0.010764,0.105284,-0.009276,0.994341],leftShoulderRot=[0.055661,-0.019608,0.098917,0.993344],leftElbowRot=[0.148934],
# baseLinVel=[-0.340837,0.377742,0.009662],baseAngVel=[0.047057,0.285253,-0.248554],chestVel=[-0.016455,-0.070035,-0.231662],neckVel=[0.072168,0.097898,-0.059063],
# rightHipVel=[-0.315908,-0.131685,1.114815],rightKneeVel=[0.103419],rightAnkleVel=[-0.409780,-0.099954,-4.241572],rightShoulderVel=[-3.324227,-2.510209,1.834637],
# rightElbowVel=[-0.212299],leftHipVel=[0.173056,-0.191063,1.226781,0.000000],leftKneeVel=[-0.665322],leftAnkleVel=[0.282716,0.086217,-3.007098,0.000000],
# leftShoulderVel=[4.253144,2.038637,1.170750],leftElbowVel=[0.387993])
#
#targetPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
#targetPose.Reset(basePos=[0.000000,0.000000,0.000000],baseOrn=[0.000000,0.000000,0.000000,1.000000],chestRot=[-0.006711,0.007196,-0.027119,0.999584],neckRot=[-0.017613,-0.033879,-0.209250,0.977116],
# rightHipRot=[-0.001697,-0.006510,0.046117,0.998913],rightKneeRot=[0.366954],rightAnkleRot=[0.012605,0.001208,-0.187007,0.982277],rightShoulderRot=[-0.468057,-0.044589,0.161134,0.867739],
# rightElbowRot=[-0.593650],leftHipRot=[0.006993,0.017242,0.049703,0.998591],leftKneeRot=[0.395147],leftAnkleRot=[-0.008922,0.026517,-0.217852,0.975581],
# leftShoulderRot=[0.426160,-0.266177,0.044672,0.863447],leftElbowRot=[-0.726281])
#out_tau= [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-33.338211,-1.381748,-118.708975,0.000000,-2.813919,-2.773850,-0.772481,0.000000,31.885372,2.658243,64.988216,0.000000,94.773133,1.784944,6.240010,5.407563,0.000000,-180.441290,-6.821173,-19.502417,0.000000,-44.518261,9.992627,-2.380950,53.057697,0.000000,111.728594,-1.218496,-4.630812,4.268995,0.000000,89.741829,-8.460265,-117.727884,0.000000,-79.481906]
#,mSimWorld->stepSimulation(timestep:0.001667, mParams.mNumSubsteps:2, subtimestep:0.000833)
#cImpPDController::CalcControlForces timestep=0.001667
def isKeyTriggered(keys, key):
o = ord(key)
if o in keys:
return keys[ord(key)] & pybullet_client.KEY_WAS_TRIGGERED
return False
animating = False
singleStep = False
#humanoid.initializePose(pose=startPose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
t=0
while (1):
keys = pybullet_client.getKeyboardEvents()
#print(keys)
if isKeyTriggered(keys, ' '):
animating = not animating
if isKeyTriggered(keys, 'b'):
singleStep = True
if animating or singleStep:
singleStep = False
#t = pybullet_client.readUserDebugParameter(timeId)
#print("t=",t)
for i in range (1):
print("t=",t)
humanoid.setSimTime(t)
humanoid.computePose(humanoid._frameFraction)
pose = humanoid._poseInterpolator
#humanoid.initializePose(pose=pose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
#humanoid.resetPose()
action = [-6.541649997234344482e-02,1.138873845338821411e-01,-1.215099543333053589e-01,4.610761404037475586e-01,-4.278013408184051514e-01,
4.064738750457763672e-02,7.801693677902221680e-02,4.934634566307067871e-01,1.321935355663299561e-01,-1.393979601562023163e-02,-6.699572503566741943e-02,
4.778462052345275879e-01,3.740053176879882812e-01,-3.230125308036804199e-01,-3.549785539507865906e-02,-3.283375874161720276e-03,5.070925354957580566e-01,
1.033667206764221191e+00,-3.644275963306427002e-01,-3.374500572681427002e-02,1.294951438903808594e-01,-5.880850553512573242e-01,
1.185980588197708130e-01,6.445263326168060303e-02,1.625719368457794189e-01,4.615224599838256836e-01,3.817881345748901367e-01,-4.382217228412628174e-01,
1.626710966229438782e-02,-4.743926972150802612e-02,3.833046853542327881e-01,1.067031383514404297e+00,3.039606213569641113e-01,
-1.891726106405258179e-01,3.595829010009765625e-02,-7.283059358596801758e-01]
pos_tar2 = humanoid.convertActionToPose(action)
desiredPose = np.array(pos_tar2)
#desiredPose = humanoid.computePose(humanoid._frameFraction)
#desiredPose = targetPose.GetPose()
#curPose = HumanoidPoseInterpolator()
#curPose.reset()
s = humanoid.getState()
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
taus = humanoid.computePDForces(desiredPose)
#print("taus=",taus)
humanoid.applyPDForces(taus)
pybullet_client.stepSimulation()
t+=1./600.
time.sleep(1./600.)

683
examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py
View File

@@ -1,683 +0,0 @@
import os, inspect
import math
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
from pybullet_utils.bullet_client import BulletClient
import pybullet_data
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
class HumanoidPose(object):
def __init__(self):
pass
def Reset(self):
self._basePos = [0,0,0]
self._baseLinVel = [0,0,0]
self._baseOrn = [0,0,0,1]
self._baseAngVel = [0,0,0]
self._chestRot = [0,0,0,1]
self._chestVel = [0,0,0]
self._neckRot = [0,0,0,1]
self._neckVel = [0,0,0]
self._rightHipRot = [0,0,0,1]
self._rightHipVel = [0,0,0]
self._rightKneeRot = [0]
self._rightKneeVel = [0]
self._rightAnkleRot = [0,0,0,1]
self._rightAnkleVel = [0,0,0]
self._rightShoulderRot = [0,0,0,1]
self._rightShoulderVel = [0,0,0]
self._rightElbowRot = [0]
self._rightElbowVel = [0]
self._leftHipRot = [0,0,0,1]
self._leftHipVel = [0,0,0]
self._leftKneeRot = [0]
self._leftKneeVel = [0]
self._leftAnkleRot = [0,0,0,1]
self._leftAnkleVel = [0,0,0]
self._leftShoulderRot = [0,0,0,1]
self._leftShoulderVel = [0,0,0]
self._leftElbowRot = [0]
self._leftElbowVel = [0]
def ComputeLinVel(self,posStart, posEnd, deltaTime):
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
return vel
def ComputeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def NormalizeQuaternion(self, orn):
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3]
if (length2>0):
length = math.sqrt(length2)
#print("Normalize? length=",length)
def PostProcessMotionData(self, frameData):
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
self.NormalizeQuaternion(baseOrn1Start)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ):
keyFrameDuration = frameData[0]
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
self._basePos = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
self._baseLinVel = self.ComputeLinVel(basePos1Start,basePos1End, keyFrameDuration)
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client)
##pre-rotate to make z-up
#y2zPos=[0,0,0.0]
#y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
#basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
self._rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
self._rightKneeVel = [(rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration]
rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client)
rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
self._rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
self._rightElbowVel = [(rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration]
leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
self._leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
self._leftKneeVel = [(leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
self._leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
self._leftElbowVel = [(leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration]
class Humanoid(object):
def __init__(self, pybullet_client, motion_data, baseShift):
"""Constructs a humanoid and reset it to the initial states.
Args:
pybullet_client: The instance of BulletClient to manage different
simulations.
"""
self._baseShift = baseShift
self._pybullet_client = pybullet_client
self.kin_client = BulletClient(pybullet_client.DIRECT)# use SHARED_MEMORY for visual debugging, start a GUI physics server first
self.kin_client.resetSimulation()
self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP,1)
self.kin_client.setGravity(0,-9.8,0)
self._motion_data = motion_data
print("LOADING humanoid!")
self._humanoid = self._pybullet_client.loadURDF(
"humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False)
self._kinematicHumanoid = self.kin_client.loadURDF(
"humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25, useFixedBase=False)
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
pose = HumanoidPose()
for i in range (self._motion_data.NumFrames()-1):
frameData = self._motion_data._motion_data['Frames'][i]
pose.PostProcessMotionData(frameData)
self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,self._baseShift,[0,0,0,1])
self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0)
for j in range (self._pybullet_client.getNumJoints(self._humanoid)):
ji = self._pybullet_client.getJointInfo(self._humanoid,j)
self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1])
#print("joint[",j,"].type=",jointTypes[ji[2]])
#print("joint[",j,"].name=",ji[1])
self._initial_state = self._pybullet_client.saveState()
self._allowed_body_parts=[11,14]
self.Reset()
def Reset(self):
self._pybullet_client.restoreState(self._initial_state)
self.SetSimTime(0)
pose = self.InitializePoseFromMotionData()
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
def CalcCycleCount(self, simTime, cycleTime):
phases = simTime / cycleTime;
count = math.floor(phases)
loop = True
#count = (loop) ? count : cMathUtil::Clamp(count, 0, 1);
return count
def SetSimTime(self, t):
self._simTime = t
#print("SetTimeTime time =",t)
keyFrameDuration = self._motion_data.KeyFrameDuraction()
cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1)
#print("self._motion_data.NumFrames()=",self._motion_data.NumFrames())
#print("cycleTime=",cycleTime)
cycles = self.CalcCycleCount(t, cycleTime)
#print("cycles=",cycles)
frameTime = t - cycles*cycleTime
if (frameTime<0):
frameTime += cycleTime
#print("keyFrameDuration=",keyFrameDuration)
#print("frameTime=",frameTime)
self._frame = int(frameTime/keyFrameDuration)
#print("self._frame=",self._frame)
self._frameNext = self._frame+1
if (self._frameNext >= self._motion_data.NumFrames()):
self._frameNext = self._frame
self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration)
#print("self._frameFraction=",self._frameFraction)
def Terminates(self):
#check if any non-allowed body part hits the ground
terminates=False
pts = self._pybullet_client.getContactPoints()
for p in pts:
part = -1
if (p[1]==self._humanoid):
part=p[3]
if (p[2]==self._humanoid):
part=p[4]
if (part >=0 and part not in self._allowed_body_parts):
terminates=True
return terminates
def BuildHeadingTrans(self, rootOrn):
#align root transform 'forward' with world-space x axis
eul = self._pybullet_client.getEulerFromQuaternion(rootOrn)
refDir = [1,0,0]
rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
heading = math.atan2(-rotVec[2], rotVec[0])
heading2=eul[1]
#print("heading=",heading)
headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading)
return headingOrn
def GetPhase(self):
keyFrameDuration = self._motion_data.KeyFrameDuraction()
cycleTime = keyFrameDuration*(self._motion_data.NumFrames()-1)
phase = self._simTime / cycleTime
phase = math.fmod(phase,1.0)
if (phase<0):
phase += 1
return phase
def BuildOriginTrans(self):
rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
#print("rootPos=",rootPos, " rootOrn=",rootOrn)
invRootPos=[-rootPos[0], 0, -rootPos[2]]
#invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn)
headingOrn = self.BuildHeadingTrans(rootOrn)
#print("headingOrn=",headingOrn)
headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
#print("headingMat=",headingMat)
#dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn)
#dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn)
invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1])
#print("invOrigTransPos=",invOrigTransPos)
#print("invOrigTransOrn=",invOrigTransOrn)
invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
#print("invOrigTransMat =",invOrigTransMat )
return invOrigTransPos, invOrigTransOrn
def InitializePoseFromMotionData(self):
frameData = self._motion_data._motion_data['Frames'][self._frame]
frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext]
pose = HumanoidPose()
pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client)
return pose
def ApplyAction(self, action):
#turn action into pose
pose = HumanoidPose()
pose.Reset()
index=0
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
#print("pose._chestRot=",pose._chestRot)
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
index+=1
pose._rightKneeRot = [angle]
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
index+=1
pose._rightElbowRot = [angle]
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
index+=1
pose._leftKneeRot = [angle]
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
axis = [action[index+1],action[index+2],action[index+3]]
index+=4
pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis,angle)
angle = action[index]
index+=1
pose._leftElbowRot = [angle]
#print("index=",index)
initializeBase = False
initializeVelocities = False
self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, self._pybullet_client)
def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid,bc):
#todo: get tunable parametes from a json file or from URDF (kd, maxForce)
if (initializeBase):
bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,0,0,1])
basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]]
bc.resetBasePositionAndOrientation(humanoid,
basePos, pose._baseOrn)
if initializeVelocities:
bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel)
#print("resetBaseVelocity=",pose._baseLinVel)
else:
bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,1,1,1])
kp=0.03
chest=1
neck=2
rightShoulder=3
rightElbow=4
leftShoulder=6
leftElbow = 7
rightHip = 9
rightKnee=10
rightAnkle=11
leftHip = 12
leftKnee=13
leftAnkle=14
controlMode = bc.POSITION_CONTROL
if (initializeBase):
if initializeVelocities:
bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot, pose._chestVel)
bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot, pose._neckVel)
bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot, pose._rightHipVel)
bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel)
bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel)
bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel)
bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel)
bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot, pose._leftHipVel)
bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel)
bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel)
bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel)
else:
bc.resetJointStateMultiDof(humanoid,chest,pose._chestRot)
bc.resetJointStateMultiDof(humanoid,neck,pose._neckRot)
bc.resetJointStateMultiDof(humanoid,rightHip,pose._rightHipRot)
bc.resetJointStateMultiDof(humanoid,rightKnee,pose._rightKneeRot)
bc.resetJointStateMultiDof(humanoid,rightAnkle,pose._rightAnkleRot)
bc.resetJointStateMultiDof(humanoid,rightShoulder,pose._rightShoulderRot)
bc.resetJointStateMultiDof(humanoid,rightElbow, pose._rightElbowRot)
bc.resetJointStateMultiDof(humanoid,leftHip, pose._leftHipRot)
bc.resetJointStateMultiDof(humanoid,leftKnee, pose._leftKneeRot)
bc.resetJointStateMultiDof(humanoid,leftAnkle, pose._leftAnkleRot)
bc.resetJointStateMultiDof(humanoid,leftShoulder, pose._leftShoulderRot)
bc.resetJointStateMultiDof(humanoid,leftElbow, pose._leftElbowRot)
bc.setJointMotorControlMultiDof(humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=[200])
bc.setJointMotorControlMultiDof(humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=[50])
bc.setJointMotorControlMultiDof(humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=[200])
bc.setJointMotorControlMultiDof(humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=[150])
bc.setJointMotorControlMultiDof(humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=[90])
bc.setJointMotorControlMultiDof(humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=[100])
bc.setJointMotorControlMultiDof(humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=[60])
bc.setJointMotorControlMultiDof(humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=[200])
bc.setJointMotorControlMultiDof(humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=[150])
bc.setJointMotorControlMultiDof(humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=[90])
bc.setJointMotorControlMultiDof(humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=[100])
bc.setJointMotorControlMultiDof(humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=[60])
#debug space
#if (False):
# for j in range (bc.getNumJoints(self._humanoid)):
# js = bc.getJointState(self._humanoid, j)
# bc.resetJointState(self._humanoidDebug, j,js[0])
# jsm = bc.getJointStateMultiDof(self._humanoid, j)
# if (len(jsm[0])>0):
# bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
def GetState(self):
stateVector = []
phase = self.GetPhase()
#print("phase=",phase)
stateVector.append(phase)
rootTransPos, rootTransOrn=self.BuildOriginTrans()
basePos,baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1])
#print("!!!rootPosRel =",rootPosRel )
#print("rootTransPos=",rootTransPos)
#print("basePos=",basePos)
localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn )
localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]]
#print("localPos=",localPos)
stateVector.append(rootPosRel[1])
self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8]
for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)):
j = self.pb2dmJoints[pbJoint]
#print("joint order:",j)
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True)
linkPos = ls[0]
linkOrn = ls[1]
linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, linkPos,linkOrn)
if (linkOrnLocal[3]<0):
linkOrnLocal=[-linkOrnLocal[0],-linkOrnLocal[1],-linkOrnLocal[2],-linkOrnLocal[3]]
linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]]
for l in linkPosLocal:
stateVector.append(l)
#re-order the quaternion, DeepMimic uses w,x,y,z
stateVector.append(linkOrnLocal[3])
stateVector.append(linkOrnLocal[0])
stateVector.append(linkOrnLocal[1])
stateVector.append(linkOrnLocal[2])
for pbJoint in range (self._pybullet_client.getNumJoints(self._humanoid)):
j = self.pb2dmJoints[pbJoint]
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True)
linkLinVel = ls[6]
linkAngVel = ls[7]
for l in linkLinVel:
stateVector.append(l)
for l in linkAngVel:
stateVector.append(l)
#print("stateVector len=",len(stateVector))
#for st in range (len(stateVector)):
# print("state[",st,"]=",stateVector[st])
return stateVector
def GetReward(self):
#from DeepMimic double cSceneImitate::CalcRewardImitate
pose_w = 0.5
vel_w = 0.05
end_eff_w = 0 #0.15
root_w = 0 #0.2
com_w = 0.1
total_w = pose_w + vel_w + end_eff_w + root_w + com_w
pose_w /= total_w
vel_w /= total_w
end_eff_w /= total_w
root_w /= total_w
com_w /= total_w
pose_scale = 2
vel_scale = 0.1
end_eff_scale = 40
root_scale = 5
com_scale = 10
err_scale = 1
reward = 0
pose_err = 0
vel_err = 0
end_eff_err = 0
root_err = 0
com_err = 0
heading_err = 0
#create a mimic reward, comparing the dynamics humanoid with a kinematic one
pose = self.InitializePoseFromMotionData()
#print("self._kinematicHumanoid=",self._kinematicHumanoid)
#print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid))
self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_client)
#const Eigen::VectorXd& pose0 = sim_char.GetPose();
#const Eigen::VectorXd& vel0 = sim_char.GetVel();
#const Eigen::VectorXd& pose1 = kin_char.GetPose();
#const Eigen::VectorXd& vel1 = kin_char.GetVel();
#tMatrix origin_trans = sim_char.BuildOriginTrans();
#tMatrix kin_origin_trans = kin_char.BuildOriginTrans();
#
#tVector com0_world = sim_char.CalcCOM();
#tVector com_vel0_world = sim_char.CalcCOMVel();
#tVector com1_world;
#tVector com_vel1_world;
#cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world);
#
root_id = 0
#tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0);
#tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1);
#tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0);
#tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1);
#tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0);
#tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1);
#tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0);
#tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1);
mJointWeights = [0.20833,0.10416, 0.0625, 0.10416,
0.0625, 0.041666666666666671, 0.0625, 0.0416,
0.00, 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000]
num_end_effs = 0
num_joints = 15
root_rot_w = mJointWeights[root_id]
#pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1)
#vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
for j in range (num_joints):
curr_pose_err = 0
curr_vel_err = 0
w = mJointWeights[j];
simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j)
#print("simJointInfo.pos=",simJointInfo[0])
#print("simJointInfo.vel=",simJointInfo[1])
kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid,j)
#print("kinJointInfo.pos=",kinJointInfo[0])
#print("kinJointInfo.vel=",kinJointInfo[1])
if (len(simJointInfo[0])==1):
angle = simJointInfo[0][0]-kinJointInfo[0][0]
curr_pose_err = angle*angle
velDiff = simJointInfo[1][0]-kinJointInfo[1][0]
curr_vel_err = velDiff*velDiff
if (len(simJointInfo[0])==4):
#print("quaternion diff")
diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0],kinJointInfo[0])
axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat)
curr_pose_err = angle*angle
diffVel = [simJointInfo[1][0]-kinJointInfo[1][0],simJointInfo[1][1]-kinJointInfo[1][1],simJointInfo[1][2]-kinJointInfo[1][2]]
curr_vel_err = diffVel[0]*diffVel[0]+diffVel[1]*diffVel[1]+diffVel[2]*diffVel[2]
pose_err += w * curr_pose_err
vel_err += w * curr_vel_err
# bool is_end_eff = sim_char.IsEndEffector(j)
# if (is_end_eff)
# {
# tVector pos0 = sim_char.CalcJointPos(j)
# tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
# double ground_h0 = mGround->SampleHeight(pos0)
# double ground_h1 = kin_char.GetOriginPos()[1]
#
# tVector pos_rel0 = pos0 - root_pos0
# tVector pos_rel1 = pos1 - root_pos1
# pos_rel0[1] = pos0[1] - ground_h0
# pos_rel1[1] = pos1[1] - ground_h1
#
# pos_rel0 = origin_trans * pos_rel0
# pos_rel1 = kin_origin_trans * pos_rel1
#
# curr_end_err = (pos_rel1 - pos_rel0).squaredNorm()
# end_eff_err += curr_end_err;
# ++num_end_effs;
# }
#}
#if (num_end_effs > 0):
# end_eff_err /= num_end_effs
#
#double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
#double root_ground_h1 = kin_char.GetOriginPos()[1]
#root_pos0[1] -= root_ground_h0
#root_pos1[1] -= root_ground_h1
#root_pos_err = (root_pos0 - root_pos1).squaredNorm()
#
#root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
#root_rot_err *= root_rot_err
#root_vel_err = (root_vel1 - root_vel0).squaredNorm()
#root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()
#root_err = root_pos_err
# + 0.1 * root_rot_err
# + 0.01 * root_vel_err
# + 0.001 * root_ang_vel_err
#com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm()
#print("pose_err=",pose_err)
#print("vel_err=",vel_err)
pose_reward = math.exp(-err_scale * pose_scale * pose_err)
vel_reward = math.exp(-err_scale * vel_scale * vel_err)
end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err)
root_reward = math.exp(-err_scale * root_scale * root_err)
com_reward = math.exp(-err_scale * com_scale * com_err)
reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward
#print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward,
# pose_reward,vel_reward,end_eff_reward, root_reward, com_reward);
return reward
def GetBasePosition(self):
pos,orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
return pos

277
examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py
View File

@@ -1,277 +0,0 @@
"""This file implements the gym environment of humanoid deepmimic using PyBullet.
"""
import math
import time
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
import gym
from gym import spaces
from gym.utils import seeding
import random
import numpy as np
import pybullet
import pybullet_data
from pybullet_envs.deep_mimic.humanoid import Humanoid
from pkg_resources import parse_version
from pybullet_utils import bullet_client
from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData
RENDER_HEIGHT = 360
RENDER_WIDTH = 480
class HumanoidDeepMimicGymEnv(gym.Env):
"""The gym environment for the humanoid deep mimic.
"""
metadata = {
"render.modes": ["human", "rgb_array"],
"video.frames_per_second": 100
}
def __init__(self,
urdf_root=pybullet_data.getDataPath(),
render=False):
"""Initialize the gym environment.
Args:
urdf_root: The path to the urdf data folder.
render: Whether to render the simulation.
Raises:
ValueError: If the urdf_version is not supported.
"""
# Set up logging.
self._urdf_root = urdf_root
self._observation = []
self._env_step_counter = 0
self._is_render = render
self._cam_dist = 1.0
self._cam_yaw = 0
self._cam_pitch = -30
self._ground_id = None
self._pybullet_client = None
self._humanoid = None
self._control_time_step = 8.*(1./240.)#0.033333
self.seed()
observation_high = (self._get_observation_upper_bound())
observation_low = (self._get_observation_lower_bound())
action_dim = 36
self._action_bound = 3.14 #todo: check this
action_high = np.array([self._action_bound] * action_dim)
self.action_space = spaces.Box(-action_high, action_high, dtype=np.float32)
self.observation_space = spaces.Box(observation_low, observation_high, dtype=np.float32)
def close(self):
self._humanoid = None
self._pybullet_client.disconnect()
def reset(self):
if (self._pybullet_client==None):
if self._is_render:
self._pybullet_client = bullet_client.BulletClient(
connection_mode=pybullet.GUI)
else:
self._pybullet_client = bullet_client.BulletClient()
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
self._motion=MotionCaptureData()
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
self._motion.Load(motionPath)
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_RENDERING, 0)
self._pybullet_client.resetSimulation()
self._pybullet_client.setGravity(0,-9.8,0)
y2zOrn = self._pybullet_client.getQuaternionFromEuler([-1.57,0,0])
self._ground_id = self._pybullet_client.loadURDF(
"%s/plane.urdf" % self._urdf_root, [0,0,0], y2zOrn)
#self._pybullet_client.changeVisualShape(self._ground_id,-1,rgbaColor=[1,1,1,0.8])
#self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION,self._ground_id)
shift=[0,0,0]
self._humanoid = Humanoid(self._pybullet_client,self._motion,shift)
self._humanoid.Reset()
simTime = random.randint(0,self._motion.NumFrames()-2)
self._humanoid.SetSimTime(simTime)
pose = self._humanoid.InitializePoseFromMotionData()
self._humanoid.ApplyPose(pose, True, True, self._humanoid._humanoid,self._pybullet_client)
self._env_step_counter = 0
self._objectives = []
self._pybullet_client.resetDebugVisualizerCamera(
self._cam_dist, self._cam_yaw, self._cam_pitch, [0, 0, 0])
self._pybullet_client.configureDebugVisualizer(
self._pybullet_client.COV_ENABLE_RENDERING, 1)
return self._get_observation()
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def step(self, action):
"""Step forward the simulation, given the action.
Args:
action: A list of desired motor angles for eight motors.
Returns:
observations: The angles, velocities and torques of all motors.
reward: The reward for the current state-action pair.
done: Whether the episode has ended.
info: A dictionary that stores diagnostic information.
Raises:
ValueError: The action dimension is not the same as the number of motors.
ValueError: The magnitude of actions is out of bounds.
"""
self._last_base_position = self._humanoid.GetBasePosition()
if self._is_render:
# Sleep, otherwise the computation takes less time than real time,
# which will make the visualization like a fast-forward video.
#time_spent = time.time() - self._last_frame_time
#self._last_frame_time = time.time()
#time_to_sleep = self._control_time_step - time_spent
#if time_to_sleep > 0:
# time.sleep(time_to_sleep)
base_pos = self._humanoid.GetBasePosition()
# Keep the previous orientation of the camera set by the user.
[yaw, pitch,
dist] = self._pybullet_client.getDebugVisualizerCamera()[8:11]
self._pybullet_client.resetDebugVisualizerCamera(dist, yaw, pitch,
base_pos)
self._humanoid.ApplyAction(action)
for s in range (8):
#print("step:",s)
self._pybullet_client.stepSimulation()
self._initial_frame = self._initial_frame + self._control_time_step
self._humanoid.setSimTime(self._initial_frame)
reward = self._reward()
done = self._termination()
self._env_step_counter += 1
return np.array(self._get_observation()), reward, done, {}
def render(self, mode="rgb_array", close=False):
if mode == "human":
self._is_render = True
if mode != "rgb_array":
return np.array([])
base_pos = self._humanoid.GetBasePosition()
view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._cam_dist,
yaw=self._cam_yaw,
pitch=self._cam_pitch,
roll=0,
upAxisIndex=1)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
fov=60,
aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._pybullet_client.getCameraImage(
width=RENDER_WIDTH,
height=RENDER_HEIGHT,
renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def _termination(self):
if (self._humanoid):
term = self._humanoid.Terminates()
return term
return False
def _reward(self):
reward = 0
if (self._humanoid):
reward = self._humanoid.GetReward()
return reward
def get_objectives(self):
return self._objectives
@property
def objective_weights(self):
"""Accessor for the weights for all the objectives.
Returns:
List of floating points that corresponds to weights for the objectives in
the order that objectives are stored.
"""
return self._objective_weights
def _get_observation(self):
"""Get observation of this environment.
"""
observation = []
if (self._humanoid):
observation = self._humanoid.GetState()
else:
observation = [0]*197
self._observation = observation
return self._observation
def _get_observation_upper_bound(self):
"""Get the upper bound of the observation.
Returns:
The upper bound of an observation. See GetObservation() for the details
of each element of an observation.
"""
upper_bound = np.zeros(self._get_observation_dimension())
upper_bound[0] = 10 #height
upper_bound[1:107] = math.pi # Joint angle.
upper_bound[107:197] = 10 #joint velocity, check it
return upper_bound
def _get_observation_lower_bound(self):
"""Get the lower bound of the observation."""
return -self._get_observation_upper_bound()
def _get_observation_dimension(self):
"""Get the length of the observation list.
Returns:
The length of the observation list.
"""
return len(self._get_observation())
def configure(self, args):
pass
if parse_version(gym.__version__) < parse_version('0.9.6'):
_render = render
_reset = reset
_seed = seed
_step = step
_close = close
@property
def pybullet_client(self):
return self._pybullet_client
@property
def ground_id(self):
return self._ground_id
@ground_id.setter
def ground_id(self, new_ground_id):
self._ground_id = new_ground_id
@property
def env_step_counter(self):
return self._env_step_counter

87
examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py
View File

@@ -1,87 +0,0 @@
import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0,parentdir)
from pybullet_envs.deep_mimic.humanoid import Humanoid
from pybullet_utils.bullet_client import BulletClient
from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData
import pybullet_data
import pybullet
import time
import random
bc = BulletClient(connection_mode=pybullet.GUI)
bc.setAdditionalSearchPath(pybullet_data.getDataPath())
bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP,1)
bc.setGravity(0,-9.8,0)
motion=MotionCaptureData()
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
motion.Load(motionPath)
#print("numFrames = ", motion.NumFrames())
simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0)
y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0])
bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn)
humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000])
simTime = 0
keyFrameDuration = motion.KeyFrameDuraction()
#print("keyFrameDuration=",keyFrameDuration)
#for i in range (50):
# bc.stepSimulation()
stage = 0
def Reset(humanoid):
global simTime
humanoid.Reset()
simTime = 0 #random.randint(0,motion.NumFrames()-2)
humanoid.SetSimTime(simTime)
pose = humanoid.InitializePoseFromMotionData()
humanoid.ApplyPose(pose, True, True, humanoid._humanoid,bc)
Reset(humanoid)
#bc.stepSimulation()
while (1):
#simTime = bc.readUserDebugParameter(frameTimeId)
#print("keyFrameDuration=",keyFrameDuration)
dt = (1./240.)
#print("------------------------------------------")
#print("dt=",dt)
#print("simTime=",simTime)
#print("humanoid.SetSimTime(simTime)")
humanoid.SetSimTime(simTime)
#pose = humanoid.InitializePoseFromMotionData()
#humanoid.ApplyPose(pose, True)#False)#False, False)
if (humanoid.Terminates()):
Reset(humanoid)
state = humanoid.GetState()
print("len(state)=",len(state))
print("state=", state)
action = [0]*36
humanoid.ApplyAction(action)
for s in range (8):
#print("step:",s)
bc.stepSimulation()
simTime += dt
time.sleep(1./240.)
reward = humanoid.GetReward()
print("reward=",reward)

1
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
View File

@@ -147,6 +147,7 @@ class PPOAgent(PGAgent):
def _decide_action(self, s, g):
with self.sess.as_default(), self.graph.as_default():
self._exp_action = self._enable_stoch_policy() and MathUtil.flip_coin(self.exp_params_curr.rate)
#print("_decide_action._exp_action=",self._exp_action)
a, logp = self._eval_actor(s, g, self._exp_action)
return a[0], logp[0]

5
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
View File

@@ -123,8 +123,7 @@ class RLAgent(ABC):
if self.need_new_action():
#print("update_new_action!!!")
self._update_new_action()
else:
print("no action???")
if (self._mode == self.Mode.TRAIN and self.enable_training):
self._update_counter += timestep
@@ -329,7 +328,9 @@ class RLAgent(ABC):
return
def _update_new_action(self):
#print("_update_new_action!")
s = self._record_state()
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
g = self._record_goal()
if not (self._is_first_step()):

19
examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py
View File

@@ -1,19 +0,0 @@
import json
class MotionCaptureData(object):
def __init__(self):
self.Reset()
def Reset(self):
self._motion_data = []
def Load(self, path):
with open(path, 'r') as f:
self._motion_data = json.load(f)
def NumFrames(self):
return len(self._motion_data['Frames'])
def KeyFrameDuraction(self):
return self._motion_data['Frames'][0][0]

15
examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
View File

@@ -16,6 +16,7 @@ import sys
import random
update_timestep = 1./600.
animating = True
def update_world(world, time_elapsed):
timeStep = 1./600.
@@ -74,9 +75,21 @@ def build_world(args, enable_draw):
world.reset()
return world
if __name__ == '__main__':
world = build_world(args, True)
while (world.env._pybullet_client.isConnected()):
timeStep = 1./600.
update_world(world, timeStep)
keys = world.env.getKeyboardEvents()
if world.env.isKeyTriggered(keys, ' '):
animating = not animating
if (animating):
update_world(world, timeStep)
#animating=False

37
examples/pybullet/gym/pybullet_envs/examples/dominoes.py Normal file
View File

@@ -0,0 +1,37 @@
import pybullet_data as pd
import pybullet_utils as pu
import pybullet
import pybullet_utils.bullet_client as bc
import time
p = bc.BulletClient(connection_mode=pybullet.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
p#.setPhysicsEngineParameter(numSolverIterations=10, fixedTimeStep=0.01)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
y2z = p.getQuaternionFromEuler([0,0,1.57])
meshScale = [1,1,1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="domino/domino.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFrameOrientation=y2z, meshScale=meshScale)
boxDimensions = [0.5*0.00635, 0.5*0.0254, 0.5*0.0508]
collisionShapeId = p.createCollisionShape(p.GEOM_BOX,halfExtents=boxDimensions)
for j in range (12):
print("j=",j)
for i in range (35):
#p.loadURDF("domino/domino.urdf",[i*0.04,0, 0.06])
p.createMultiBody(baseMass=0.025,baseCollisionShapeIndex = collisionShapeId,baseVisualShapeIndex = visualShapeId, basePosition = [i*0.04,j*0.05, 0.06], useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.setGravity(0,0,-9.8)
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-9.8)
#p.stepSimulation(1./100.)
time.sleep(1./240.)

201
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py Normal file
View File

@@ -0,0 +1,201 @@
import numpy as np
class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def quatMul(self, q1, q2):
return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]]
def computeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
q_diff = self.quatMul(ornStartConjugate, ornEnd)#bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
#print("q1=",q1)
#qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
#print("baseLinVel=",baseLinVel)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
#qError = [0,0,0, 0,0,0,0]
desiredOrn = [desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
axis1 = self._pb.getAxisDifferenceQuaternion(desiredOrn,curOrn)
angDiff = [0,0,0]#self.computeAngVel(curOrn, desiredOrn, 1, self._pb)
qError=[ desiredPositions[0]-curPos[0], desiredPositions[1]-curPos[1], desiredPositions[2]-curPos[2],angDiff[0],angDiff[1],angDiff[2],0]
target_pos = np.array(desiredPositions)
#np.savetxt("pb_target_pos.csv", target_pos, delimiter=",")
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
#axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
angDiff = self.computeAngVelRel(jointPos, desiredPos, 1, self._pb)
#angDiff = self._pb.computeAngVelRel(jointPos, desiredPos, 1)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(angDiff[0])
qError.append(angDiff[1])
qError.append(angDiff[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
q = np.array(q1)
qerr = np.array(qError)
#np.savetxt("pb_qerro.csv",qerr,delimiter=",")
#np.savetxt("pb_q.csv", q, delimiter=",")
qdot=np.array(qdot1)
#np.savetxt("qdot.csv", qdot, delimiter=",")
qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired-qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
#np.savetxt("pb_d.csv", d, delimiter=",")
#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
M2 = np.array(M1)
#np.savetxt("M2.csv", M2, delimiter=",")
M = (M2 + Kd * timeStep)
#np.savetxt("pbM_tKd.csv",M, delimiter=",")
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
c = np.array(c1)
#np.savetxt("pb_C.csv",c, delimiter=",")
A = M
#p = [0]*43
#np.savetxt("pb_kp_dot_qError.csv", p)
#np.savetxt("pb_kd_dot_vError.csv", d)
b = p + d -c
#np.savetxt("pb_b_acc.csv",b, delimiter=",")
useNumpySolver = True
if useNumpySolver:
qddot = np.linalg.solve(A, b)
else:
dofCount = len(b)
print(dofCount)
qddot = self._pb.ldltSolve(bodyUniqueId, jointPositions=q1, b=b.tolist(), kd=kds, t=timeStep)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
#np.savetxt("pb_tau_not_clamped.csv", tau, delimiter=",")
maxF = np.array(maxForces)
#print("maxF",maxF)
forces = np.clip(tau, -maxF , maxF )
#np.savetxt("pb_tau_clamped.csv", tau, delimiter=",")
return forces
class PDControllerStable(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
#print("c=",c)
return tau

9
examples/pybullet/pybullet.c
View File

@@ -9286,7 +9286,7 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
int physicsClientId = 0;
int hasQuatStart = 0;
int hasQuatEnd = 0;
static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId))
{
@@ -10711,11 +10711,11 @@ static PyMethodDef SpamMethods[] = {
{NULL, NULL, 0, NULL} /* Sentinel */
};
///copied from CommonWindowInterface.h
///copied from CommonCallbacks.h
enum
{
B3G_ESCAPE = 27,
B3G_SPACE = 32,
B3G_F1 = 0xff00,
B3G_F2,
B3G_F3,
@@ -10745,7 +10745,7 @@ enum
B3G_SHIFT,
B3G_CONTROL,
B3G_ALT,
B3G_RETURN
B3G_RETURN,
};
#if PY_MAJOR_VERSION >= 3
@@ -10959,6 +10959,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL);
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN);
PyModule_AddIntConstant(m, "B3G_SPACE", B3G_SPACE);
PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);