Merge pull request #2001 from erwincoumans/master

DeepMimic using PyBullet (work-in-progress)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -5266,6 +5266,16 @@ B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double
 	outOrn[3] = result[3];
 }
 
+B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/])
+{
+	b3Quaternion q(quat[0], quat[1], quat[2], quat[3]);
+	b3Vector3 v = b3MakeVector3(vec[0], vec[1], vec[2]);
+	b3Vector3 vout = b3QuatRotate(q, v);
+	vecOut[0] = vout[0];
+	vecOut[1] = vout[1];
+	vecOut[2] = vout[2];
+}
+
 B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/])
 {
 	b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);

examples/SharedMemory/PhysicsClientC_API.h

@@ -639,6 +639,7 @@ extern "C"
 	B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle);
 	B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]);
 	B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]);
+	B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]);
 
 	
 

examples/SharedMemory/Win32SharedMemory.cpp

@@ -91,7 +91,7 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreat
 		}
 		else
 		{
-			b3Warning("Could not create file mapping object (%d).\n", GetLastError());
+			//b3Warning("Could not create file mapping object (%d).\n", GetLastError());
 			return 0;
 		}
 	}

examples/pybullet/examples/shiftCenterOfMass.py

@@ -0,0 +1,22 @@
+import pybullet as p
+import time
+import pybullet_data
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.loadURDF("plane.urdf")
+p.setGravity(0,0,-10)
+visualShift = [0,0,0]
+collisionShift = [0,0,0]
+inertiaShift = [0,0,-0.5]
+
+meshScale=[1,1,1]
+visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale)
+collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
+
+p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False)
+while (1):
+	p.stepSimulation()
+	time.sleep(1./240.)
+		

examples/pybullet/gym/pybullet_data/args/kin_char_args.txt

@@ -0,0 +1,4 @@
+--scene kin_char
+
+--character_file data/characters/humanoid3d.txt
+--motion_file data/motions/humanoid3d_spinkick.txt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_backflip.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_backflip.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 9 10 12 13
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_cartwheel.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_cartwheel.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_crawl.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_crawl.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_dance_a.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_dance_a.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_dance_b.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_dance_b.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_getup_facedown.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_getup_facedown.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_getup_faceup.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_getup_faceup.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_jump.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_jump.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_kick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_kick.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_punch.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_punch.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_roll.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_roll.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_run.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_run.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spin.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_spin.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spinkick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_spinkick.ckpt

examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt

@@ -0,0 +1,25 @@
+--scene imitate
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_walk.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+--train_agents false
+
+#--output_path output
+#--int_output_path output/intermediate
+--model_files data/policies/humanoid3d/humanoid3d_walk.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt

@@ -0,0 +1,34 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_backflip.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+--enable_root_rot_fail false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt

@@ -0,0 +1,34 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 9 10 12 13
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_cartwheel.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+--enable_root_rot_fail false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_crawl.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_dance_a.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_dance_b.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt

@@ -0,0 +1,34 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+--hold_end_frame 0.3
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_getup_facedown.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt

@@ -0,0 +1,34 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+--hold_end_frame 0.3
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_getup_faceup.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_jump.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 4
+--time_end_lim_max 4
+--time_end_lim_exp 10
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_kick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 4
+--time_end_lim_max 4
+--time_end_lim_exp 10
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_punch.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--enable_char_contact_fall false
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_roll.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_run.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spin.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_spinkick.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt

@@ -0,0 +1,33 @@
+--scene imitate
+
+--time_lim_min 0.5
+--time_lim_max 0.5
+--time_lim_exp 0.2
+--time_end_lim_min 20
+--time_end_lim_max 20
+--time_end_lim_exp 50
+--anneal_samples 32000000
+
+--num_update_substeps 10
+--num_sim_substeps 2
+--world_scale 4
+
+--terrain_file data/terrain/plane.txt
+
+--char_types general
+--character_files data/characters/humanoid3d.txt
+--enable_char_soft_contact false
+--fall_contact_bodies 0 1 2 3 4 6 7 8 9 10 12 13 14
+
+--char_ctrls ct_pd
+--char_ctrl_files data/controllers/humanoid3d_ctrl.txt
+--motion_file data/motions/humanoid3d_walk.txt
+--sync_char_root_pos true
+--sync_char_root_rot false
+
+--agent_files data/agents/ct_agent_humanoid_ppo.txt
+#--train_agents false
+
+--output_path output
+#--int_output_path output/intermediate
+#--model_files data/policies/humanoid3d/agent0_model.ckpt

examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf

@@ -1,12 +1,12 @@
 <robot name="dumpUrdf">
-	<link name="root" >
+	<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" />
 		</inertial>
 	</link>
-	<link name="link0" >
+	<link name="root" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 0.280000 0.000000" />
 			 <mass value = "6.000000" />
@@ -19,12 +19,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint0" type="fixed" >
-		<parent link = "root" />
-		<child link="link0" />
+	<joint name="root" type="fixed" >
+		<parent link = "base" />
+		<child link="root" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
 	</joint>
-	<link name="link1" >
+	<link name="chest" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 0.480000 0.000000" />
 			 <mass value = "14.000000" />
@@ -37,12 +37,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint1" type="spherical" >
-		<parent link="link0" />
-		<child link="link1" />
+	<joint name="chest" type="spherical" >
+		<parent link="root" />
+		<child link="chest" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.944604 0.000000" />
 	</joint>
-	<link name="link2" >
+	<link name="neck" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 0.700000 0.000000" />
 			 <mass value = "2.000000" />
@@ -55,12 +55,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint2" type="spherical" >
-		<parent link="link1" />
-		<child link="link2" />
+	<joint name="neck" type="spherical" >
+		<parent link="chest" />
+		<child link="neck" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.895576 0.000000" />
 	</joint>
-	<link name="link3" >
+	<link name="right_hip" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
 			 <mass value = "4.500000" />
@@ -73,12 +73,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint3" type="spherical" >
-		<parent link="link0" />
-		<child link="link3" />
+	<joint name="right_hip" type="spherical" >
+		<parent link="root" />
+		<child link="right_hip" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.339548" />
 	</joint>
-	<link name="link4" >
+	<link name="right_knee" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
 			 <mass value = "3.000000" />
@@ -91,13 +91,14 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint4" type="continuous" >
-		<parent link="link3" />
-		<child link="link4" />
-				<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
+	<joint name="right_knee" type="revolute" >
+		<parent link="right_hip" />
+		<child link="right_knee" />
+		 <limit effort="1000.0" lower="-3.14" upper="0." velocity="0.5"/>
+			<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
 		<axis xyz = "0.000000 0.000000 1.000000" />
 	</joint>
-	<link name="link5" >
+	<link name="right_ankle" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
 			 <mass value = "1.000000" />
@@ -110,12 +111,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint5" type="spherical" >
-		<parent link="link4" />
-		<child link="link5" />
+	<joint name="right_ankle" type="spherical" >
+		<parent link="right_knee" />
+		<child link="right_ankle" />
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
 	</joint>
-	<link name="link6" >
+	<link name="right_shoulder" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
 			 <mass value = "1.500000" />
@@ -128,12 +129,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint6" type="spherical" >
-		<parent link="link1" />
-		<child link="link6" />
+	<joint name="right_shoulder" type="spherical" >
+		<parent link="chest" />
+		<child link="right_shoulder" />
 				<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 0.732440" />
 	</joint>
-	<link name="link7" >
+	<link name="right_elbow" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
 			 <mass value = "1.000000" />
@@ -146,13 +147,14 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint7" type="continuous" >
-		<parent link="link6" />
-		<child link="link7" />
+	<joint name="right_elbow" type="revolute" >
+		<parent link="right_shoulder" />
+		<child link="right_elbow" />
+		<limit effort="1000.0" lower="0" upper="3.14" velocity="0.5"/>
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
 		<axis xyz = "0.000000 0.000000 1.000000" />
 	</joint>
-	<link name="link8" >
+	<link name="right_wrist" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
 			 <mass value = "0.500000" />
@@ -165,12 +167,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint8" type="fixed" >
-		<parent link="link7" />
-		<child link="link8" />
+	<joint name="right_wrist" type="fixed" >
+		<parent link="right_elbow" />
+		<child link="right_wrist" />
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
 	</joint>
-	<link name="link9" >
+	<link name="left_hip" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
 			 <mass value = "4.500000" />
@@ -183,12 +185,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint9" type="spherical" >
-		<parent link="link0" />
-		<child link="link9" />
+	<joint name="left_hip" type="spherical" >
+		<parent link="root" />
+		<child link="left_hip" />
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 -0.339548" />
 	</joint>
-	<link name="link10" >
+	<link name="left_knee" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
 			 <mass value = "3.000000" />
@@ -201,13 +203,14 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint10" type="continuous" >
-		<parent link="link9" />
-		<child link="link10" />
+	<joint name="left_knee" type="revolute" >
+		<parent link="left_hip" />
+		<child link="left_knee" />
+		<limit effort="1000.0" lower="-3.14" upper="0." velocity="0.5"/>
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
 		<axis xyz = "0.000000 0.000000 1.000000" />
 	</joint>
-	<link name="link11" >
+	<link name="left_ankle" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
 			 <mass value = "1.000000" />
