add table tennis environment with new mujoco binding
This commit is contained in:
Hongyi Zhou
parent
fd4f9ae0bc
commit
a6cca617e1
1
fancy_gym/envs/mujoco/table_tennis/__init__.py
Normal file
1
fancy_gym/envs/mujoco/table_tennis/__init__.py
Normal file
@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/base_link_convex.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/base_link_convex.stl
Executable file
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/base_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/base_link_fine.stl
Executable file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/cup.stl
Normal file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/cup.stl
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/elbow_link_convex.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/elbow_link_convex.stl
Executable file
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/elbow_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/elbow_link_fine.stl
Executable file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/forearm_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/forearm_link_fine.stl
Executable file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/shoulder_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/shoulder_link_fine.stl
Executable file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/upper_arm_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/upper_arm_link_fine.stl
Executable file
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/wrist_palm_link_convex.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/wrist_palm_link_convex.stl
Executable file
Binary file not shown.
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/wrist_palm_link_fine.stl
Executable file
BIN
fancy_gym/envs/mujoco/table_tennis/assets/meshes/wam/wrist_palm_link_fine.stl
Executable file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,36 @@
|
||||
<mujocoinclude>
|
||||
<actuator>
|
||||
<!-- <motor name="wam/base_slide_motor" joint="wam/base_slide_joint" ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0"/>-->
|
||||
<motor name="wam/base_motor" joint="wam/base_yaw_joint_right" ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0"/>
|
||||
<motor name="wam/shoulder_pitch_motor" joint='wam/shoulder_pitch_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="125.0"/>
|
||||
<motor name="wam/shoulder_yaw_motor" joint='wam/shoulder_yaw_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="40.0"/>
|
||||
<motor name="wam/elbow_pitch_motor" joint='wam/elbow_pitch_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0"/>
|
||||
<motor name="wam/wrist_yaw_motor" joint='wam/wrist_yaw_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0"/>
|
||||
<motor name="wam/wrist_pitch_motor" joint='wam/wrist_pitch_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0"/>
|
||||
<motor name="wam/palm_yaw_motor" joint='wam/palm_yaw_joint_right' ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0"/>
|
||||
</actuator>
|
||||
</mujocoinclude>
|
||||
|
||||
<!--<mujocoinclude>-->
|
||||
<!--<actuator>-->
|
||||
<!-- <motor name="wam/base_motor" joint="wam/base_yaw_joint_right"/>-->
|
||||
<!-- <motor name="wam/shoulder_pitch_motor" joint='wam/shoulder_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/shoulder_yaw_motor" joint='wam/shoulder_yaw_joint_right'/>-->
|
||||
<!-- <motor name="wam/elbow_pitch_motor" joint='wam/elbow_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/wrist_yaw_motor" joint='wam/wrist_yaw_joint_right'/>-->
|
||||
<!-- <motor name="wam/wrist_pitch_motor" joint='wam/wrist_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/palm_yaw_motor" joint='wam/palm_yaw_joint_right'/>-->
|
||||
<!--</actuator>-->
|
||||
<!--</mujocoinclude>-->
|
||||
|
||||
<!--<mujocoinclude>-->
|
||||
<!-- <actuator boastype="none">-->
|
||||
<!-- <motor name="wam/shoulder_yaw_link_right_motor" joint="wam/base_yaw_joint_right"/>-->
|
||||
<!-- <motor name="wam/shoulder_pitch_joint_right_motor" joint='wam/shoulder_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/shoulder_yaw_joint_right_motor" joint='wam/shoulder_yaw_joint_right'/>-->
|
||||
<!-- <motor name="wam/elbow_pitch_joint_right_motor" joint='wam/elbow_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/wrist_yaw_joint_right_motor" joint='wam/wrist_yaw_joint_right'/>-->
|
||||
<!-- <motor name="wam/wrist_pitch_joint_right_motor" joint='wam/wrist_pitch_joint_right'/>-->
|
||||
<!-- <motor name="wam/palm_yaw_joint_right_motor" joint='wam/palm_yaw_joint_right'/>-->
|
||||
<!-- </actuator>-->
|
||||
<!--</mujocoinclude>-->
|
||||
@ -0,0 +1,104 @@
|
||||
<mujocoinclue>
|
||||
<body name="wam/base_link_right" pos="2.1 0 2.0" quat="0 0 1 0" childclass="wam" >
|
||||
<!-- <joint name="wam/base_slide_joint" axis="0 1 0" type="slide" damping="1.98" range="-0.8 0.8"/>-->
|
||||
<inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1"/>
|
||||
<geom name="base_link_fine" class="viz" mesh="base_link_fine" rgba="0.5 0.5 0.5 0"/>
|
||||
<geom name="base_link_convex" class="col" mesh="base_link_convex" rgba="0.5 0.5 0.5 1"/>
|
||||
<body name="wam/shoulder_yaw_link_right" pos="0 0 0.346">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5"
|
||||
diaginertia="0.135089 0.113095 0.0904426"/>
|
||||
<!-- control 0: 1.6-->
|
||||
<joint name="wam/base_yaw_joint_right" range="-2.6 2.6" damping="1.98"/>
|
||||
<geom name="shoulder_link_fine" class="viz" mesh="shoulder_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p1" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p1"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p2" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p2"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p3" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p3"/>
|
||||
<body name="wam/shoulder_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592"
|
||||
mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452"/> <!--seems off-->
|
||||
<!-- control 1: 0-->
|
||||
<joint name="wam/shoulder_pitch_joint_right" range="-2 2" damping="0.55"/>
|
||||
<geom name="shoulder_pitch_link_fine" class="viz" mesh="shoulder_pitch_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="shoulder_pitch_link_convex" class="col" mesh="shoulder_pitch_link_convex"/>
|
||||
<body name="wam/upper_arm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049"
|
||||
mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419"/>
|
||||
<!-- control 2: 0-->
|
||||
<joint name="wam/shoulder_yaw_joint_right" range="-2.8 2.8" damping="1.65"/>
|
||||
<geom name="upper_arm_link_fine" class="viz" mesh="upper_arm_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="upper_arm_link_convex_decomposition_p1" class="col"
|
||||
mesh="upper_arm_link_convex_decomposition_p1" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom name="upper_arm_link_convex_decomposition_p2" class="col"
|
||||
mesh="upper_arm_link_convex_decomposition_p2" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/forearm_link_right" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.0400149 -0.142717 -0.00022942"
|
||||
quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168"
|
||||
diaginertia="0.0151047 0.0148285 0.00275805"/>
|
||||
<!-- control 3: 2.4-->
|
||||
<joint name="wam/elbow_pitch_joint_right" range="-0.9 3.1" damping="0.88"/>
|
||||
<geom name="elbow_link_fine" class="viz" mesh="elbow_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="elbow_link_convex" class="col" mesh="elbow_link_convex"/>
|
||||
<geom name="forearm_link_fine" class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="1 1 1 0"/>
|
||||
<geom name="forearm_link_convex_decomposition_p1" class="col"
|
||||
mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom name="forearm_link_convex_decomposition_p2" class="col"
|
||||
mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/wrist_yaw_link_right" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-005 0.00435824 -0.00511217"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/> <!--this is an approximation-->
|
||||
<!-- control 4: 0-->
|
||||
<joint name="wam/wrist_yaw_joint_right" range="-4.8 1.3" damping="0.55"/>
|
||||
<geom name="wrist_yaw_link_fine" class="viz" mesh="wrist_yaw_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="wrist_yaw_link_convex_decomposition_p1" class="col"
|
||||
mesh="wrist_yaw_link_convex_decomposition_p1"/>
|
||||
<geom name="wrist_yaw_link_convex_decomposition_p2" class="col"
|
||||
mesh="wrist_yaw_link_convex_decomposition_p2"/>
|
||||
<body name="wam/wrist_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/>
|
||||
<!-- control 5: 0-->
|
||||
<joint name="wam/wrist_pitch_joint_right" range="-1.6 1.6" damping="0.11"/>
|
||||
<geom name="wrist_pitch_link_fine" class="viz" mesh="wrist_pitch_link_fine"
|
||||
rgba="1 1 1 0"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p1" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p1"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p2" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p2"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p3" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p3"/>
|
||||
<body name="wam/wrist_palm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613"
|
||||
diaginertia="0.00020683 0.00010859 0.00010851"/>
|
||||
<!-- control 6: 1.8-->
|
||||
<joint name="wam/palm_yaw_joint_right" range="-2.2 2.2" damping="0.11"/>
|
||||
<geom name="wrist_palm_link_fine" class="viz" mesh="wrist_palm_link_fine"
|
||||
rgba="1 1 1 0"/>
|
||||
<geom name="wrist_palm_link_convex" class="col" mesh="wrist_palm_link_convex"/>
|
||||
<!-- EE=wam/paddle, configure name to the end effector name-->
|
||||
<body name="EE" pos="0 0 0.26" childclass="contact_geom">
|
||||
<geom name="bat" type="cylinder" size="0.075 0.005" rgba="1 0 0 1"
|
||||
quat="0.71 0 0.71 0"/>
|
||||
<geom name="bat_back" type="cylinder" size="0.0749 0.0025" rgba="0 1 0 1"
|
||||
quat="0.71 0 0.71 0" pos="-0.0026 0 0"/>
|
||||
<geom name="wam/paddle_handle" type="box" size="0.005 0.01 0.05" pos="0 0 -0.08"
|
||||
rgba="1 1 1 1"/>
|
||||
<!-- Extract information for sampling goals.-->
|
||||
<site name="wam/paddle_center" pos="0 0 0" rgba="1 1 1 1" size="0.00001"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</mujocoinclue>
|
||||
@ -0,0 +1,11 @@
|
||||
<mujocoinclude>
|
||||
<body name="target_ball" pos="0 0 0">
|
||||
<!-- <joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>-->
|
||||
<!-- <joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>-->
|
||||
<!-- <joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>-->
|
||||
<joint name="ball_joint" damping="0.0" pos="0 0 0" stiffness="0" frictionloss="0" type="free"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="target_ball_contact" rgba="1 1 0 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="0.1 0.03" priority="1"/>
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@ -0,0 +1,30 @@
|
||||
<mujocoinclude>
|
||||
<body name="table_tennis_table" pos="0 0 0">
|
||||
<geom class="viz" name="table_base_1" pos="1 0.7 0.375" rgba="1 1 1 1" size="0.05 0.05 .375" type="box" />
|
||||
<geom class="viz" name="table_base_2" pos="1 -0.7 0.375" rgba="1 1 1 1" size="0.05 0.05 .375" type="box" />
|
||||
<geom class="viz" name="table_base_3" pos="-1 -0.7 0.375" rgba="1 1 1 1" size="0.05 0.05 .375" type="box" />
|
||||
<geom class="viz" name="table_base_4" pos="-1 0.7 0.375" rgba="1 1 1 1" size="0.05 0.05 .375" type="box" />
|
||||
<body name="table_top" pos="0 0 0.76">
|
||||
<geom class="contact_geom" name="table_tennis_table" pos="0 0 0" rgba="0 0 0.5 1" size="1.37 .7625 .01" type="box" />
|
||||
<site name="left_up_corner" pos="-1.37 .7625 0.01" rgba="1 1 1 1" size="0.00001" />
|
||||
<site name="middle_up_corner" pos="0 .7625 0.01" rgba="1 1 1 1" size="0.00001" />
|
||||
<site name="left_down_corner" pos="-1.37 -0.7625 0.01" rgba="1 1 1 1" size="0.00001" />
|
||||
<site name="middle_down_corner" pos="0 -.7625 0.01" rgba="1 1 1 1" size="0.00001" />
|
||||
<geom class="contact_geom" material="floor_plane" name="table_te_context_spacennis_net" pos="0 0 0.08625" rgba="0 0 1 0.5" size="0.01 0.915 0.07625" type="box" />
|
||||
<geom class="viz" name="left_while_line" pos="0 -0.7425 0.01" rgba="1 1 1 1" size="1.37 .02 .001" type="box" />
|
||||
<geom class="viz" name="center_while_line" pos="0 0 0.01" rgba="1 1 1 1" size="1.37 .01 .001" type="box" />
|
||||
<geom class="viz" name="right_while_line" pos="0 0.7425 0.01" rgba="1 1 1 1" size="1.37 .02 .001" type="box" />
|
||||
<geom class="viz" name="right_side_line" pos="1.35 0 0.01" rgba="1 1 1 1" size="0.02 .7625 .001" type="box" />
|
||||
<geom class="viz" name="left_side_line" pos="-1.35 0 0.01" rgba="1 1 1 1" size="0.02 .7625 .001" type="box" />
|
||||
</body>
|
||||
<body name="achieved_pos" pos="0 0 0.5">
|
||||
<geom class="viz" name="achieved_point_geom" pos="0 0 0" rgba="0 1 0 1" size="0.02 0.001" type="cylinder" />
|
||||
</body>
|
||||
<body name="right_achieved_pos" pos="0 0 0.5">
|
||||
<geom class="viz" name="hitting_achieved_point_geom" pos="0 0 0" rgba="1 0 0 1" size="0.02 0.001" type="cylinder" />
|
||||
</body>
|
||||
<body name="target_point" pos="0 0 0.5">
|
||||
<geom class="viz" name="target_point_geom" pos="0 0 0" rgba="1 1 0 1" size="0.02 0.001" type="cylinder" />
|
||||
</body>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@ -0,0 +1,10 @@
|
||||
<mujocoinclude>
|
||||
<body name="target_ball" pos="0. 0. 0.1">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="target_ball_contact" rgba="1 1 0 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="0.1 0.03" priority="1"/>
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@ -0,0 +1,54 @@
|
||||
<mujocoinclude>
|
||||
<actuator>
|
||||
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="162.0" />-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="122.0" />-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="102.0" />-->
|
||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="100.0" />-->
|
||||
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="151.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="125.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="122.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="121.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="99.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="103.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="99.0" ctrllimited="true"/>-->
|
||||
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="100.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="600.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="122.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="500.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="99.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="103.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="99.0" ctrllimited="true"/>-->
|
||||
|
||||
<position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="800.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="800.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="800.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="800.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="100.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="1000.0" ctrllimited="true"/>
|
||||
<position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="100.0" ctrllimited="true"/>
|
||||
|
||||
<velocity ctrlrange="-3 3" joint="wam/base_yaw_joint_right" kv="0.1" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/shoulder_pitch_joint_right" kv="0.1" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/shoulder_yaw_joint_right" kv="0.1" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/elbow_pitch_joint_right" kv="0.01" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/wrist_yaw_joint_right" kv="0.01" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/wrist_pitch_joint_right" kv="0.1" ctrllimited="true"/>
|
||||
<velocity ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kv="0.1" ctrllimited="true"/>
|
||||
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="1600.0" ctrllimited="true"/>-->
|
||||
<!--<!– <velocity ctrlrange="-50 50" joint="wam/base_yaw_joint_right" kv="100" ctrllimited="true"/>–>-->
|
||||
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="2000.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="800.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="1200.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="100.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="2000.0" ctrllimited="true"/>-->
|
||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="100.0" ctrllimited="true"/>-->
|
||||
</actuator>
|
||||
</mujocoinclude>
|
||||
46
fancy_gym/envs/mujoco/table_tennis/assets/xml/shared.xml
Normal file
46
fancy_gym/envs/mujoco/table_tennis/assets/xml/shared.xml
Normal file
@ -0,0 +1,46 @@
|
||||
<mujocoinclude>
|
||||
<default>
|
||||
<default class="wam">
|
||||
<joint type="hinge" limited="true" pos="0 0 0" axis="0 0 1"/>
|
||||
</default>
|
||||
<default class="viz">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1"/>
|
||||
</default>
|
||||
<default class="col">
|
||||
<geom type="mesh" contype="0" conaffinity="1" group="0" rgba="1 1 1 1"/>
|
||||
</default>
|
||||
<default class="contact_geom">
|
||||
<geom condim="4" friction="0.1 0.1 0.1" margin="0" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
</default>
|
||||
</default>
|
||||
<asset>
|
||||
<mesh file="base_link_fine.stl"/>
|
||||
<mesh file="base_link_convex.stl"/>
|
||||
<mesh file="shoulder_link_fine.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p3.stl"/>
|
||||
<mesh file="shoulder_pitch_link_fine.stl"/>
|
||||
<mesh file="shoulder_pitch_link_convex.stl"/>
|
||||
<mesh file="upper_arm_link_fine.stl"/>
|
||||
<mesh file="upper_arm_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="upper_arm_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="elbow_link_fine.stl"/>
|
||||
<mesh file="elbow_link_convex.stl"/>
|
||||
<mesh file="forearm_link_fine.stl"/>
|
||||
<mesh file="forearm_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="forearm_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_yaw_link_fine.stl"/>
|
||||
<mesh file="wrist_yaw_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="wrist_yaw_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_pitch_link_fine.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p3.stl"/>
|
||||
<mesh file="wrist_palm_link_fine.stl"/>
|
||||
<mesh file="wrist_palm_link_convex.stl"/>
|
||||
<texture builtin="checker" height="512" name="texplane" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" type="2d"
|
||||
width="512"/>
|
||||
<material name="floor_plane" reflectance="0.5" texrepeat="1 1" texture="texplane" texuniform="true"/>
|
||||
</asset>
|
||||
</mujocoinclude>
|
||||
@ -0,0 +1,20 @@
|
||||
<mujoco model="table_tennis(v0.1)">
|
||||
<compiler angle="radian" coordinate="local" meshdir="../meshes/wam" />
|
||||
<option gravity="0 0 -9.81" timestep="0.002">
|
||||
<flag warmstart="enable" />
|
||||
</option>
|
||||
<custom>
|
||||
<numeric data="0 0 0 0 0 0 0" name="START_ANGLES" />
|
||||
</custom>
|
||||
<include file="shared.xml" />
|
||||
<worldbody>
|
||||
<light cutoff="60" diffuse="1 1 1" dir="-.1 -.2 -1.3" directional="true" exponent="1" pos=".1 .2 1.3" specular=".1 .1 .1" />
|
||||
<geom conaffinity="1" contype="1" material="floor_plane" name="floor" pos="0 0 0" size="10 5 1" type="plane" />
|
||||
<include file="include_table.xml" />
|
||||
<include file="include_barrett_wam_7dof_right.xml" />
|
||||
<!-- <include file="include_target_ball.xml" />-->
|
||||
<include file="include_free_ball.xml" />
|
||||
</worldbody>
|
||||
<include file="include_7_motor_actuator.xml" />
|
||||
<!-- <include file="right_arm_actuator.xml"/>-->
|
||||
</mujoco>
|
||||
29
fancy_gym/envs/mujoco/table_tennis/mp_wrapper.py
Normal file
29
fancy_gym/envs/mujoco/table_tennis/mp_wrapper.py
Normal file
@ -0,0 +1,29 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
# Random x goal + random init pos
|
||||
@property
|
||||
def context_mask(self):
|
||||
return np.hstack([
|
||||
[False] * 7, # joints position
|
||||
[False] * 7, # joints velocity
|
||||
[False] * 3, # position of box
|
||||
[False] * 4, # orientation of box
|
||||
[True] * 3, # position of target
|
||||
[True] * 4, # orientation of target
|
||||
# [True] * 1, # time
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.data.qpos[:7].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.data.qvel[:7].copy()
|
||||
85
fancy_gym/envs/mujoco/table_tennis/table_tennis_env.py
Normal file
85
fancy_gym/envs/mujoco/table_tennis/table_tennis_env.py
Normal file
@ -0,0 +1,85 @@
|
||||
import os
|
||||
|
||||
import numpy as np
|
||||
from gym import utils, spaces
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
import mujoco
|
||||
|
||||
MAX_EPISODE_STEPS_TABLE_TENNIS = 250
|
||||
|
||||
CONTEXT_BOUNDS_2DIMS = np.array([[-1.2, -0.6], [-0.2, 0.0]])
|
||||
CONTEXT_BOUNDS_4DIMS = np.array([[-1.2, -0.6, -1.0, -0.65],
|
||||
[-0.2, 0.6, -0.2, 0.65]])
|
||||
|
||||
|
||||
class TableTennisEnv(MujocoEnv, utils.EzPickle):
|
||||
"""
|
||||
7 DoF table tennis environment
|
||||
"""
|
||||
|
||||
def __init__(self, frame_skip: int = 4):
|
||||
utils.EzPickle.__init__(**locals())
|
||||
self._steps = 0
|
||||
|
||||
self.hit_ball = False
|
||||
self.ball_land_on_table = False
|
||||
self._id_set = False
|
||||
MujocoEnv.__init__(self,
|
||||
model_path=os.path.join(os.path.dirname(__file__), "assets", "xml", "table_tennis_env.xml"),
|
||||
frame_skip=frame_skip,
|
||||
mujoco_bindings="mujoco")
|
||||
self.action_space = spaces.Box(low=-1, high=1, shape=(7,), dtype=np.float32)
|
||||
|
||||
def _set_ids(self):
|
||||
self._floor_id = self.model.geom("floor").bodyid[0]
|
||||
self._ball_id = self.model.geom("target_ball_contact").bodyid[0]
|
||||
self._bat_front_id = self.model.geom("bat").bodyid[0]
|
||||
self._bat_back_id = self.model.geom("bat_back").bodyid[0]
|
||||
self._table_id = self.model.geom("table_tennis_table").bodyid[0]
|
||||
self._id_set = True
|
||||
|
||||
def step(self, action):
|
||||
if not self._id_set:
|
||||
self._set_ids()
|
||||
|
||||
unstable_simulation = False
|
||||
|
||||
try:
|
||||
self.do_simulation(action, self.frame_skip)
|
||||
except Exception as e:
|
||||
print("Simulation get unstable return with MujocoException: ", e)
|
||||
unstable_simulation = True
|
||||
|
||||
self._steps += 1
|
||||
episode_end = True if self._steps >= MAX_EPISODE_STEPS_TABLE_TENNIS else False
|
||||
|
||||
obs = self._get_obs()
|
||||
|
||||
return obs, 0., False, {}
|
||||
|
||||
def _contact_checker(self, id_1, id_2):
|
||||
for coni in range(0, self.data.ncon):
|
||||
con = self.data.contact[coni]
|
||||
if (con.geom1 == id_1 and con.geom2 == id_2) or (con.geom1 == id_2 and con.geom2 == id_1):
|
||||
return True
|
||||
return False
|
||||
|
||||
def reset_model(self):
|
||||
self._steps = 0
|
||||
return self._get_obs()
|
||||
|
||||
def _get_obs(self):
|
||||
obs = np.concatenate([
|
||||
self.data.qpos.flat[:7],
|
||||
self.data.qvel.flat[:7],
|
||||
])
|
||||
return obs
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = TableTennisEnv()
|
||||
env.reset()
|
||||
while True:
|
||||
env.render("human")
|
||||
env.step(env.action_space.sample())
|
||||
1
fancy_gym/envs/mujoco/table_tennis/table_tennis_utils.py
Normal file
1
fancy_gym/envs/mujoco/table_tennis/table_tennis_utils.py
Normal file
@ -0,0 +1 @@
|
||||
|
||||
Loading…
Reference in New Issue
Block a user