diff options
| author | erwincoumans <erwin.coumans@gmail.com> | 2019-02-11 05:45:21 -0800 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2019-02-11 05:45:21 -0800 |
| commit | 12e647868965d00046903396f088eaa810efd4bf (patch) | |
| tree | a91b0ab51e27c94a363e7984557eabf0be56623a /examples/pybullet/gym | |
| parent | b574a360f584bcd54cd8ed59b725af2224cd1ffe (diff) | |
| parent | d4292fdac3fe71d404732a91849428732aabc8dd (diff) | |
| download | bullet3-12e647868965d00046903396f088eaa810efd4bf.tar.gz | |
Merge pull request #2103 from erwincoumans/master
PyBullet deep_mimic backflip re-using original DeepMimic policy from Jason Peng
Diffstat (limited to 'examples/pybullet/gym')
27 files changed, 821 insertions, 1290 deletions
diff --git a/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.data-00000-of-00001 b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.data-00000-of-00001 Binary files differnew file mode 100644 index 000000000..aac769ddc --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.data-00000-of-00001 diff --git a/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.index b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.index Binary files differnew file mode 100644 index 000000000..bfbcad39c --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/data/policies/humanoid3d/humanoid3d_backflip.ckpt.index diff --git a/examples/pybullet/gym/pybullet_data/domino/domino.jpg b/examples/pybullet/gym/pybullet_data/domino/domino.jpg Binary files differnew file mode 100644 index 000000000..11d0d0750 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/domino/domino.jpg diff --git a/examples/pybullet/gym/pybullet_data/domino/domino.mtl b/examples/pybullet/gym/pybullet_data/domino/domino.mtl new file mode 100644 index 000000000..a602fae98 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/domino/domino.mtl @@ -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 diff --git a/examples/pybullet/gym/pybullet_data/domino/domino.obj b/examples/pybullet/gym/pybullet_data/domino/domino.obj new file mode 100644 index 000000000..6103b5ed5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/domino/domino.obj @@ -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 diff --git a/examples/pybullet/gym/pybullet_data/domino/domino.urdf b/examples/pybullet/gym/pybullet_data/domino/domino.urdf new file mode 100644 index 000000000..8a91e09e2 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/domino/domino.urdf @@ -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> + diff --git a/examples/pybullet/gym/pybullet_data/domino/license.txt b/examples/pybullet/gym/pybullet_data/domino/license.txt new file mode 100644 index 000000000..bd3b1943f --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/domino/license.txt @@ -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} +} diff --git a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf index 334b5eda1..ca5205f67 100644 --- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf +++ b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf @@ -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" >
diff --git a/examples/pybullet/gym/pybullet_data/plane_implicit.urdf b/examples/pybullet/gym/pybullet_data/plane_implicit.urdf new file mode 100644 index 000000000..7547f4698 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane_implicit.urdf @@ -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> + diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.mtl b/examples/pybullet/gym/pybullet_data/plane_transparent.mtl new file mode 100644 index 000000000..8ce3f6b30 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane_transparent.mtl @@ -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 + + diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.obj b/examples/pybullet/gym/pybullet_data/plane_transparent.obj new file mode 100644 index 000000000..02e572bcc --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane_transparent.obj @@ -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 diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.urdf b/examples/pybullet/gym/pybullet_data/plane_transparent.urdf new file mode 100644 index 000000000..99fad2902 --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/plane_transparent.urdf @@ -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> + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py index f2b2400ca..8b1378917 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py @@ -1 +1 @@ -from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py index 0a6581c62..af0870ff5 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py @@ -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): - - 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] + 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 = basePos + self._baseLinVel = baseLinVel + #print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel) + self._baseOrn =baseOrn + self._baseAngVel = baseAngVel + + self._chestRot = chestRot + self._chestVel =chestVel + self._neckRot = neckRot + self._neckVel = neckVel + + self._rightHipRot = rightHipRot + self._rightHipVel = rightHipVel + self._rightKneeRot =rightKneeRot + self._rightKneeVel = rightKneeVel + self._rightAnkleRot = rightAnkleRot + self._rightAnkleVel = rightAnkleVel + + 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._leftShoulderRot = [0,0,0,1] - self._leftShoulderVel = [0,0,0] - self._leftElbowRot = [0] - self._leftElbowVel = [0] + self._leftHipRot = leftHipRot + self._leftHipVel = leftHipVel + self._leftKneeRot = leftKneeRot + self._leftKneeVel = leftKneeVel + self._leftAnkleRot =leftAnkleRot + self._leftAnkleVel = leftAnkleVel + + 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]] diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py index 3239a7e6c..c3e7ca54e 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py @@ -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): diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py deleted file mode 100644 index bb0f5f243..000000000 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py +++ /dev/null @@ -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 diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 272c9df13..3159fcb29 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -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 diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py new file mode 100644 index 000000000..5cf9a5bcc --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py @@ -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.)
\ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py deleted file mode 100644 index 1c7dbcc3a..000000000 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py +++ /dev/null @@ -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 - diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py deleted file mode 100644 index 124664e91..000000000 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py +++ /dev/null @@ -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 diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py deleted file mode 100644 index c4eacbc1c..000000000 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py +++ /dev/null @@ -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) - diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py index 1b9489489..ece705302 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py @@ -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] diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py index 25886c659..3e66eb270 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py @@ -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()): diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py deleted file mode 100644 index 7a9c86b76..000000000 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py +++ /dev/null @@ -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] - diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py index 0ed1afde8..31b976563 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py @@ -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 diff --git a/examples/pybullet/gym/pybullet_envs/examples/dominoes.py b/examples/pybullet/gym/pybullet_envs/examples/dominoes.py new file mode 100644 index 000000000..c96c07ebd --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/examples/dominoes.py @@ -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.)
\ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py new file mode 100644 index 000000000..04424b9a1 --- /dev/null +++ b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py @@ -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 |