@@ -220,12 +223,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint11" type="spherical" >
-		<parent link="link10" />
-		<child link="link11" />
+	<joint name="left_ankle" type="spherical" >
+		<parent link="left_knee" />
+		<child link="left_ankle" />
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
 	</joint>
-	<link name="link12" >
+	<link name="left_shoulder" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
 			 <mass value = "1.500000" />
@@ -238,12 +241,12 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint12" type="spherical" >
-		<parent link="link1" />
-		<child link="link12" />
+	<joint name="left_shoulder" type="spherical" >
+		<parent link="chest" />
+		<child link="left_shoulder" />
 				<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 -0.732440" />
 	</joint>
-	<link name="link13" >
+	<link name="left_elbow" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
 			 <mass value = "1.000000" />
@@ -256,13 +259,14 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint13" type="continuous" >
-		<parent link="link12" />
-		<child link="link13" />
+	<joint name="left_elbow" type="revolute" >
+		<parent link="left_shoulder" />
+		<child link="left_elbow" />
+		<limit effort="1000.0" lower="0" upper="3.14" velocity="0.5"/>
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
 		<axis xyz = "0.000000 0.000000 1.000000" />
 	</joint>
-	<link name="link14" >
+	<link name="left_wrist" >
 		<inertial>
 				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
 			 <mass value = "0.500000" />
@@ -275,9 +279,9 @@
 			</geometry>
 		</collision>
 	</link>
-	<joint name="joint14" type="fixed" >
-		<parent link="link13" />
-		<child link="link14" />
+	<joint name="left_wrist" type="fixed" >
+		<parent link="left_elbow" />
+		<child link="left_wrist" />
 				<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
 	</joint>
 </robot>

examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt

