diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 40eff6ae8..f8ee5b4aa 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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]); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 14ce0f706..cb7c40f16 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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*/]); diff --git a/examples/SharedMemory/Win32SharedMemory.cpp b/examples/SharedMemory/Win32SharedMemory.cpp index 5e973ae5e..53fdad102 100644 --- a/examples/SharedMemory/Win32SharedMemory.cpp +++ b/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; } } diff --git a/examples/pybullet/examples/shiftCenterOfMass.py b/examples/pybullet/examples/shiftCenterOfMass.py new file mode 100644 index 000000000..c77629b32 --- /dev/null +++ b/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.) + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/kin_char_args.txt b/examples/pybullet/gym/pybullet_data/args/kin_char_args.txt new file mode 100644 index 000000000..aad477912 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_backflip_args.txt new file mode 100644 index 000000000..ebf17ae56 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_cartwheel_args.txt new file mode 100644 index 000000000..65a43595c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_crawl_args.txt new file mode 100644 index 000000000..ac0f6e3aa --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_a_args.txt new file mode 100644 index 000000000..3dbce45e3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_dance_b_args.txt new file mode 100644 index 000000000..acbe85d7f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_facedown_args.txt new file mode 100644 index 000000000..1326c2734 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_getup_faceup_args.txt new file mode 100644 index 000000000..d36661ac3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_jump_args.txt new file mode 100644 index 000000000..2b1eb42c5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_kick_args.txt new file mode 100644 index 000000000..2d946ceeb --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_punch_args.txt new file mode 100644 index 000000000..31c1bef66 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_roll_args.txt new file mode 100644 index 000000000..12ecf0114 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_run_args.txt new file mode 100644 index 000000000..84019d705 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spin_args.txt new file mode 100644 index 000000000..ce8d569b5 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_spinkick_args.txt new file mode 100644 index 000000000..95fcd02f7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt b/examples/pybullet/gym/pybullet_data/args/run_humanoid3d_walk_args.txt new file mode 100644 index 000000000..dab89ba2b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_backflip_args.txt new file mode 100644 index 000000000..3e069f29d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_cartwheel_args.txt new file mode 100644 index 000000000..21850cb2a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_crawl_args.txt new file mode 100644 index 000000000..c64f45adf --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_a_args.txt new file mode 100644 index 000000000..784ed728a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_dance_b_args.txt new file mode 100644 index 000000000..afef9a240 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_facedown_args.txt new file mode 100644 index 000000000..b1af6b78e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_getup_faceup_args.txt new file mode 100644 index 000000000..1d36937ca --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_jump_args.txt new file mode 100644 index 000000000..23cc690c9 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_kick_args.txt new file mode 100644 index 000000000..94359b039 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_punch_args.txt new file mode 100644 index 000000000..f6e14bf8b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_roll_args.txt new file mode 100644 index 000000000..1184afe11 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_run_args.txt new file mode 100644 index 000000000..2971efa6b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spin_args.txt new file mode 100644 index 000000000..7795cc7cf --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_spinkick_args.txt new file mode 100644 index 000000000..c38bb62e0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt b/examples/pybullet/gym/pybullet_data/args/train_humanoid3d_walk_args.txt new file mode 100644 index 000000000..54f544971 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf index 9a3d4938e..334b5eda1 100644 --- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf +++ b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf @@ -1,12 +1,12 @@ - + - + @@ -19,12 +19,12 @@ - - - + + + - + @@ -37,12 +37,12 @@ - - - + + + - + @@ -55,12 +55,12 @@ - - - + + + - + @@ -73,12 +73,12 @@ - - - + + + - + @@ -91,13 +91,14 @@ - - - - + + + + + - + @@ -110,12 +111,12 @@ - - - + + + - + @@ -128,12 +129,12 @@ - - - + + + - + @@ -146,13 +147,14 @@ - - - + + + + - + @@ -165,12 +167,12 @@ - - - + + + - + @@ -183,12 +185,12 @@ - - - + + + - + @@ -201,13 +203,14 @@ - - - + + + + - + @@ -220,12 +223,12 @@ - - - + + + - + @@ -238,12 +241,12 @@ - - - + + + - + @@ -256,13 +259,14 @@ - - - + + + + - + @@ -275,9 +279,9 @@ - - - + + + diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt b/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt new file mode 100644 index 000000000..740688a16 --- /dev/null +++ b/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] +] +} \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/ARS/ars.py b/examples/pybullet/gym/pybullet_envs/ARS/ars.py index b4c0c3127..760ebd732 100644 --- a/examples/pybullet/gym/pybullet_envs/ARS/ars.py +++ b/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() diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index d2a5a95b4..974001b79 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/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', diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py new file mode 100644 index 000000000..f2b2400ca --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py @@ -0,0 +1 @@ +from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid.py new file mode 100644 index 000000000..b2ebf0994 --- /dev/null +++ b/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 + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_deepmimic_gym_env.py new file mode 100644 index 000000000..9e1a674f2 --- /dev/null +++ b/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 + + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/humanoid_test.py new file mode 100644 index 000000000..c4eacbc1c --- /dev/null +++ b/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) + diff --git a/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py similarity index 100% rename from examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py rename to examples/pybullet/gym/pybullet_envs/deep_mimic/motion_capture_data.py diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py deleted file mode 100644 index 4a5dd01d9..000000000 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py +++ /dev/null @@ -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) - \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py deleted file mode 100644 index 975b2004b..000000000 --- a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py +++ /dev/null @@ -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.) - diff --git a/examples/pybullet/gym/pybullet_utils/arg_parser.py b/examples/pybullet/gym/pybullet_utils/arg_parser.py new file mode 100644 index 000000000..5461a5bf2 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py b/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py index c1e39745d..f7903262f 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/mjcf2urdf.py +++ b/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) diff --git a/examples/pybullet/gym/pybullet_utils/examples/testargs.py b/examples/pybullet/gym/pybullet_utils/examples/testargs.py new file mode 100644 index 000000000..d03a82041 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/examples/testlog.py b/examples/pybullet/gym/pybullet_utils/examples/testlog.py new file mode 100644 index 000000000..9a275160a --- /dev/null +++ b/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() \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/logger.py b/examples/pybullet/gym/pybullet_utils/logger.py new file mode 100644 index 000000000..ceb2605ba --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_utils/math_util.py b/examples/pybullet/gym/pybullet_utils/math_util.py new file mode 100644 index 000000000..d0f333b3f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/mpi_util.py b/examples/pybullet/gym/pybullet_utils/mpi_util.py new file mode 100644 index 000000000..30ca6aed0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 6b05ce72c..3edca4596 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -273,7 +273,7 @@ class UrdfEditor(object): file.write("\t\t\n") - def writeLink(self, file, urdfLink): + def writeLink(self, file, urdfLink,saveVisuals): file.write("\t\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\n") @@ -329,7 +329,7 @@ class UrdfEditor(object): file.write(str) file.write("\t\n") - def saveUrdf(self, fileName): + def saveUrdf(self, fileName, saveVisuals=True): file = open(fileName,"w") file.write("\n") 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) diff --git a/examples/pybullet/gym/pybullet_utils/util.py b/examples/pybullet/gym/pybullet_utils/util.py new file mode 100644 index 000000000..c11ccb811 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d25f896c4..1086614c3 100644 --- a/examples/pybullet/pybullet.c +++ b/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"}, diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h index 98e07c98d..9bd5ff7d9 100644 --- a/src/Bullet3Common/b3Quaternion.h +++ b/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 diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e8a766ecc..89c163e45 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/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 ///