add deepmimic args file (for testing)

implement deepmimic reward function (only joint angles/velocities for now)
fix an out-of-date comment, related to contactPoint.m_lateralFrictionInitialized in btSequentialImpulseConstraintSolver.cpp

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_envs/mimic/humanoid.py

@@ -4,6 +4,9 @@ currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentfram
 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"]
               
@@ -166,14 +169,24 @@ class Humanoid(object):
     """
     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
     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)
+      
     #self._humanoidDebug = self._pybullet_client.loadURDF(
     #  "humanoid/humanoid.urdf", [0,0.9,3],globalScaling=0.25, useFixedBase=True)
       
-    print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
+    #print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
     pose = HumanoidPose()
     
     for i in range (self._motion_data.NumFrames()-1):
@@ -186,8 +199,8 @@ class Humanoid(object):
       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])
+      #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]
@@ -195,9 +208,9 @@ class Humanoid(object):
     
   def Reset(self):
     self._pybullet_client.restoreState(self._initial_state)
-    self.SetSimTime(100)
+    self.SetSimTime(0)
     pose = self.InitializePoseFromMotionData()
-    self.ApplyPose(pose, True, True)
+    self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
 
   def CalcCycleCount(self, simTime, cycleTime):
     phases = simTime / cycleTime;
@@ -208,7 +221,7 @@ class Humanoid(object):
 
   def SetSimTime(self, t):
     self._simTime = t
-    print("SetTimeTime time =",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())
@@ -253,7 +266,7 @@ class Humanoid(object):
     rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
     heading = math.atan2(-rotVec[2], rotVec[0])
     heading2=eul[1]
-    print("heading=",heading)
+    #print("heading=",heading)
     headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading)
     return headingOrn
 
@@ -269,21 +282,21 @@ class Humanoid(object):
   def BuildOriginTrans(self):
     rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
     
-    print("rootPos=",rootPos, " rootOrn=",rootOrn)
+    #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)
+    #print("headingOrn=",headingOrn)
     headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
-    print("headingMat=",headingMat)
+    #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)
+    #print("invOrigTransPos=",invOrigTransPos)
+    #print("invOrigTransOrn=",invOrigTransOrn)
     invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
-    print("invOrigTransMat =",invOrigTransMat )
+    #print("invOrigTransMat =",invOrigTransMat )
     return invOrigTransPos, invOrigTransOrn
     
   def InitializePoseFromMotionData(self):
@@ -360,28 +373,30 @@ class Humanoid(object):
     pose._leftElbowRot = [angle]
     
     
-    print("index=",index)
+    #print("index=",index)
     
     initializeBase = False
     initializeVelocities = False
-    self.ApplyPose(pose, initializeBase, initializeVelocities)
+    self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, self._pybullet_client)
     
     
-  def ApplyPose(self, pose, initializeBase, initializeVelocities):
+  def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid,bc):
     #todo: get tunable parametes from a json file or from URDF (kd, maxForce)
     if (initializeBase):
-      self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1])
+      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]]
       
-      self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,
+      bc.resetBasePositionAndOrientation(humanoid,
         basePos, pose._baseOrn)
       if initializeVelocities:
-        self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel)
+        bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel)
         #print("resetBaseVelocity=",pose._baseLinVel)
     else:
-      self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1])
+      bc.changeVisualShape(humanoid, 2 , rgbaColor=[1,1,1,1])
     
-    kp=0.01
+    
+    
+    kp=0.03
     chest=1
     neck=2
     rightShoulder=3
@@ -394,63 +409,63 @@ class Humanoid(object):
     leftHip = 12
     leftKnee=13
     leftAnkle=14
-    controlMode = self._pybullet_client.POSITION_CONTROL
+    controlMode = bc.POSITION_CONTROL
     
     if (initializeBase):
       if initializeVelocities:
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot, pose._chestVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot, pose._neckVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot, pose._rightHipVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot, pose._rightKneeVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot, pose._rightElbowVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot, pose._leftHipVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot, pose._leftKneeVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel)
-        self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot, pose._leftElbowVel)
+        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:
-        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)
+        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)
     
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=200)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=50)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=200)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=150)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=90)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=100)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=60)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=200)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=150)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=90)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=100)
-    self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=60)
+    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 (self._pybullet_client.getNumJoints(self._humanoid)):
-    #    js = self._pybullet_client.getJointState(self._humanoid, j)
-    #    self._pybullet_client.resetJointState(self._humanoidDebug, j,js[0])
-    #    jsm = self._pybullet_client.getJointStateMultiDof(self._humanoid, j)
+    #  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):
-    #      self._pybullet_client.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
+    #      bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
         
   def GetState(self):
 
     stateVector = []
     phase = self.GetPhase()
-    print("phase=",phase)
+    #print("phase=",phase)
     stateVector.append(phase)
     
     rootTransPos, rootTransOrn=self.BuildOriginTrans()
@@ -459,16 +474,19 @@ class Humanoid(object):
     rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1])
     #print("!!!rootPosRel =",rootPosRel )
     #print("rootTransPos=",rootTransPos)
-    print("basePos=",basePos)
+    #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)
+    #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 j in range (self._pybullet_client.getNumJoints(self._humanoid)):
+    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]
@@ -478,11 +496,16 @@ class Humanoid(object):
       linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]]
       for l in linkPosLocal:
         stateVector.append(l)
-      for l in linkOrnLocal:
-        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 j in range (self._pybullet_client.getNumJoints(self._humanoid)):
+    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]
@@ -491,7 +514,167 @@ class Humanoid(object):
       for l in linkAngVel:
         stateVector.append(l)
     
-    print("stateVector len=",len(stateVector))  
+    #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

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

@@ -19,19 +19,19 @@ motion=MotionCaptureData()
 
 motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
 motion.Load(motionPath)
-print("numFrames = ", motion.NumFrames())
+#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])
+humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000])
 
 simTime = 0
 
 
 keyFrameDuration = motion.KeyFrameDuraction()
-print("keyFrameDuration=",keyFrameDuration)
+#print("keyFrameDuration=",keyFrameDuration)
 #for i in range (50):
 # bc.stepSimulation()
 
@@ -44,25 +44,25 @@ stage = 0
 def Reset(humanoid):
 	global simTime
 	humanoid.Reset()
-	simTime = random.randint(0,motion.NumFrames()-2)
+	simTime = 0 #random.randint(0,motion.NumFrames()-2)
 	humanoid.SetSimTime(simTime)
 	pose = humanoid.InitializePoseFromMotionData()
-	humanoid.ApplyPose(pose, True, True)
+	humanoid.ApplyPose(pose, True, True, humanoid._humanoid,bc)
 
 
 Reset(humanoid)
-bc.stepSimulation()
+#bc.stepSimulation()
 
 
 while (1):
   #simTime = bc.readUserDebugParameter(frameTimeId)
   #print("keyFrameDuration=",keyFrameDuration)
   dt = (1./240.)
-  print("------------------------------------------")
-  print("dt=",dt)
+  #print("------------------------------------------")
+  #print("dt=",dt)
   
-  print("simTime=",simTime)
-  print("humanoid.SetSimTime(simTime)")
+  #print("simTime=",simTime)
+  #print("humanoid.SetSimTime(simTime)")
   humanoid.SetSimTime(simTime)
 
   #pose = humanoid.InitializePoseFromMotionData()
@@ -75,8 +75,10 @@ while (1):
   action = [0]*36
   humanoid.ApplyAction(action)
   for s in range (8):
-    print("step:",s)
+    #print("step:",s)
     bc.stepSimulation()
     simTime += dt   
     time.sleep(1./240.)
+  reward = humanoid.GetReward()
+  print("reward=",reward)
   

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)

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
 			///