@@ -0,0 +1,45 @@
+{
+"Loop": "wrap",
+"Frames":
+[
+[        0.0333320000,        0.0000000000,        0.8475320000,        0.0000000000,        0.9986780000,        0.0141040000,       -0.0006980000,       -0.0494230000,        0.9988130000,        0.0094850000,       -0.0475600000,       -0.0044750000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9649400000,        0.0243690000,       -0.0575550000,        0.2549220000,       -0.2491160000,        0.9993660000,        0.0099520000,        0.0326540000,        0.0100980000,        0.9854980000,       -0.0644070000,        0.0932430000,       -0.1262970000,        0.1705710000,        0.9927550000,       -0.0209010000,        0.0888240000,       -0.0781780000,       -0.3915320000,        0.9828790000,        0.1013910000,       -0.0551600000,        0.1436190000,        0.9659420000,        0.1884590000,       -0.1422460000,        0.1058540000,        0.5813480000],
+[        0.0333320000,        0.0415400000,        0.8465850000,        0.0047220000,        0.9986070000,       -0.0037870000,        0.0074560000,       -0.0520870000,        0.9986070000,        0.0231250000,       -0.0473580000,        0.0025780000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9670290000,        0.0373040000,       -0.0422520000,        0.2483510000,       -0.2977710000,        0.9998640000,        0.0127380000,        0.0096330000,       -0.0042340000,        0.9855560000,       -0.0665800000,        0.0873690000,       -0.1288940000,        0.1815050000,        0.9942520000,        0.0102260000,        0.0881860000,       -0.0598400000,       -0.5307910000,        0.9843850000,        0.0977870000,       -0.0740410000,        0.1262580000,        0.9693550000,        0.1748130000,       -0.1459730000,        0.0921060000,        0.5672590000],
+[        0.0333320000,        0.0817250000,        0.8482530000,        0.0105010000,        0.9983820000,       -0.0253180000,        0.0032440000,       -0.0508180000,        0.9981930000,        0.0412880000,       -0.0434720000,        0.0040260000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9673350000,        0.0621840000,       -0.0445370000,        0.2416880000,       -0.3727180000,        0.9994770000,       -0.0213330000,        0.0181200000,        0.0161820000,        0.9851630000,       -0.0713220000,        0.1006090000,       -0.1193520000,        0.1756110000,        0.9943800000,        0.0459520000,        0.0909190000,       -0.0288200000,       -0.7122180000,        0.9896690000,        0.0717890000,       -0.0983600000,        0.0756710000,        0.9762440000,        0.1541960000,       -0.1302060000,        0.0788530000,        0.5370070000],
+[        0.0333320000,        0.1169630000,        0.8506510000,        0.0135860000,        0.9981250000,       -0.0353450000,        0.0015130000,       -0.0499400000,        0.9977950000,        0.0508470000,       -0.0416980000,        0.0089820000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9703800000,        0.0745730000,       -0.0429560000,        0.2257350000,       -0.3977670000,        0.9984780000,       -0.0369530000,        0.0142340000,        0.0383920000,        0.9847180000,       -0.0777390000,        0.1190810000,       -0.1005320000,        0.1713110000,        0.9949900000,        0.0647560000,        0.0743250000,        0.0166370000,       -0.9223910000,        0.9933260000,        0.0336230000,       -0.1079000000,        0.0230540000,        0.9827350000,        0.1368340000,       -0.1094470000,        0.0594120000,        0.5121800000],
+[        0.0333320000,        0.1503280000,        0.8547200000,        0.0157810000,        0.9980230000,       -0.0394800000,        0.0077050000,       -0.0483040000,        0.9977330000,        0.0519640000,       -0.0418120000,        0.0089230000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9747330000,        0.0790650000,       -0.0543960000,        0.2017060000,       -0.3934650000,        0.9972590000,       -0.0417730000,        0.0177010000,        0.0584460000,        0.9850070000,       -0.0833490000,        0.1302310000,       -0.0765050000,        0.1796490000,        0.9937930000,        0.0745150000,        0.0477340000,        0.0674090000,       -1.1046640000,        0.9928650000,        0.0062250000,       -0.1077960000,       -0.0505960000,        0.9871540000,        0.1235870000,       -0.0915170000,        0.0433300000,        0.4724830000],
+[        0.0333320000,        0.1830650000,        0.8600640000,        0.0173490000,        0.9981580000,       -0.0400090000,        0.0126040000,       -0.0438360000,        0.9979440000,        0.0506730000,       -0.0387370000,        0.0062810000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9799630000,        0.0791850000,       -0.0591360000,        0.1729320000,       -0.3762820000,        0.9962450000,       -0.0438390000,        0.0191110000,        0.0721740000,        0.9853390000,       -0.0903550000,        0.1340710000,       -0.0544840000,        0.2085050000,        0.9897330000,        0.0805020000,        0.0279590000,        0.1147470000,       -1.2360120000,        0.9845650000,       -0.0265310000,       -0.1003850000,       -0.1408900000,        0.9893540000,        0.1141940000,       -0.0850200000,        0.0301840000,        0.4168040000],
+[        0.0333320000,        0.2152590000,        0.8651470000,        0.0180420000,        0.9985020000,       -0.0367590000,        0.0105990000,       -0.0391100000,        0.9984290000,        0.0444100000,       -0.0339390000,        0.0040300000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9852450000,        0.0761030000,       -0.0563350000,        0.1425730000,       -0.3495410000,        0.9953800000,       -0.0434500000,        0.0206190000,        0.0831060000,        0.9844050000,       -0.0980730000,        0.1420870000,       -0.0337490000,        0.2497670000,        0.9845040000,        0.0806090000,        0.0192330000,        0.1545420000,       -1.3198790000,        0.9833010000,       -0.0173270000,       -0.0852970000,       -0.1598240000,        0.9909870000,        0.1089630000,       -0.0765260000,        0.0147010000,        0.3608970000],
+[        0.0333320000,        0.2470560000,        0.8693730000,        0.0179380000,        0.9989030000,       -0.0322850000,        0.0051270000,       -0.0335350000,        0.9989250000,        0.0357960000,       -0.0294280000,        0.0011110000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9898410000,        0.0716480000,       -0.0546010000,        0.1100030000,       -0.3161610000,        0.9945200000,       -0.0444670000,        0.0253540000,        0.0911600000,        0.9815660000,       -0.1079990000,        0.1571830000,       -0.0125770000,        0.3013270000,        0.9789940000,        0.0788810000,        0.0172150000,        0.1872250000,       -1.3577300000,        0.9891780000,        0.0057330000,       -0.0707090000,       -0.1284310000,        0.9924060000,        0.1060630000,       -0.0621780000,       -0.0039700000,        0.3157520000],
+[        0.0333320000,        0.2769800000,        0.8722940000,        0.0172890000,        0.9991290000,       -0.0268760000,       -0.0023020000,       -0.0318330000,        0.9992850000,        0.0284200000,       -0.0249180000,        0.0004180000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9928290000,        0.0652220000,       -0.0553300000,        0.0835140000,       -0.2875030000,        0.9934340000,       -0.0442090000,        0.0353110000,        0.0994410000,        0.9765250000,       -0.1222710000,        0.1770780000,        0.0095710000,        0.3523340000,        0.9722720000,        0.0757490000,        0.0196330000,        0.2203740000,       -1.3511600000,        0.9940540000,        0.0327350000,       -0.0606840000,       -0.0842780000,        0.9934730000,        0.1033700000,       -0.0410110000,       -0.0253650000,        0.2805770000],
+[        0.0333320000,        0.3062360000,        0.8738900000,        0.0171740000,        0.9991940000,       -0.0233840000,       -0.0108290000,       -0.0307910000,        0.9995110000,        0.0230890000,       -0.0210780000,       -0.0005190000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9947400000,        0.0606280000,       -0.0587570000,        0.0580050000,       -0.2592610000,        0.9921310000,       -0.0447990000,        0.0471680000,        0.1069800000,        0.9700480000,       -0.1378520000,        0.1973670000,        0.0324100000,        0.4029120000,        0.9655460000,        0.0745680000,        0.0249000000,        0.2480750000,       -1.3006270000,        0.9968650000,        0.0607040000,       -0.0424000000,       -0.0278910000,        0.9935740000,        0.1020610000,       -0.0138080000,       -0.0469420000,        0.2516170000],
+[        0.0333320000,        0.3342070000,        0.8741440000,        0.0164240000,        0.9991300000,       -0.0197040000,       -0.0202830000,       -0.0306650000,        0.9996800000,        0.0180100000,       -0.0177710000,       -0.0002450000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9960580000,        0.0549300000,       -0.0590820000,        0.0368760000,       -0.2401550000,        0.9906490000,       -0.0457460000,        0.0565300000,        0.1154430000,        0.9626530000,       -0.1537270000,        0.2156710000,        0.0561530000,        0.4451960000,        0.9598350000,        0.0731180000,        0.0338350000,        0.2687470000,       -1.1992650000,        0.9976300000,        0.0616240000,       -0.0216780000,        0.0216220000,        0.9923620000,        0.1015390000,        0.0171820000,       -0.0679090000,        0.2264920000],
+[        0.0333320000,        0.3616910000,        0.8735920000,        0.0151980000,        0.9989560000,       -0.0165980000,       -0.0273870000,       -0.0325760000,        0.9997680000,        0.0143740000,       -0.0160460000,       -0.0007560000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9968970000,        0.0493380000,       -0.0577300000,        0.0206980000,       -0.2312330000,        0.9893570000,       -0.0444080000,        0.0616240000,        0.1241060000,        0.9557200000,       -0.1679910000,        0.2277620000,        0.0806430000,        0.4772300000,        0.9556040000,        0.0704640000,        0.0421420000,        0.2829830000,       -1.0487890000,        0.9973700000,        0.0658490000,       -0.0045910000,        0.0299310000,        0.9901070000,        0.1009600000,        0.0439320000,       -0.0869740000,        0.2013980000],
+[        0.0333320000,        0.3886570000,        0.8721270000,        0.0138460000,        0.9986920000,       -0.0155010000,       -0.0321950000,       -0.0365910000,        0.9998060000,        0.0122360000,       -0.0154300000,       -0.0001980000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9973380000,        0.0456920000,       -0.0563030000,        0.0076880000,       -0.2255760000,        0.9880090000,       -0.0457620000,        0.0644030000,        0.1326500000,        0.9506770000,       -0.1786930000,        0.2314570000,        0.1034830000,        0.5028640000,        0.9530950000,        0.0682600000,        0.0505600000,        0.2905080000,       -0.8624410000,        0.9979920000,        0.0621880000,        0.0073170000,        0.0095870000,        0.9873870000,        0.1008070000,        0.0654830000,       -0.1030330000,        0.1768320000],
+[        0.0333320000,        0.4162070000,        0.8698920000,        0.0128170000,        0.9982790000,       -0.0177930000,       -0.0365930000,       -0.0422430000,        0.9998090000,        0.0128130000,       -0.0147600000,        0.0008960000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9975290000,        0.0453390000,       -0.0534220000,       -0.0052250000,       -0.2157660000,        0.9868960000,       -0.0450330000,        0.0659550000,        0.1402090000,        0.9471080000,       -0.1865960000,        0.2306250000,        0.1223940000,        0.5269460000,        0.9519170000,        0.0678870000,        0.0651890000,        0.2915410000,       -0.6550040000,        0.9984650000,        0.0539830000,        0.0111310000,       -0.0055220000,        0.9844990000,        0.1012040000,        0.0846310000,       -0.1155700000,        0.1551490000],
+[        0.0333320000,        0.4453160000,        0.8670590000,        0.0119150000,        0.9978440000,       -0.0228310000,       -0.0404430000,       -0.0463760000,        0.9997520000,        0.0168290000,       -0.0145230000,        0.0013260000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9972860000,        0.0471740000,       -0.0525150000,       -0.0209060000,       -0.2060700000,        0.9862080000,       -0.0410770000,        0.0700370000,        0.1442280000,        0.9437600000,       -0.1928680000,        0.2311860000,        0.1366480000,        0.5453590000,        0.9522830000,        0.0672370000,        0.0863160000,        0.2849320000,       -0.4426240000,        0.9986390000,        0.0506980000,       -0.0056960000,       -0.0108540000,        0.9808630000,        0.1016680000,        0.1058650000,       -0.1279200000,        0.1464150000],
+[        0.0333320000,        0.4768750000,        0.8640800000,        0.0093330000,        0.9972500000,       -0.0259170000,       -0.0484160000,       -0.0497730000,        0.9997120000,        0.0204270000,       -0.0125950000,        0.0009340000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9968350000,        0.0439130000,       -0.0556600000,       -0.0359640000,       -0.2058370000,        0.9849700000,       -0.0350340000,        0.0819060000,        0.1479800000,        0.9408130000,       -0.1976120000,        0.2337280000,        0.1455710000,        0.5620740000,        0.9539720000,        0.0617530000,        0.1125740000,        0.2710180000,       -0.2369080000,        0.9988540000,        0.0337440000,       -0.0318460000,       -0.0117250000,        0.9767650000,        0.1021930000,        0.1266790000,       -0.1394210000,        0.1471610000],
+[        0.0333320000,        0.5103140000,        0.8608720000,        0.0054350000,        0.9967350000,       -0.0269650000,       -0.0565160000,       -0.0509760000,        0.9997290000,        0.0207800000,       -0.0103310000,       -0.0016980000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9963180000,        0.0367660000,       -0.0593190000,       -0.0497910000,       -0.2214360000,        0.9835200000,       -0.0312430000,        0.0927800000,        0.1520030000,        0.9390830000,       -0.1992430000,        0.2368990000,        0.1493440000,        0.5758430000,        0.9576770000,        0.0560540000,        0.1220800000,        0.2545750000,       -0.0806400000,        0.9988660000,        0.0226260000,       -0.0412300000,       -0.0074390000,        0.9730470000,        0.1040510000,        0.1439350000,       -0.1470930000,        0.1518900000],
+[        0.0333320000,        0.5454110000,        0.8576010000,        0.0010220000,        0.9963850000,       -0.0276470000,       -0.0627650000,       -0.0501190000,        0.9997670000,        0.0193860000,       -0.0089680000,       -0.0030990000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9958970000,        0.0288280000,       -0.0587060000,       -0.0625510000,       -0.2506660000,        0.9828260000,       -0.0305860000,        0.0978030000,        0.1534660000,        0.9377420000,       -0.1985960000,        0.2440980000,        0.1470230000,        0.5853580000,        0.9596930000,        0.0545350000,        0.1175900000,        0.2493760000,       -0.0592230000,        0.9995230000,       -0.0059220000,       -0.0294090000,        0.0072780000,        0.9708880000,        0.1070260000,        0.1505500000,       -0.1524990000,        0.1588850000],
+[        0.0333320000,        0.5809200000,        0.8549060000,       -0.0043250000,        0.9964830000,       -0.0246090000,       -0.0607910000,       -0.0521520000,        0.9998220000,        0.0160670000,       -0.0097780000,       -0.0011460000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9957560000,        0.0184400000,       -0.0587140000,       -0.0684330000,       -0.2949010000,        0.9833820000,       -0.0303580000,        0.0973770000,        0.1501890000,        0.9387990000,       -0.1946620000,        0.2480910000,        0.1386140000,        0.5890180000,        0.9586480000,        0.0495910000,        0.1034380000,        0.2604540000,       -0.1549670000,        0.9995790000,        0.0047500000,       -0.0278210000,       -0.0067700000,        0.9702230000,        0.1116970000,        0.1506550000,       -0.1532770000,        0.1637470000],
+[        0.0333320000,        0.6192950000,        0.8475320000,       -0.0000000000,        0.9986780000,       -0.0141040000,        0.0006980000,       -0.0494230000,        0.9988130000,       -0.0094850000,        0.0475600000,       -0.0044750000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9927550000,        0.0209010000,       -0.0888240000,       -0.0781780000,       -0.3915320000,        0.9828790000,       -0.1013910000,        0.0551600000,        0.1436190000,        0.9659420000,       -0.1884590000,        0.1422460000,        0.1058540000,        0.5813480000,        0.9649400000,       -0.0243690000,        0.0575550000,        0.2549220000,       -0.2491160000,        0.9993660000,       -0.0099520000,       -0.0326540000,        0.0100980000,        0.9854980000,        0.0644070000,       -0.0932430000,       -0.1262970000,        0.1705710000],
+[        0.0333320000,        0.6608350000,        0.8465850000,       -0.0047220000,        0.9986070000,        0.0037870000,       -0.0074560000,       -0.0520870000,        0.9986070000,       -0.0231250000,        0.0473580000,        0.0025780000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9942520000,       -0.0102260000,       -0.0881860000,       -0.0598400000,       -0.5307910000,        0.9843850000,       -0.0977870000,        0.0740410000,        0.1262580000,        0.9693550000,       -0.1748130000,        0.1459730000,        0.0921060000,        0.5672590000,        0.9670290000,       -0.0373040000,        0.0422520000,        0.2483510000,       -0.2977710000,        0.9998640000,       -0.0127380000,       -0.0096330000,       -0.0042340000,        0.9855560000,        0.0665800000,       -0.0873690000,       -0.1288940000,        0.1815050000],
+[        0.0333320000,        0.7010200000,        0.8482530000,       -0.0105010000,        0.9983820000,        0.0253180000,       -0.0032440000,       -0.0508180000,        0.9981930000,       -0.0412880000,        0.0434720000,        0.0040260000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9943800000,       -0.0459520000,       -0.0909190000,       -0.0288200000,       -0.7122180000,        0.9896690000,       -0.0717890000,        0.0983600000,        0.0756710000,        0.9762440000,       -0.1541960000,        0.1302060000,        0.0788530000,        0.5370070000,        0.9673350000,       -0.0621840000,        0.0445370000,        0.2416880000,       -0.3727180000,        0.9994770000,        0.0213330000,       -0.0181200000,        0.0161820000,        0.9851630000,        0.0713220000,       -0.1006090000,       -0.1193520000,        0.1756110000],
+[        0.0333320000,        0.7362580000,        0.8506510000,       -0.0135860000,        0.9981250000,        0.0353450000,       -0.0015130000,       -0.0499400000,        0.9977950000,       -0.0508470000,        0.0416980000,        0.0089820000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9949900000,       -0.0647560000,       -0.0743250000,        0.0166370000,       -0.9223910000,        0.9933260000,       -0.0336230000,        0.1079000000,        0.0230540000,        0.9827350000,       -0.1368340000,        0.1094470000,        0.0594120000,        0.5121800000,        0.9703800000,       -0.0745730000,        0.0429560000,        0.2257350000,       -0.3977670000,        0.9984780000,        0.0369530000,       -0.0142340000,        0.0383920000,        0.9847180000,        0.0777390000,       -0.1190810000,       -0.1005320000,        0.1713110000],
+[        0.0333320000,        0.7696230000,        0.8547200000,       -0.0157810000,        0.9980230000,        0.0394800000,       -0.0077050000,       -0.0483040000,        0.9977330000,       -0.0519640000,        0.0418120000,        0.0089230000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9937930000,       -0.0745150000,       -0.0477340000,        0.0674090000,       -1.1046640000,        0.9928650000,       -0.0062250000,        0.1077960000,       -0.0505960000,        0.9871540000,       -0.1235870000,        0.0915170000,        0.0433300000,        0.4724830000,        0.9747330000,       -0.0790650000,        0.0543960000,        0.2017060000,       -0.3934650000,        0.9972590000,        0.0417730000,       -0.0177010000,        0.0584460000,        0.9850070000,        0.0833490000,       -0.1302310000,       -0.0765050000,        0.1796490000],
+[        0.0333320000,        0.8023600000,        0.8600640000,       -0.0173490000,        0.9981580000,        0.0400090000,       -0.0126040000,       -0.0438360000,        0.9979440000,       -0.0506730000,        0.0387370000,        0.0062810000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9897330000,       -0.0805020000,       -0.0279590000,        0.1147470000,       -1.2360120000,        0.9845650000,        0.0265310000,        0.1003850000,       -0.1408900000,        0.9893540000,       -0.1141940000,        0.0850200000,        0.0301840000,        0.4168040000,        0.9799630000,       -0.0791850000,        0.0591360000,        0.1729320000,       -0.3762820000,        0.9962450000,        0.0438390000,       -0.0191110000,        0.0721740000,        0.9853390000,        0.0903550000,       -0.1340710000,       -0.0544840000,        0.2085050000],
+[        0.0333320000,        0.8345540000,        0.8651470000,       -0.0180420000,        0.9985020000,        0.0367590000,       -0.0105990000,       -0.0391100000,        0.9984290000,       -0.0444100000,        0.0339390000,        0.0040300000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9845040000,       -0.0806090000,       -0.0192330000,        0.1545420000,       -1.3198790000,        0.9833010000,        0.0173270000,        0.0852970000,       -0.1598240000,        0.9909870000,       -0.1089630000,        0.0765260000,        0.0147010000,        0.3608970000,        0.9852450000,       -0.0761030000,        0.0563350000,        0.1425730000,       -0.3495410000,        0.9953800000,        0.0434500000,       -0.0206190000,        0.0831060000,        0.9844050000,        0.0980730000,       -0.1420870000,       -0.0337490000,        0.2497670000],
+[        0.0333320000,        0.8663510000,        0.8693730000,       -0.0179380000,        0.9989030000,        0.0322850000,       -0.0051270000,       -0.0335350000,        0.9989250000,       -0.0357960000,        0.0294280000,        0.0011110000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9789940000,       -0.0788810000,       -0.0172150000,        0.1872250000,       -1.3577300000,        0.9891780000,       -0.0057330000,        0.0707090000,       -0.1284310000,        0.9924060000,       -0.1060630000,        0.0621780000,       -0.0039700000,        0.3157520000,        0.9898410000,       -0.0716480000,        0.0546010000,        0.1100030000,       -0.3161610000,        0.9945200000,        0.0444670000,       -0.0253540000,        0.0911600000,        0.9815660000,        0.1079990000,       -0.1571830000,       -0.0125770000,        0.3013270000],
+[        0.0333320000,        0.8962750000,        0.8722940000,       -0.0172890000,        0.9991290000,        0.0268760000,        0.0023020000,       -0.0318330000,        0.9992850000,       -0.0284200000,        0.0249180000,        0.0004180000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9722720000,       -0.0757490000,       -0.0196330000,        0.2203740000,       -1.3511600000,        0.9940540000,       -0.0327350000,        0.0606840000,       -0.0842780000,        0.9934730000,       -0.1033700000,        0.0410110000,       -0.0253650000,        0.2805770000,        0.9928290000,       -0.0652220000,        0.0553300000,        0.0835140000,       -0.2875030000,        0.9934340000,        0.0442090000,       -0.0353110000,        0.0994410000,        0.9765250000,        0.1222710000,       -0.1770780000,        0.0095710000,        0.3523340000],
+[        0.0333320000,        0.9255310000,        0.8738900000,       -0.0171740000,        0.9991940000,        0.0233840000,        0.0108290000,       -0.0307910000,        0.9995110000,       -0.0230890000,        0.0210780000,       -0.0005190000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9655460000,       -0.0745680000,       -0.0249000000,        0.2480750000,       -1.3006270000,        0.9968650000,       -0.0607040000,        0.0424000000,       -0.0278910000,        0.9935740000,       -0.1020610000,        0.0138080000,       -0.0469420000,        0.2516170000,        0.9947400000,       -0.0606280000,        0.0587570000,        0.0580050000,       -0.2592610000,        0.9921310000,        0.0447990000,       -0.0471680000,        0.1069800000,        0.9700480000,        0.1378520000,       -0.1973670000,        0.0324100000,        0.4029120000],
+[        0.0333320000,        0.9535020000,        0.8741440000,       -0.0164240000,        0.9991300000,        0.0197040000,        0.0202830000,       -0.0306650000,        0.9996800000,       -0.0180100000,        0.0177710000,       -0.0002450000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9598350000,       -0.0731180000,       -0.0338350000,        0.2687470000,       -1.1992650000,        0.9976300000,       -0.0616240000,        0.0216780000,        0.0216220000,        0.9923620000,       -0.1015390000,       -0.0171820000,       -0.0679090000,        0.2264920000,        0.9960580000,       -0.0549300000,        0.0590820000,        0.0368760000,       -0.2401550000,        0.9906490000,        0.0457460000,       -0.0565300000,        0.1154430000,        0.9626530000,        0.1537270000,       -0.2156710000,        0.0561530000,        0.4451960000],
+[        0.0333320000,        0.9809860000,        0.8735920000,       -0.0151980000,        0.9989560000,        0.0165980000,        0.0273870000,       -0.0325760000,        0.9997680000,       -0.0143740000,        0.0160460000,       -0.0007560000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9556040000,       -0.0704640000,       -0.0421420000,        0.2829830000,       -1.0487890000,        0.9973700000,       -0.0658490000,        0.0045910000,        0.0299310000,        0.9901070000,       -0.1009600000,       -0.0439320000,       -0.0869740000,        0.2013980000,        0.9968970000,       -0.0493380000,        0.0577300000,        0.0206980000,       -0.2312330000,        0.9893570000,        0.0444080000,       -0.0616240000,        0.1241060000,        0.9557200000,        0.1679910000,       -0.2277620000,        0.0806430000,        0.4772300000],
+[        0.0333320000,        1.0079520000,        0.8721270000,       -0.0138460000,        0.9986920000,        0.0155010000,        0.0321950000,       -0.0365910000,        0.9998060000,       -0.0122360000,        0.0154300000,       -0.0001980000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9530950000,       -0.0682600000,       -0.0505600000,        0.2905080000,       -0.8624410000,        0.9979920000,       -0.0621880000,       -0.0073170000,        0.0095870000,        0.9873870000,       -0.1008070000,       -0.0654830000,       -0.1030330000,        0.1768320000,        0.9973380000,       -0.0456920000,        0.0563030000,        0.0076880000,       -0.2255760000,        0.9880090000,        0.0457620000,       -0.0644030000,        0.1326500000,        0.9506770000,        0.1786930000,       -0.2314570000,        0.1034830000,        0.5028640000],
+[        0.0333320000,        1.0355020000,        0.8698920000,       -0.0128170000,        0.9982790000,        0.0177930000,        0.0365930000,       -0.0422430000,        0.9998090000,       -0.0128130000,        0.0147600000,        0.0008960000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9519170000,       -0.0678870000,       -0.0651890000,        0.2915410000,       -0.6550040000,        0.9984650000,       -0.0539830000,       -0.0111310000,       -0.0055220000,        0.9844990000,       -0.1012040000,       -0.0846310000,       -0.1155700000,        0.1551490000,        0.9975290000,       -0.0453390000,        0.0534220000,       -0.0052250000,       -0.2157660000,        0.9868960000,        0.0450330000,       -0.0659550000,        0.1402090000,        0.9471080000,        0.1865960000,       -0.2306250000,        0.1223940000,        0.5269460000],
+[        0.0333320000,        1.0646110000,        0.8670590000,       -0.0119150000,        0.9978440000,        0.0228310000,        0.0404430000,       -0.0463760000,        0.9997520000,       -0.0168290000,        0.0145230000,        0.0013260000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9522830000,       -0.0672370000,       -0.0863160000,        0.2849320000,       -0.4426240000,        0.9986390000,       -0.0506980000,        0.0056960000,       -0.0108540000,        0.9808630000,       -0.1016680000,       -0.1058650000,       -0.1279200000,        0.1464150000,        0.9972860000,       -0.0471740000,        0.0525150000,       -0.0209060000,       -0.2060700000,        0.9862080000,        0.0410770000,       -0.0700370000,        0.1442280000,        0.9437600000,        0.1928680000,       -0.2311860000,        0.1366480000,        0.5453590000],
+[        0.0333320000,        1.0961700000,        0.8640800000,       -0.0093330000,        0.9972500000,        0.0259170000,        0.0484160000,       -0.0497730000,        0.9997120000,       -0.0204270000,        0.0125950000,        0.0009340000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9539720000,       -0.0617530000,       -0.1125740000,        0.2710180000,       -0.2369080000,        0.9988540000,       -0.0337440000,        0.0318460000,       -0.0117250000,        0.9767650000,       -0.1021930000,       -0.1266790000,       -0.1394210000,        0.1471610000,        0.9968350000,       -0.0439130000,        0.0556600000,       -0.0359640000,       -0.2058370000,        0.9849700000,        0.0350340000,       -0.0819060000,        0.1479800000,        0.9408130000,        0.1976120000,       -0.2337280000,        0.1455710000,        0.5620740000],
+[        0.0333320000,        1.1296090000,        0.8608720000,       -0.0054350000,        0.9967350000,        0.0269650000,        0.0565160000,       -0.0509760000,        0.9997290000,       -0.0207800000,        0.0103310000,       -0.0016980000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9576770000,       -0.0560540000,       -0.1220800000,        0.2545750000,       -0.0806400000,        0.9988660000,       -0.0226260000,        0.0412300000,       -0.0074390000,        0.9730470000,       -0.1040510000,       -0.1439350000,       -0.1470930000,        0.1518900000,        0.9963180000,       -0.0367660000,        0.0593190000,       -0.0497910000,       -0.2214360000,        0.9835200000,        0.0312430000,       -0.0927800000,        0.1520030000,        0.9390830000,        0.1992430000,       -0.2368990000,        0.1493440000,        0.5758430000],
+[        0.0333320000,        1.1647060000,        0.8576010000,       -0.0010220000,        0.9963850000,        0.0276470000,        0.0627650000,       -0.0501190000,        0.9997670000,       -0.0193860000,        0.0089680000,       -0.0030990000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9596930000,       -0.0545350000,       -0.1175900000,        0.2493760000,       -0.0592230000,        0.9995230000,        0.0059220000,        0.0294090000,        0.0072780000,        0.9708880000,       -0.1070260000,       -0.1505500000,       -0.1524990000,        0.1588850000,        0.9958970000,       -0.0288280000,        0.0587060000,       -0.0625510000,       -0.2506660000,        0.9828260000,        0.0305860000,       -0.0978030000,        0.1534660000,        0.9377420000,        0.1985960000,       -0.2440980000,        0.1470230000,        0.5853580000],
+[        0.0333320000,        1.2002150000,        0.8549060000,        0.0043250000,        0.9964830000,        0.0246090000,        0.0607910000,       -0.0521520000,        0.9998220000,       -0.0160670000,        0.0097780000,       -0.0011460000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9586480000,       -0.0495910000,       -0.1034380000,        0.2604540000,       -0.1549670000,        0.9995790000,       -0.0047500000,        0.0278210000,       -0.0067700000,        0.9702230000,       -0.1116970000,       -0.1506550000,       -0.1532770000,        0.1637470000,        0.9957560000,       -0.0184400000,        0.0587140000,       -0.0684330000,       -0.2949010000,        0.9833820000,        0.0303580000,       -0.0973770000,        0.1501890000,        0.9387990000,        0.1946620000,       -0.2480910000,        0.1386140000,        0.5890180000],
+[        0.0000000000,        1.2385900000,        0.8475320000,       -0.0000000000,        0.9965700000,        0.0147720000,        0.0589580000,       -0.0561480000,        0.9999300000,       -0.0073250000,        0.0089320000,        0.0024790000,        1.0000000000,        0.0000000000,        0.0000000000,        0.0000000000,        0.9606960000,       -0.0389450000,       -0.0711450000,        0.2654910000,       -0.2366380000,        0.9998620000,       -0.0068670000,        0.0079280000,       -0.0128890000,        0.9703810000,       -0.1177290000,       -0.1477780000,       -0.1505410000,        0.1671120000,        0.9959150000,        0.0009110000,        0.0595720000,       -0.0678530000,       -0.3567200000,        0.9845480000,        0.0261520000,       -0.0998040000,        0.1414910000,        0.9415800000,        0.1883880000,       -0.2513170000,        0.1215580000,        0.5897850000]
+]
+}

examples/pybullet/gym/pybullet_envs/ARS/ars.py

@@ -1,18 +1,28 @@
 # AI 2018
 
+import os
+import 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)
+
 # Importing the libraries
 import os
 import numpy as np
 import gym
 from gym import wrappers
 import pybullet_envs
+import time
+import multiprocessing as mp
+from multiprocessing import Process, Pipe
+import argparse
+
 
 # Setting the Hyper Parameters
-
 class Hp():
     
     def __init__(self):
-        self.nb_steps = 1000
+        self.nb_steps = 10000
         self.episode_length = 1000
         self.learning_rate = 0.02
         self.nb_directions = 16
@@ -22,6 +32,58 @@ class Hp():
         self.seed = 1
         self.env_name = 'HalfCheetahBulletEnv-v0'
 
+
+# Multiprocess Exploring the policy on one specific direction and over one episode
+
+_RESET = 1
+_CLOSE = 2
+_EXPLORE = 3
+
+def ExploreWorker(rank,childPipe, envname, args):
+    env = gym.make(envname)
+    nb_inputs = env.observation_space.shape[0]
+    normalizer = Normalizer(nb_inputs)
+    observation_n = env.reset()
+    n=0
+    while True:
+      n+=1
+      try:
+        # Only block for short times to have keyboard exceptions be raised.
+        if not childPipe.poll(0.001):
+          continue
+        message, payload = childPipe.recv()
+      except (EOFError, KeyboardInterrupt):
+        break
+      if message == _RESET:
+        observation_n = env.reset()
+        childPipe.send(["reset ok"])
+        continue
+      if message == _EXPLORE:
+        #normalizer = payload[0] #use our local normalizer
+        policy = payload[1]
+        hp = payload[2]
+        direction = payload[3]
+        delta = payload[4]
+        state = env.reset()
+        done = False
+        num_plays = 0.
+        sum_rewards = 0
+        while not done and num_plays < hp.episode_length:
+            normalizer.observe(state)
+            state = normalizer.normalize(state)
+            action = policy.evaluate(state, delta, direction,hp)
+            state, reward, done, _ = env.step(action)
+            reward = max(min(reward, 1), -1)
+            sum_rewards += reward
+            num_plays += 1
+        childPipe.send([sum_rewards])
+        continue
+      if message == _CLOSE:
+        childPipe.send(["close ok"])
+        break
+    childPipe.close()
+        
+
 # Normalizing the states
 
 class Normalizer():
@@ -47,11 +109,14 @@ class Normalizer():
 # Building the AI
 
 class Policy():
-    
-    def __init__(self, input_size, output_size):
-        self.theta = np.zeros((output_size, input_size))
-        print("self.theta=",self.theta)    
-    def evaluate(self, input, delta = None, direction = None):
+    def __init__(self, input_size, output_size, env_name, args):
+        try:
+          self.theta = np.load(args.policy)
+        except:
+          self.theta = np.zeros((output_size, input_size))
+        self.env_name = env_name
+        print("Starting policy theta=",self.theta)
+    def evaluate(self, input, delta, direction, hp):
         if direction is None:
             return np.clip(self.theta.dot(input), -1.0, 1.0)
         elif direction == "positive":
@@ -62,15 +127,18 @@ class Policy():
     def sample_deltas(self):
         return [np.random.randn(*self.theta.shape) for _ in range(hp.nb_directions)]
     
-    def update(self, rollouts, sigma_r):
+    def update(self, rollouts, sigma_r, args):
         step = np.zeros(self.theta.shape)
         for r_pos, r_neg, d in rollouts:
             step += (r_pos - r_neg) * d
         self.theta += hp.learning_rate / (hp.nb_best_directions * sigma_r) * step
+        timestr = time.strftime("%Y%m%d-%H%M%S")
+        np.save(args.logdir+"/policy_"+self.env_name+"_"+timestr+".npy", self.theta)
+
 
 # Exploring the policy on one specific direction and over one episode
 
-def explore(env, normalizer, policy, direction = None, delta = None):
+def explore(env, normalizer, policy, direction, delta, hp):
     state = env.reset()
     done = False
     num_plays = 0.
@@ -78,7 +146,7 @@ def explore(env, normalizer, policy, direction = None, delta = None):
     while not done and num_plays < hp.episode_length:
         normalizer.observe(state)
         state = normalizer.normalize(state)
-        action = policy.evaluate(state, delta, direction)
+        action = policy.evaluate(state, delta, direction, hp)
         state, reward, done, _ = env.step(action)
         reward = max(min(reward, 1), -1)
         sum_rewards += reward
@@ -87,7 +155,7 @@ def explore(env, normalizer, policy, direction = None, delta = None):
 
 # Training the AI
 
-def train(env, policy, normalizer, hp):
+def train(env, policy, normalizer, hp, parentPipes, args):
     
     for step in range(hp.nb_steps):
         
@@ -96,13 +164,29 @@ def train(env, policy, normalizer, hp):
         positive_rewards = [0] * hp.nb_directions
         negative_rewards = [0] * hp.nb_directions
         
-        # Getting the positive rewards in the positive directions
-        for k in range(hp.nb_directions):
-            positive_rewards[k] = explore(env, normalizer, policy, direction = "positive", delta = deltas[k])
+        if parentPipes:
+          for k in range(hp.nb_directions):
+            parentPipe = parentPipes[k]
+            parentPipe.send([_EXPLORE,[normalizer, policy, hp, "positive", deltas[k]]])
+          for k in range(hp.nb_directions):
+            positive_rewards[k] = parentPipes[k].recv()[0]
+          
+          for k in range(hp.nb_directions):
+            parentPipe = parentPipes[k]
+            parentPipe.send([_EXPLORE,[normalizer, policy, hp, "negative", deltas[k]]])
+          for k in range(hp.nb_directions):
+            negative_rewards[k] = parentPipes[k].recv()[0]
+          
+        else:
+          # Getting the positive rewards in the positive directions
+          for k in range(hp.nb_directions):
+              positive_rewards[k] = explore(env, normalizer, policy, "positive", deltas[k], hp)
         
-        # Getting the negative rewards in the negative/opposite directions
-        for k in range(hp.nb_directions):
-            negative_rewards[k] = explore(env, normalizer, policy, direction = "negative", delta = deltas[k])
+          
+          # Getting the negative rewards in the negative/opposite directions
+          for k in range(hp.nb_directions):
+              negative_rewards[k] = explore(env, normalizer, policy, "negative", deltas[k], hp)
+            
         
         # Gathering all the positive/negative rewards to compute the standard deviation of these rewards
         all_rewards = np.array(positive_rewards + negative_rewards)
@@ -114,10 +198,10 @@ def train(env, policy, normalizer, hp):
         rollouts = [(positive_rewards[k], negative_rewards[k], deltas[k]) for k in order]
         
         # Updating our policy
-        policy.update(rollouts, sigma_r)
+        policy.update(rollouts, sigma_r, args)
         
         # Printing the final reward of the policy after the update
-        reward_evaluation = explore(env, normalizer, policy)
+        reward_evaluation = explore(env, normalizer, policy, None, None, hp)
         print('Step:', step, 'Reward:', reward_evaluation)
 
 # Running the main code
@@ -127,16 +211,67 @@ def mkdir(base, name):
     if not os.path.exists(path):
         os.makedirs(path)
     return path
-work_dir = mkdir('exp', 'brs')
-monitor_dir = mkdir(work_dir, 'monitor')
 
-hp = Hp()
-np.random.seed(hp.seed)
-env = gym.make(hp.env_name)
-# env.render(mode = "human")
-#env = wrappers.Monitor(env, monitor_dir, force = True)
-nb_inputs = env.observation_space.shape[0]
-nb_outputs = env.action_space.shape[0]
-policy = Policy(nb_inputs, nb_outputs)
-normalizer = Normalizer(nb_inputs)
-train(env, policy, normalizer, hp)
+
+
+
+if __name__ == "__main__":
+    mp.freeze_support()
+
+    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument('--env', help='Gym environment name', type=str, default='HalfCheetahBulletEnv-v0')
+    parser.add_argument('--seed', help='RNG seed', type=int, default=1)
+    parser.add_argument('--render', help='OpenGL Visualizer', type=int, default=0)
+    parser.add_argument('--movie',help='rgb_array gym movie',type=int, default=0)
+    parser.add_argument('--steps', help='Number of steps', type=int, default=10000)
+    parser.add_argument('--policy', help='Starting policy file (npy)', type=str, default='')
+    parser.add_argument('--logdir', help='Directory root to log policy files (npy)', type=str, default='.')
+    parser.add_argument('--mp', help='Enable multiprocessing', type=int, default=1)
+        
+    args = parser.parse_args()
+        
+    hp = Hp()
+    hp.env_name = args.env
+    hp.seed = args.seed
+    hp.nb_steps = args.steps
+    print("seed = ", hp.seed)
+    np.random.seed(hp.seed)
+
+    parentPipes = None
+    if args.mp:
+      num_processes = hp.nb_directions
+      processes = []
+      childPipes = []
+      parentPipes = []
+      
+      for pr in range (num_processes):
+        parentPipe, childPipe = Pipe()
+        parentPipes.append(parentPipe)
+        childPipes.append(childPipe)
+      
+      for rank in range(num_processes):
+          p = mp.Process(target=ExploreWorker, args=(rank,childPipes[rank], hp.env_name, args))
+          p.start()
+          processes.append(p)
+        
+    work_dir = mkdir('exp', 'brs')
+    monitor_dir = mkdir(work_dir, 'monitor')
+    env = gym.make(hp.env_name)
+    if args.render:
+      env.render(mode = "human")
+    if args.movie:
+      env = wrappers.Monitor(env, monitor_dir, force = True)
+    nb_inputs = env.observation_space.shape[0]
+    nb_outputs = env.action_space.shape[0]
+    policy = Policy(nb_inputs, nb_outputs,hp.env_name, args)
+    normalizer = Normalizer(nb_inputs)
+    
+    print("start training")
+    train(env, policy, normalizer, hp, parentPipes, args)
+
+    if args.mp:
+      for parentPipe in parentPipes:
+        parentPipe.send([_CLOSE,"pay2"])
+      
+      for p in processes:
+        p.join()

examples/pybullet/gym/pybullet_envs/__init__.py

@@ -8,6 +8,13 @@ def register(id,*args,**kvargs):
 
 # ------------bullet-------------
 
+register(
+        id='HumanoidDeepMimicBulletEnv-v1',
+        entry_point='pybullet_envs.deep_mimic:HumanoidDeepMimicGymEnv',
+        max_episode_steps=1000,
+        reward_threshold=20000.0,
+)
+
 register(
 	id='CartPoleBulletEnv-v1',
 	entry_point='pybullet_envs.bullet:CartPoleBulletEnv',

examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py

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

examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py

@@ -0,0 +1,683 @@
+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
+

examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py

@@ -0,0 +1,277 @@
+"""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)
+    self.observation_space = spaces.Box(observation_low, observation_high)
+
+  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()
+    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'):
+                close = _close
+                render = _render
+                reset = _reset
+                seed = _seed
+                step = _step
+
+
+  @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
+
+

examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py

@@ -0,0 +1,87 @@
+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)
+  

examples/pybullet/gym/pybullet_envs/mimic/humanoid.py

@@ -1,289 +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)
-
-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._motion_data = motion_data
-    self._humanoid = self._pybullet_client.loadURDF(
-      "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25)
-    
-    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)):
-      self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
-      self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,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.SetFrameTime(0)
-    pose = self.InitializePoseFromMotionData()
-    self.ApplyPose(pose, True)
-
-
-  def SetFrameTime(self, t):
-    self._frameTime = t
-    self._frame = int(self._frameTime)
-    self._frameNext = self._frame+1
-    if (self._frameNext >=  self._motion_data.NumFrames()):
-      self._frameNext = self._frame
-    self._frameFraction = self._frameTime - self._frame
-
-  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 not in self._allowed_body_parts):
-        terminates=True
-      
-    return terminates
-        
-        
-      
-    
-    
-  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()
-    #todo: convert action vector into pose
-    #convert from angle-axis to quaternion for spherical joints
-    
-    initializeBase = False
-    self.ApplyPose(pose, initializeBase)
-    
-  def ApplyPose(self, pose, initializeBase):
-    if (initializeBase):
-      self._pybullet_client.changeVisualShape(self._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]]
-      self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,
-        basePos, pose._baseOrn)
-      self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel)
-      print("resetBaseVelocity=",pose._baseLinVel)
-    else:
-      self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1])
-    maxForce=1000
-    kp=0.8
-    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 = self._pybullet_client.POSITION_CONTROL
-    
-    if (initializeBase):
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot)
-      self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot)
-    
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=maxForce)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=maxForce)
-    

examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py

@@ -1,76 +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_backflip.txt"#humanoid3d_spinkick.txt"#humanoid3d_backflip.txt"
-motion.Load(motionPath)
-print("numFrames = ", motion.NumFrames())
-frameTimeId= bc.addUserDebugParameter("frameTime",0,motion.NumFrames()-1.1,0)
-
-y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0])
-bc.loadURDF("plane.urdf",[0,-0.08,0], y2zOrn)
-
-humanoid = Humanoid(bc, motion,[0,0,0])
-
-frameTime = 0
-keyFrameDuration = motion.KeyFrameDuraction()
-print("keyFrameDuration=",keyFrameDuration)
-#for i in range (50):
-# bc.stepSimulation()
-
-stage = 0
-
-
-
-
-
-def Reset(humanoid):
-	global frameTime
-	humanoid.Reset()
-	frameTime = 0#random.randint(0,motion.NumFrames()-2)
-	print("RESET frametime=",frameTime)
-	humanoid.SetFrameTime(frameTime)
-	pose = humanoid.InitializePoseFromMotionData()
-	humanoid.ApplyPose(pose, True)
-
-
-Reset(humanoid)
-bc.stepSimulation()
-
-while (1):
-  #frameTime = bc.readUserDebugParameter(frameTimeId)
-  #print("keyFrameDuration=",keyFrameDuration)
-  dt = (1./240.)/keyFrameDuration
-  #print("dt=",dt)
-  frameTime += dt
-  if (frameTime >= (motion.NumFrames())-1.1):
-    frameTime = motion.NumFrames()-1.1
-  #print("frameTime=", frameTime)
-  humanoid.SetFrameTime(frameTime)
-
-  pose = humanoid.InitializePoseFromMotionData()
-
-  #humanoid.ApplyPose(pose, False)#False, False)
-  if (humanoid.Terminates()):
-  	Reset(humanoid)
-  	
-  bc.stepSimulation()
-  
-  
-  time.sleep(1./240.)
-

examples/pybullet/gym/pybullet_utils/arg_parser.py

@@ -0,0 +1,124 @@
+import re as RE
+
+class ArgParser(object):
+    global_parser = None
+
+    def __init__(self):
+        self._table = dict()
+        return
+
+    def clear(self):
+        self._table.clear()
+        return
+
+    def load_args(self, arg_strs):
+        succ = True
+        vals = []
+        curr_key = ''
+
+        for str in arg_strs:
+            if not (self._is_comment(str)):
+                is_key = self._is_key(str)
+                if (is_key):
+                    if (curr_key != ''):
+                        if (curr_key not in self._table):
+                            self._table[curr_key] = vals
+
+                    vals = []
+                    curr_key = str[2::]
+                else:
+                    vals.append(str)
+
+        if (curr_key != ''):
+            if (curr_key not in self._table):
+                self._table[curr_key] = vals
+
+            vals = []
+
+        return succ
+
+    def load_file(self, filename):
+        succ = False
+        with open(filename, 'r') as file:
+            lines = RE.split(r'[\n\r]+', file.read())
+            file.close()
+
+            arg_strs = []
+            for line in lines:
+                if (len(line) > 0 and not self._is_comment(line)):
+                    arg_strs += line.split()
+
+            succ = self.load_args(arg_strs)
+        return succ
+
+    def has_key(self, key):
+        return key in self._table
+
+    def parse_string(self, key, default=''):
+        str = default
+        if self.has_key(key):
+            str = self._table[key][0]
+        return str
+
+    def parse_strings(self, key, default=[]):
+        arr = default
+        if self.has_key(key):
+            arr = self._table[key]
+        return arr
+
+    def parse_int(self, key, default=0):
+        val = default
+        if self.has_key(key):
+            val = int(self._table[key][0])
+        return val
+
+    def parse_ints(self, key, default=[]):
+        arr = default
+        if self.has_key(key):
+            arr = [int(str) for str in self._table[key]]
+        return arr
+
+    def parse_float(self, key, default=0.0):
+        val = default
+        if self.has_key(key):
+            val = float(self._table[key][0])
+        return val
+
+    def parse_floats(self, key, default=[]):
+        arr = default
+        if self.has_key(key):
+            arr = [float(str) for str in self._table[key]]
+        return arr
+
+    def parse_bool(self, key, default=False):
+        val = default
+        if self.has_key(key):
+            val = self._parse_bool(self._table[key][0])
+        return val
+
+    def parse_bools(self, key, default=[]):
+        arr = default
+        if self.has_key(key):
+            arr = [self._parse_bool(str) for str in self._table[key]]
+        return arr
+
+    def _is_comment(self, str):
+        is_comment = False
+        if (len(str) > 0):
+            is_comment = str[0] == '#'
+
+        return is_comment
+        
+    def _is_key(self, str):
+        is_key = False
+        if (len(str) >= 3):
+            is_key = str[0] == '-' and str[1] == '-'
+
+        return is_key
+
+    def _parse_bool(self, str):
+        val = False
+        if (str == 'true' or str == 'True' or str == '1' 
+            or str == 'T' or str == 't'):
+            val = True
+        return val

examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py

@@ -18,4 +18,11 @@ for o in objs:
 	humanoid = objs[o]
 	ed0 = ed.UrdfEditor()
 	ed0.initializeFromBulletBody(humanoid, p._client)
-	ed0.saveUrdf(p.getBodyInfo(0)[1]+"_"+p.getBodyInfo(o)[0]+".urdf")
+	robotName = str(p.getBodyInfo(o)[1],'utf-8')
+	partName = str(p.getBodyInfo(o)[0], 'utf-8')
+	
+	print("robotName=",robotName)
+	print("partName=",partName)
+	
+	saveVisuals=False
+	ed0.saveUrdf(robotName+"_"+partName+".urdf", saveVisuals)

examples/pybullet/gym/pybullet_utils/examples/testargs.py

@@ -0,0 +1,26 @@
+import pybullet_data
+from pybullet_utils.arg_parser import	ArgParser
+from pybullet_utils.logger import	Logger
+import sys
+
+def	build_arg_parser(args):
+		arg_parser = ArgParser()
+		arg_parser.load_args(args)
+
+		arg_file = arg_parser.parse_string('arg_file', '')
+		if (arg_file !=	''):
+				path = pybullet_data.getDataPath()+"/args/"+arg_file
+				succ = arg_parser.load_file(path)
+				Logger.print2(arg_file)
+				assert succ, Logger.print2('Failed to	load args	from:	'	+	arg_file)
+
+		return arg_parser
+
+args = sys.argv[1:]
+arg_parser = build_arg_parser(args)
+motion_file	=	arg_parser.parse_string("motion_file")
+print("motion_file=",motion_file)
+bodies = arg_parser.parse_ints("fall_contact_bodies")
+print("bodies=",bodies)
+int_output_path = arg_parser.parse_string("int_output_path")
+print("int_output_path=",int_output_path)

examples/pybullet/gym/pybullet_utils/examples/testlog.py

@@ -0,0 +1,9 @@
+from pybullet_utils.logger import Logger
+logger = Logger()
+logger.configure_output_file("e:/mylog.txt")
+for i in range (10):
+	logger.log_tabular("Iteration", 1)
+Logger.print2("hello world")
+
+logger.print_tabular()
+logger.dump_tabular()

examples/pybullet/gym/pybullet_utils/logger.py

@@ -0,0 +1,128 @@
+import pybullet_utils.mpi_util as MPIUtil
+
+"""
+
+Some simple logging functionality, inspired by rllab's logging.
+Assumes that each diagnostic gets logged each iteration
+
+Call logz.configure_output_file() to start logging to a 
+tab-separated-values file (some_file_name.txt)
+
+To load the learning curves, you can do, for example
+
+A = np.genfromtxt('/tmp/expt_1468984536/log.txt',delimiter='\t',dtype=None, names=True)
+A['EpRewMean']
+
+"""
+
+import os.path as osp, shutil, time, atexit, os, subprocess
+
+class Logger:
+    def print2(str):
+        if (MPIUtil.is_root_proc()):
+            print(str)
+        return
+
+    def __init__(self):
+        self.output_file = None
+        self.first_row = True
+        self.log_headers = []
+        self.log_current_row = {}
+        self._dump_str_template = ""
+        return
+
+    def reset(self):
+        self.first_row = True
+        self.log_headers = []
+        self.log_current_row = {}
+        if self.output_file is not None:
+            self.output_file = open(output_path, 'w')
+        return
+
+    def configure_output_file(self, filename=None):
+        """
+        Set output directory to d, or to /tmp/somerandomnumber if d is None
+        """
+        self.first_row = True
+        self.log_headers = []
+        self.log_current_row = {}
+
+        output_path = filename or "output/log_%i.txt"%int(time.time())
+
+        out_dir = os.path.dirname(output_path)
+        if not os.path.exists(out_dir) and MPIUtil.is_root_proc():
+            os.makedirs(out_dir)
+
+        if (MPIUtil.is_root_proc()):
+            self.output_file = open(output_path, 'w')
+            assert osp.exists(output_path)
+            atexit.register(self.output_file.close)
+
+            Logger.print2("Logging data to " + self.output_file.name)
+        return
+
+    def log_tabular(self, key, val):
+        """
+        Log a value of some diagnostic
+        Call this once for each diagnostic quantity, each iteration
+        """
+        if self.first_row and key not in self.log_headers:
+            self.log_headers.append(key)
+        else:
+            assert key in self.log_headers, "Trying to introduce a new key %s that you didn't include in the first iteration"%key
+        self.log_current_row[key] = val
+        return
+
+    def get_num_keys(self):
+        return len(self.log_headers)
+
+    def print_tabular(self):
+        """
+        Print all of the diagnostics from the current iteration
+        """
+        if (MPIUtil.is_root_proc()):
+            vals = []
+            Logger.print2("-"*37)
+            for key in self.log_headers:
+                val = self.log_current_row.get(key, "")
+                if isinstance(val, float):
+                    valstr = "%8.3g"%val
+                elif isinstance(val, int):
+                    valstr = str(val)
+                else: 
+                    valstr = val
+                Logger.print2("| %15s | %15s |"%(key, valstr))
+                vals.append(val)
+            Logger.print2("-" * 37)
+        return
+
+    def dump_tabular(self):
+        """
+        Write all of the diagnostics from the current iteration
+        """
+        if (MPIUtil.is_root_proc()):
+            if (self.first_row):
+                self._dump_str_template = self._build_str_template()
+
+            vals = []
+            for key in self.log_headers:
+                val = self.log_current_row.get(key, "")
+                vals.append(val)
+            
+            if self.output_file is not None:
+                if self.first_row:
+                    header_str = self._dump_str_template.format(*self.log_headers)
+                    self.output_file.write(header_str + "\n")
+
+                val_str = self._dump_str_template.format(*map(str,vals))
+                self.output_file.write(val_str + "\n")
+                self.output_file.flush()
+
+        self.log_current_row.clear()
+        self.first_row=False
+        return
+
+    def _build_str_template(self):
+        num_keys = self.get_num_keys()
+        template = "{:<25}" * num_keys
+        return template

examples/pybullet/gym/pybullet_utils/math_util.py

@@ -0,0 +1,18 @@
+import numpy as np
+
+RAD_TO_DEG = 57.2957795
+DEG_TO_RAD = 1.0 / RAD_TO_DEG
+INVALID_IDX = -1
+
+def lerp(x, y, t):
+    return (1 - t) * x + t * y
+
+def log_lerp(x, y, t):
+    return np.exp(lerp(np.log(x), np.log(y), t))
+
+def flatten(arr_list):
+    return np.concatenate([np.reshape(a, [-1]) for a in arr_list], axis=0)
+
+def flip_coin(p):
+    rand_num = np.random.binomial(1, p, 1)
+    return rand_num[0] == 1

examples/pybullet/gym/pybullet_utils/mpi_util.py

@@ -0,0 +1,52 @@
+import numpy as np
+from mpi4py import MPI
+
+ROOT_PROC_RANK = 0
+
+def get_num_procs():
+    return MPI.COMM_WORLD.Get_size()
+
+def get_proc_rank():
+    return MPI.COMM_WORLD.Get_rank()
+
+def is_root_proc():
+    rank = get_proc_rank()
+    return rank == ROOT_PROC_RANK
+
+def bcast(x):
+    MPI.COMM_WORLD.Bcast(x, root=ROOT_PROC_RANK)
+    return
+
+def reduce_sum(x):
+    return reduce_all(x, MPI.SUM)
+
+def reduce_prod(x):
+    return reduce_all(x, MPI.PROD)
+
+def reduce_avg(x):
+    buffer = reduce_sum(x)
+    buffer /= get_num_procs()
+    return buffer
+
+def reduce_min(x):
+    return reduce_all(x, MPI.MIN)
+
+def reduce_max(x):
+    return reduce_all(x, MPI.MAX)
+
+def reduce_all(x, op):
+    is_array = isinstance(x, np.ndarray)
+    x_buf = x if is_array else np.array([x])
+    buffer = np.zeros_like(x_buf)
+    MPI.COMM_WORLD.Allreduce(x_buf, buffer, op=op)
+    buffer = buffer if is_array else buffer[0]
+    return buffer
+
+def gather_all(x):
+    is_array = isinstance(x, np.ndarray)
+    x_buf = np.array([x])
+    buffer = np.zeros_like(x_buf)
+    buffer = np.repeat(buffer, get_num_procs(), axis=0)
+    MPI.COMM_WORLD.Allgather(x_buf, buffer)
+    buffer = list(buffer)
+    return buffer

examples/pybullet/gym/pybullet_utils/urdfEditor.py

@@ -273,7 +273,7 @@ class UrdfEditor(object):
 		file.write("\t\t</collision>\n")
 
 
-	def writeLink(self, file, urdfLink):
+	def writeLink(self, file, urdfLink,saveVisuals):
 		file.write("\t<link name=\"")
 		file.write(urdfLink.link_name)
 		file.write("\">\n")
@@ -283,9 +283,9 @@ class UrdfEditor(object):
 		for v in urdfLink.urdf_visual_shapes:
 			if (v.geom_type == p.GEOM_CAPSULE):
 				hasCapsules = True
-		if (not hasCapsules):
-			for v in urdfLink.urdf_visual_shapes:
-				self.writeVisualShape(file,v)
+		if (saveVisuals and not hasCapsules):
+				for v in urdfLink.urdf_visual_shapes:
+					self.writeVisualShape(file,v)
 		for c in urdfLink.urdf_collision_shapes:
 			self.writeCollisionShape(file,c)
 		file.write("\t</link>\n")
@@ -329,7 +329,7 @@ class UrdfEditor(object):
 		file.write(str)
 		file.write("\t</joint>\n")
 
-	def saveUrdf(self, fileName):
+	def saveUrdf(self, fileName, saveVisuals=True):
 		file = open(fileName,"w")
 		file.write("<?xml version=\"0.0\" ?>\n")
 		file.write("<robot name=\"")
@@ -337,7 +337,7 @@ class UrdfEditor(object):
 		file.write("\">\n")
 
 		for link in self.urdfLinks:
-			self.writeLink(file,link)
+			self.writeLink(file,link, saveVisuals)
 
 		for joint in self.urdfJoints:
 			self.writeJoint(file,joint)

examples/pybullet/gym/pybullet_utils/util.py

@@ -0,0 +1,13 @@
+import random
+import numpy as np
+
+def set_global_seeds(seed):
+    try:
+        import tensorflow as tf
+    except ImportError:
+        pass
+    else:
+        tf.set_random_seed(seed)
+    np.random.seed(seed)
+    random.seed(seed)
+    return

examples/pybullet/pybullet.c

@@ -8808,6 +8808,53 @@ static PyObject* pybullet_invertTransform(PyObject* self,
 	return NULL;
 }
 
+static PyObject* pybullet_rotateVector(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	PyObject* quatObj;
+	PyObject* vectorObj;
+	double quat[4];
+	double vec[3];
+	int physicsClientId = 0;
+	int hasQuat = 0;
+	int hasVec = 0;
+
+	static char* kwlist[] = { "quaternion", "vector", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatObj, &vectorObj, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	if (quatObj)
+	{
+		hasQuat = pybullet_internalSetVector4d(quatObj, quat);
+	}
+
+	if (vectorObj)
+	{
+		hasVec = pybullet_internalSetVectord(vectorObj, vec);
+	}
+	if (hasQuat && hasVec)
+	{
+		double vecOut[3];
+		b3RotateVector(quat, vec, vecOut);
+		{
+			PyObject* pylist;
+			int i;
+			pylist = PyTuple_New(3);
+			for (i = 0; i < 3; i++)
+				PyTuple_SetItem(pylist, i, PyFloat_FromDouble(vecOut[i]));
+			return pylist;
+		}
+	}
+	else
+	{
+		PyErr_SetString(SpamError, "Require quaternion with 4 components [x,y,z,w] and a vector [x,y,z].");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
 
 static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -10315,6 +10362,11 @@ static PyMethodDef SpamMethods[] = {
 	 { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS,
 		 "Compute the angular velocity given start and end quaternion and delta time." },
 
+	{ "rotateVector", (PyCFunction)pybullet_rotateVector, METH_VARARGS | METH_KEYWORDS,
+		"Rotate a vector using a quaternion." },
+
+		
+
 	{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS,
 	 "Given an object id, joint positions, joint velocities and joint "
 	 "accelerations, compute the joint forces using Inverse Dynamics"},

src/Bullet3Common/b3Quaternion.h

@@ -96,9 +96,16 @@ public:
 	{
 		b3Scalar d = axis.length();
 		b3Assert(d != b3Scalar(0.0));
-		b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
-		setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
-				 b3Cos(_angle * b3Scalar(0.5)));
+		if (d < B3_EPSILON)
+		{
+			setValue(0, 0, 0, 1);
+		}
+		else
+		{
+			b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
+			setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
+				b3Cos(_angle * b3Scalar(0.5)));
+		}
 	}
 	/**@brief Set the quaternion using Euler angles
    * @param yaw Angle around Y

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1110,7 +1110,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 			///
 			///The user can manually override the friction directions for certain contacts using a contact callback,
-			///and set the cp.m_lateralFrictionInitialized to true
+			///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///