renameing alr module and updating tests
This commit is contained in:
Fabian
@@ -0,0 +1 @@
|
||||
from .mp_wrapper import MPWrapper
|
||||
@@ -0,0 +1,238 @@
|
||||
<mujoco model="wam(v1.31)">
|
||||
<compiler angle="radian" meshdir="../../meshes/wam/" />
|
||||
<option timestep="0.0005" integrator="Euler" />
|
||||
<size njmax="500" nconmax="100" />
|
||||
<default class="main">
|
||||
<joint limited="true" frictionloss="0.001" damping="0.07"/>
|
||||
<default class="viz">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
||||
</default>
|
||||
<default class="col">
|
||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
||||
</default>
|
||||
</default>
|
||||
<asset>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3_table" file="cup_split3.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup4_table" file="cup_split4.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup5_table" file="cup_split5.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup6_table" file="cup_split6.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup7_table" file="cup_split7.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup8_table" file="cup_split8.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup9_table" file="cup_split9.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup10_table" file="cup_split10.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup15_table" file="cup_split15.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup16_table" file="cup_split16.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup17_table" file="cup_split17.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup18_table" file="cup_split18.stl" scale="0.00211 0.00211 0.01" />
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="ground" size="5 5 1" type="plane" material="MatGnd" />
|
||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
||||
|
||||
<body name="wam/base_link" pos="0 0 0.6">
|
||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" name="base_link_convex_geom"/>
|
||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" name="shoulder_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" name="shoulder_link_convex_decomposition_p2_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" name="shoulder_link_convex_decomposition_p3_geom"/>
|
||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" name="upper_arm_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" name="upper_arm_link_convex_decomposition_p2_geom"/>
|
||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
||||
<geom class="viz" mesh="elbow_link_fine" />
|
||||
<geom class="col" mesh="elbow_link_convex" />
|
||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
||||
<body name="cup" pos="0 0 0" quat="-0.000203673 0 0 1">
|
||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="0.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
||||
<geom priority="1" name="cup_geom1" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup1" />
|
||||
<geom priority="1" name="cup_geom2" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup2" />
|
||||
<geom priority="1" name="cup_geom3" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3" />
|
||||
<geom priority="1" name="cup_geom4" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4" />
|
||||
<geom priority="1" name="cup_geom5" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5" />
|
||||
<geom priority="1" name="cup_geom6" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6" />
|
||||
<geom priority="1" name="cup_geom7" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7" />
|
||||
<geom priority="1" name="cup_geom8" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8" />
|
||||
<geom priority="1" name="cup_geom9" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9" />
|
||||
<geom priority="1" name="cup_geom10" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup10" />
|
||||
<geom name="cup_base" pos="0 -0.05 0.1165" euler="-1.57 0 0" type="cylinder" size="0.038 0.025" solref="-10000 -100"/>
|
||||
<geom name="cup_base_contact" pos="0 -0.005 0.1165" euler="-1.57 0 0" type="cylinder" size="0.03 0.0005" solref="-10000 -100" rgba="0 0 255 1"/>
|
||||
|
||||
<geom priority="1" name="cup_geom15" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15" />
|
||||
<geom priority="1" name="cup_geom16" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16" />
|
||||
<geom priority="1" name="cup_geom17" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17" />
|
||||
<geom priority="1" name="cup_geom18" pos="0 0.015 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18" />
|
||||
<site name="cup_robot" pos="0 0.05 0.1165" rgba="255 0 0 1"/>
|
||||
<site name="cup_robot_final" pos="0 -0.025 0.1165" rgba="0 255 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="table_body" pos="0 -1.85 0.4025">
|
||||
<geom name="table" type="box" size="0.4 0.6 0.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="table_contact_geom" type="box" size="0.4 0.6 0.01" pos="0 0 0.41" rgba="1.4 0.8 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
</body>
|
||||
<geom name="table_robot" type="box" size="0.1 0.1 0.3" pos="0 0.00 0.3025" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="wall" type="box" quat="1 0 0 0" size="0.4 0.04 1.1" pos="0. -2.45 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<!-- <geom name="side_wall_left" type="box" quat="1 0 0 0" size="0.04 0.4 0.5" pos="0.45 -2.05 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"-->
|
||||
<!-- solref="-10000 -100"/>-->
|
||||
<!-- <geom name="side_wall_right" type="box" quat="1 0 0 0" size="0.04 0.4 0.5" pos="-0.45 -2.05 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"-->
|
||||
<!-- solref="-10000 -100"/>-->
|
||||
|
||||
<!-- <body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">-->
|
||||
<!-- <geom priority= "1" type="box" size ="0.1 0.1 0.1" name="cup_base_table" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -10" mass="10"/>-->
|
||||
<!-- </body>-->
|
||||
|
||||
<body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">
|
||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="10.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
||||
<geom priority= "1" name="cup_geom_table3" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table4" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table5" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table6" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table7" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table9" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table10" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -010" type="mesh" mesh="cup10_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table" pos="0 -0.035 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.08 0.045" solref="-10000 -100" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table_contact" pos="0 0.015 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.06 0.0005" solref="-10000 -100" rgba="0 0 255 1" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table15" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table16" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table17" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom1_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18_table" mass="10"/>
|
||||
<site name="cup_goal_table" pos="0 0.11 0.1337249" rgba="255 0 0 1"/>
|
||||
<site name="cup_goal_final_table" pos="0.0 0.025 0.1337249" rgba="0 255 0 1"/>
|
||||
<site name="bounce_table" pos="0.0 -0.015 -0.2 " rgba="255 255 0 1"/>
|
||||
|
||||
<!-- <site name="cup_goal_final_table" pos="0.0 -0.025 0.1337249" rgba="0 255 0 1"/>-->
|
||||
</body>
|
||||
<!-- <body name="ball" pos="0.0 -0.813 2.382">-->
|
||||
<body name="ball" pos="0.0 -0.813 2.382">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<geom priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
|
||||
<!-- friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>-->
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
<camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>
|
||||
<camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />
|
||||
<site name="test" pos="0.1 0.1 0.1" rgba="0 0 1 1" type="sphere"/>
|
||||
</worldbody>
|
||||
<!-- <actuator>-->
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint" kp="800"/>-->
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint" kp="800"/>-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint" kp="800"/>-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint" kp="800"/>-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint" kp="100"/>-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint" kp="2000"/>-->
|
||||
<!-- <position ctrlrange="-2.7 2.7" joint="wam/palm_yaw_joint" kp="100"/>-->
|
||||
<!-- </actuator>-->
|
||||
<!-- <actuator>-->
|
||||
<!-- <motor ctrlrange="-150 150" joint="wam/base_yaw_joint"/>-->
|
||||
<!-- <motor ctrlrange="-125 125" joint="wam/shoulder_pitch_joint"/>-->
|
||||
<!-- <motor ctrlrange="-40 40" joint="wam/shoulder_yaw_joint"/>-->
|
||||
<!-- <motor ctrlrange="-60 60" joint="wam/elbow_pitch_joint"/>-->
|
||||
<!-- <motor ctrlrange="-5 5" joint="wam/wrist_yaw_joint"/>-->
|
||||
<!-- <motor ctrlrange="-5 5" joint="wam/wrist_pitch_joint"/>-->
|
||||
<!-- <motor ctrlrange="-2 2" joint="wam/palm_yaw_joint"/>-->
|
||||
<!-- </actuator>-->
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="125.0" joint="wam/shoulder_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="40.0" joint="wam/shoulder_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
||||
</actuator>
|
||||
</mujoco>
|
||||
@@ -0,0 +1,187 @@
|
||||
<mujoco model="wam(v1.31)">
|
||||
<compiler angle="radian" meshdir="../../meshes/wam/" />
|
||||
<option timestep="0.005" integrator="Euler" />
|
||||
<size njmax="500" nconmax="100" />
|
||||
<default class="main">
|
||||
<joint limited="true" frictionloss="0.001" damping="0.07"/>
|
||||
<default class="viz">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
||||
</default>
|
||||
<default class="col">
|
||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
||||
</default>
|
||||
</default>
|
||||
<asset>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3_table" file="cup_split3.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup4_table" file="cup_split4.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup5_table" file="cup_split5.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup6_table" file="cup_split6.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup7_table" file="cup_split7.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup8_table" file="cup_split8.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup9_table" file="cup_split9.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup10_table" file="cup_split10.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup15_table" file="cup_split15.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup16_table" file="cup_split16.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup17_table" file="cup_split17.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup18_table" file="cup_split18.stl" scale="0.00211 0.00211 0.01" />
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="ground" size="5 5 1" type="plane" material="MatGnd" />
|
||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
||||
|
||||
<body name="wam/base_link" pos="0 0 0.6">
|
||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" name="base_link_convex_geom"/>
|
||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" name="shoulder_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" name="shoulder_link_convex_decomposition_p2_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" name="shoulder_link_convex_decomposition_p3_geom"/>
|
||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" name="upper_arm_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" name="upper_arm_link_convex_decomposition_p2_geom"/>
|
||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
||||
<geom class="viz" mesh="elbow_link_fine" />
|
||||
<geom class="col" mesh="elbow_link_convex" />
|
||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
||||
<site name="init_ball_pos" size="0.025 0.025 0.025" pos="0.0 0.0 0.035" rgba="0 1 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="table_body" pos="0 -1.85 0.4025">
|
||||
<geom name="table" type="box" size="0.4 0.6 0.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="table_contact_geom" type="box" size="0.4 0.6 0.1" pos="0 0 0.31" rgba="1.4 0.8 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
</body>
|
||||
<geom name="table_robot" type="box" size="0.1 0.1 0.3" pos="0 0.00 0.3025" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="wall" type="box" quat="1 0 0 0" size="0.4 0.04 1.1" pos="0. -2.45 1.1" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
|
||||
<body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">
|
||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="10.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
||||
<geom priority= "1" name="cup_geom_table3" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table4" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table5" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table6" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table7" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table9" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table10" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -010" type="mesh" mesh="cup10_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table" pos="0 -0.035 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.08 0.045" solref="-10000 -100" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table_contact" pos="0 0.015 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.07 0.01" solref="-10000 -100" rgba="0 0 255 1" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table15" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table16" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table17" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom1_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18_table" mass="10"/>
|
||||
<site name="cup_goal_table" pos="0 0.11 0.1337249" rgba="255 0 0 1"/>
|
||||
<site name="cup_goal_final_table" pos="0.0 0.025 0.1337249" rgba="0 255 0 1"/>
|
||||
</body>
|
||||
<body name="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" limited="false"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<geom priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
<!-- <camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>-->
|
||||
<!-- <camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />-->
|
||||
<camera name="visualization" mode="fixed" euler="2.35 2.0 -0.75" pos="2.0 -0.0 1.85"/>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="wam/shoulder_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="50.0" joint="wam/shoulder_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
||||
</actuator>
|
||||
|
||||
</mujoco>
|
||||
@@ -0,0 +1,188 @@
|
||||
<mujoco model="wam(v1.31)">
|
||||
<compiler angle="radian" meshdir="./meshes/wam/" />
|
||||
<option timestep="0.005" integrator="Euler" />
|
||||
<size njmax="500" nconmax="100" />
|
||||
<default class="main">
|
||||
<joint limited="true" frictionloss="0.001" damping="0.07"/>
|
||||
<default class="viz">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.7 0.7 0.7 1" />
|
||||
</default>
|
||||
<default class="col">
|
||||
<geom type="mesh" contype="0" rgba="0.5 0.6 0.7 1" />
|
||||
</default>
|
||||
</default>
|
||||
<asset>
|
||||
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.25 0.26 0.25" rgb2="0.22 0.22 0.22" markrgb="0.3 0.3 0.3" width="100" height="100" />
|
||||
<material name="MatGnd" texture="groundplane" texrepeat="5 5" specular="1" shininess="0.3" reflectance="1e-05" />
|
||||
<mesh name="base_link_fine" file="base_link_fine.stl" />
|
||||
<mesh name="base_link_convex" file="base_link_convex.stl" />
|
||||
<mesh name="shoulder_link_fine" file="shoulder_link_fine.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p1" file="shoulder_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p2" file="shoulder_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="shoulder_link_convex_decomposition_p3" file="shoulder_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="shoulder_pitch_link_fine" file="shoulder_pitch_link_fine.stl" />
|
||||
<mesh name="shoulder_pitch_link_convex" file="shoulder_pitch_link_convex.stl" />
|
||||
<mesh name="upper_arm_link_fine" file="upper_arm_link_fine.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p1" file="upper_arm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="upper_arm_link_convex_decomposition_p2" file="upper_arm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="elbow_link_fine" file="elbow_link_fine.stl" />
|
||||
<mesh name="elbow_link_convex" file="elbow_link_convex.stl" />
|
||||
<mesh name="forearm_link_fine" file="forearm_link_fine.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p1" file="forearm_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="forearm_link_convex_decomposition_p2" file="forearm_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_yaw_link_fine" file="wrist_yaw_link_fine.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p1" file="wrist_yaw_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_yaw_link_convex_decomposition_p2" file="wrist_yaw_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_fine" file="wrist_pitch_link_fine.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p1" file="wrist_pitch_link_convex_decomposition_p1.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p2" file="wrist_pitch_link_convex_decomposition_p2.stl" />
|
||||
<mesh name="wrist_pitch_link_convex_decomposition_p3" file="wrist_pitch_link_convex_decomposition_p3.stl" />
|
||||
<mesh name="wrist_palm_link_fine" file="wrist_palm_link_fine.stl" />
|
||||
<mesh name="wrist_palm_link_convex" file="wrist_palm_link_convex.stl" />
|
||||
<mesh name="cup1" file="cup_split1.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup2" file="cup_split2.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3" file="cup_split3.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup4" file="cup_split4.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup5" file="cup_split5.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup6" file="cup_split6.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup7" file="cup_split7.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup8" file="cup_split8.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup9" file="cup_split9.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup10" file="cup_split10.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup11" file="cup_split11.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup12" file="cup_split12.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup13" file="cup_split13.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup14" file="cup_split14.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup15" file="cup_split15.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup16" file="cup_split16.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup17" file="cup_split17.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup18" file="cup_split18.stl" scale="0.001 0.001 0.001" />
|
||||
<mesh name="cup3_table" file="cup_split3.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup4_table" file="cup_split4.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup5_table" file="cup_split5.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup6_table" file="cup_split6.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup7_table" file="cup_split7.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup8_table" file="cup_split8.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup9_table" file="cup_split9.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup10_table" file="cup_split10.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup15_table" file="cup_split15.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup16_table" file="cup_split16.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup17_table" file="cup_split17.stl" scale="0.00211 0.00211 0.01" />
|
||||
<mesh name="cup18_table" file="cup_split18.stl" scale="0.00211 0.00211 0.01" />
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<geom name="ground" size="5 5 1" type="plane" material="MatGnd" />
|
||||
<light pos="0.1 0.2 1.3" dir="-0.0758098 -0.32162 -0.985527" directional="true" cutoff="60" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
|
||||
|
||||
<body name="wam/base_link" pos="0 0 0.6">
|
||||
<inertial pos="6.93764e-06 0.0542887 0.076438" quat="0.496481 0.503509 -0.503703 0.496255" mass="27.5544" diaginertia="0.432537 0.318732 0.219528" />
|
||||
<geom class="viz" quat="0.707107 0 0 -0.707107" mesh="base_link_fine" />
|
||||
<geom class="col" quat="0.707107 0 0 -0.707107" mesh="base_link_convex" name="base_link_convex_geom"/>
|
||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.16" quat="0.707107 0 0 -0.707107">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.12189" quat="0.999995 0.000984795 0.00270132 0.00136071" mass="10.7677" diaginertia="0.507411 0.462983 0.113271" />
|
||||
<joint name="wam/base_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.6 2.6" />
|
||||
<geom class="viz" pos="0 0 0.186" mesh="shoulder_link_fine" />
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p1" name="shoulder_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" name="shoulder_link_convex_decomposition_p2_geom"/>
|
||||
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" name="shoulder_link_convex_decomposition_p3_geom"/>
|
||||
<body name="wam/shoulder_pitch_link" pos="0 0 0.184" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236983 -0.0154211 0.0310561" quat="0.961781 -0.272983 0.0167269 0.0133385" mass="3.87494" diaginertia="0.0214207 0.0167101 0.0126465" />
|
||||
<joint name="wam/shoulder_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.985 1.985" />
|
||||
<geom class="viz" mesh="shoulder_pitch_link_fine" />
|
||||
<geom class="col" mesh="shoulder_pitch_link_convex" />
|
||||
<body name="wam/upper_arm_link" pos="0 -0.505 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-0.0382586 3.309e-05 -0.207508" quat="0.705455 0.0381914 0.0383402 0.706686" mass="1.80228" diaginertia="0.0665697 0.0634285 0.00622701" />
|
||||
<joint name="wam/shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.8 2.8" />
|
||||
<geom class="viz" pos="0 0 -0.505" mesh="upper_arm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p1" name="upper_arm_link_convex_decomposition_p1_geom"/>
|
||||
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" name="upper_arm_link_convex_decomposition_p2_geom"/>
|
||||
<body name="wam/forearm_link" pos="0.045 0 0.045" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="0.00498512 -0.132717 -0.00022942" quat="0.546303 0.447151 -0.548676 0.447842" mass="2.40017" diaginertia="0.0196896 0.0152225 0.00749914" />
|
||||
<joint name="wam/elbow_pitch_joint" pos="0 0 0" axis="0 0 1" range="-0.9 3.14159" />
|
||||
<geom class="viz" mesh="elbow_link_fine" />
|
||||
<geom class="col" mesh="elbow_link_convex" />
|
||||
<geom class="viz" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_fine" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p1" name="forearm_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="-0.045 -0.073 0" quat="0.707388 0.706825 0 0" mesh="forearm_link_convex_decomposition_p2" name="forearm_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_yaw_link" pos="-0.045 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-05 0.00435824 -0.00511217" quat="0.708528 -0.000120667 0.000107481 0.705683" mass="0.12376" diaginertia="0.0112011 0.0111887 7.58188e-05" />
|
||||
<joint name="wam/wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-4.55 1.25" />
|
||||
<geom class="viz" pos="0 0 0.3" mesh="wrist_yaw_link_fine" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p1" name="wrist_yaw_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" pos="0 0 0.3" mesh="wrist_yaw_link_convex_decomposition_p2" name="wrist_yaw_link_convex_decomposition_p2_geom" />
|
||||
<body name="wam/wrist_pitch_link" pos="0 0 0.3" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319" quat="0.994687 -0.102891 0.000824211 -0.00336105" mass="0.417974" diaginertia="0.000555166 0.000463174 0.00023407" />
|
||||
<joint name="wam/wrist_pitch_joint" pos="0 0 0" axis="0 0 1" range="-1.5707 1.5707" />
|
||||
<geom class="viz" mesh="wrist_pitch_link_fine" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" name="wrist_pitch_link_convex_decomposition_p1_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" name="wrist_pitch_link_convex_decomposition_p2_geom" />
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" name="wrist_pitch_link_convex_decomposition_p3_geom" />
|
||||
<body name="wam/wrist_palm_link" pos="0 -0.06 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="-7.974e-05 -0.00323552 -0.00016313" quat="0.594752 0.382453 0.382453 0.594752" mass="0.0686475" diaginertia="7.408e-05 3.81466e-05 3.76434e-05" />
|
||||
<joint name="wam/palm_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" />
|
||||
<geom class="viz" pos="0 0 -0.06" mesh="wrist_palm_link_fine" />
|
||||
<geom class="col" pos="0 0 -0.06" mesh="wrist_palm_link_convex" name="wrist_palm_link_convex_geom" />
|
||||
<site name="init_ball_pos" size="0.025 0.025 0.025" pos="0.0 0.0 0.035" rgba="0 1 0 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
<body name="table_body" pos="0 -2.8 0.4025">
|
||||
<geom name="table" type="box" size="1.5 1.5 0.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="table_contact_geom" type="box" size="1.5 1.5 0.1" pos="0 0 0.31" rgba="1.4 0.8 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
</body>
|
||||
<geom name="table_robot" type="box" size="0.1 0.1 0.3" pos="0 0.00 0.3025" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
<geom name="wall" type="box" quat="1 0 0 0" size="1.5 0.04 1.4" pos="0. -4.3 1.4" rgba="0.8 0.655 0.45 1" solimp="0.999 0.999 0.001"
|
||||
solref="-10000 -100"/>
|
||||
|
||||
<!-- <body name="cup_table" pos="0.32 -1.55 0.84" quat="0.7071068 0.7071068 0 0">-->
|
||||
<body name="cup_table" pos="-1.42 -4.05 0.84" quat="0.7071068 0.7071068 0 0">
|
||||
<inertial pos="-3.75236e-10 8.27811e-05 0.0947015" quat="0.999945 -0.0104888 0 0" mass="10.132" diaginertia="0.000285643 0.000270485 9.65696e-05" />
|
||||
<geom priority= "1" name="cup_geom_table3" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table4" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table5" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table6" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table7" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table9" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table10" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -010" type="mesh" mesh="cup10_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table" pos="0 -0.035 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.08 0.045" solref="-10000 -100" mass="10"/>
|
||||
<geom priority= "1" name="cup_base_table_contact" pos="0 0.015 0.1337249" euler="-1.57 0 0" type="cylinder" size="0.07 0.01" solref="-10000 -100" rgba="0 0 255 1" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table15" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table16" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom_table17" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17_table" mass="10"/>
|
||||
<geom priority= "1" name="cup_geom1_table8" pos="0 0.1 0.001" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18_table" mass="10"/>
|
||||
<site name="cup_goal_table" pos="0 0.11 0.1337249" rgba="255 0 0 1"/>
|
||||
<site name="cup_goal_final_table" pos="0.0 0.025 0.1337249" rgba="0 255 0 1"/>
|
||||
</body>
|
||||
<body name="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" limited="false"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0" limited="false"/>
|
||||
<geom priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
<!-- <camera name="visualization" mode="targetbody" target="wam/wrist_yaw_link" pos="1.5 -0.4 2.2"/>-->
|
||||
<!-- <camera name="experiment" mode="fixed" quat="0.44418059 0.41778323 0.54301123 0.57732103" pos="1.5 -0.3 1.33" />-->
|
||||
<camera name="visualization" mode="fixed" euler="2.35 2.0 -0.75" pos="2.0 -0.0 1.85"/>
|
||||
</worldbody>
|
||||
<actuator>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="150.0" joint="wam/base_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="wam/shoulder_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="50.0" joint="wam/shoulder_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="60.0" joint="wam/elbow_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_yaw_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="5.0" joint="wam/wrist_pitch_joint"/>
|
||||
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="2.0" joint="wam/palm_yaw_joint"/>
|
||||
</actuator>
|
||||
|
||||
</mujoco>
|
||||
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
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
Executable
BIN
Binary file not shown.
Executable
BIN
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,335 @@
|
||||
import os
|
||||
from typing import Optional
|
||||
|
||||
import mujoco_py.builder
|
||||
import numpy as np
|
||||
from gym import utils
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
# XML Variables
|
||||
ROBOT_COLLISION_OBJ = ["wrist_palm_link_convex_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
||||
"forearm_link_convex_decomposition_p1_geom",
|
||||
"forearm_link_convex_decomposition_p2_geom",
|
||||
"upper_arm_link_convex_decomposition_p1_geom",
|
||||
"upper_arm_link_convex_decomposition_p2_geom",
|
||||
"shoulder_link_convex_decomposition_p1_geom",
|
||||
"shoulder_link_convex_decomposition_p2_geom",
|
||||
"shoulder_link_convex_decomposition_p3_geom",
|
||||
"base_link_convex_geom", "table_contact_geom"]
|
||||
|
||||
CUP_COLLISION_OBJ = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "cup_geom_table6",
|
||||
"cup_geom_table7", "cup_geom_table8", "cup_geom_table9", "cup_geom_table10",
|
||||
"cup_geom_table15", "cup_geom_table16", "cup_geom_table17", "cup_geom1_table8"]
|
||||
|
||||
|
||||
class BeerPongEnv(MujocoEnv, utils.EzPickle):
|
||||
def __init__(self, frame_skip=2):
|
||||
self._steps = 0
|
||||
# Small Context -> Easier. Todo: Should we do different versions?
|
||||
# self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
# "beerpong_wo_cup" + ".xml")
|
||||
# self._cup_pos_min = np.array([-0.32, -2.2])
|
||||
# self._cup_pos_max = np.array([0.32, -1.2])
|
||||
|
||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
"beerpong_wo_cup_big_table" + ".xml")
|
||||
self._cup_pos_min = np.array([-1.42, -4.05])
|
||||
self._cup_pos_max = np.array([1.42, -1.25])
|
||||
|
||||
self._start_pos = np.array([0.0, 1.35, 0.0, 1.18, 0.0, -0.786, -1.59])
|
||||
self._start_vel = np.zeros(7)
|
||||
|
||||
self.release_step = 100 # time step of ball release
|
||||
self.ep_length = 600 // frame_skip
|
||||
|
||||
self.repeat_action = frame_skip
|
||||
# TODO: If accessing IDs is easier in the (new) official mujoco bindings, remove this
|
||||
self.model = None
|
||||
self.site_id = lambda x: self.model.site_name2id(x)
|
||||
self.body_id = lambda x: self.model.body_name2id(x)
|
||||
self.geom_id = lambda x: self.model.geom_name2id(x)
|
||||
|
||||
# for reward calculation
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
MujocoEnv.__init__(self, self.xml_path, frame_skip=1, mujoco_bindings="mujoco_py")
|
||||
utils.EzPickle.__init__(self)
|
||||
|
||||
@property
|
||||
def start_pos(self):
|
||||
return self._start_pos
|
||||
|
||||
@property
|
||||
def start_vel(self):
|
||||
return self._start_vel
|
||||
|
||||
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
return super().reset()
|
||||
|
||||
def reset_model(self):
|
||||
init_pos_all = self.init_qpos.copy()
|
||||
init_pos_robot = self.start_pos
|
||||
init_vel = np.zeros_like(init_pos_all)
|
||||
|
||||
self._steps = 0
|
||||
|
||||
start_pos = init_pos_all
|
||||
start_pos[0:7] = init_pos_robot
|
||||
|
||||
# TODO: Ask Max why we need to set the state twice.
|
||||
self.set_state(start_pos, init_vel)
|
||||
start_pos[7::] = self.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.set_state(start_pos, init_vel)
|
||||
xy = self.np_random.uniform(self._cup_pos_min, self._cup_pos_max)
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = xy
|
||||
xyz[-1] = 0.840
|
||||
self.model.body_pos[self.body_id("cup_table")] = xyz
|
||||
return self._get_obs()
|
||||
|
||||
def step(self, a):
|
||||
crash = False
|
||||
for _ in range(self.repeat_action):
|
||||
applied_action = a + self.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
||||
try:
|
||||
self.do_simulation(applied_action, self.frame_skip)
|
||||
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
|
||||
if self._steps < self.release_step:
|
||||
self.data.qpos[7::] = self.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.data.qvel[7::] = self.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
|
||||
crash = False
|
||||
except mujoco_py.builder.MujocoException:
|
||||
crash = True
|
||||
|
||||
ob = self._get_obs()
|
||||
|
||||
if not crash:
|
||||
reward, reward_infos = self._get_reward(applied_action)
|
||||
is_collided = reward_infos['is_collided']
|
||||
done = is_collided or self._steps == self.ep_length - 1
|
||||
self._steps += 1
|
||||
else:
|
||||
reward = -30
|
||||
done = True
|
||||
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
|
||||
|
||||
infos = dict(
|
||||
reward=reward,
|
||||
action=a,
|
||||
q_pos=self.data.qpos[0:7].ravel().copy(),
|
||||
q_vel=self.data.qvel[0:7].ravel().copy(), sim_crash=crash,
|
||||
)
|
||||
infos.update(reward_infos)
|
||||
return ob, reward, done, infos
|
||||
|
||||
def _get_obs(self):
|
||||
theta = self.data.qpos.flat[:7]
|
||||
theta_dot = self.data.qvel.flat[:7]
|
||||
ball_pos = self.data.get_body_xpos("ball").copy()
|
||||
cup_goal_diff_final = ball_pos - self.data.get_site_xpos("cup_goal_final_table").copy()
|
||||
cup_goal_diff_top = ball_pos - self.data.get_site_xpos("cup_goal_table").copy()
|
||||
return np.concatenate([
|
||||
np.cos(theta),
|
||||
np.sin(theta),
|
||||
theta_dot,
|
||||
cup_goal_diff_final,
|
||||
cup_goal_diff_top,
|
||||
self.model.body_pos[self.body_id("cup_table")][:2].copy(),
|
||||
# [self._steps], # Use TimeAwareObservation Wrapper instead ....
|
||||
])
|
||||
|
||||
@property
|
||||
def dt(self):
|
||||
return super(BeerPongEnv, self).dt * self.repeat_action
|
||||
|
||||
def _get_reward(self, action):
|
||||
goal_pos = self.data.get_site_xpos("cup_goal_table")
|
||||
ball_pos = self.data.get_body_xpos("ball")
|
||||
ball_vel = self.data.get_body_xvelp("ball")
|
||||
goal_final_pos = self.data.get_site_xpos("cup_goal_final_table")
|
||||
|
||||
self._check_contacts()
|
||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
||||
self.dist_ground_cup = np.linalg.norm(ball_pos - goal_pos) \
|
||||
if self.ball_ground_contact_first and self.dist_ground_cup == -1 else self.dist_ground_cup
|
||||
action_cost = np.sum(np.square(action))
|
||||
self.action_costs.append(np.copy(action_cost))
|
||||
# # ##################### Reward function which does not force to bounce once on the table (quad dist) #########
|
||||
|
||||
# Is this needed?
|
||||
# self._is_collided = self._check_collision_with_itself([self.geom_id(name) for name in CUP_COLLISION_OBJ])
|
||||
|
||||
if self._steps == self.ep_length - 1: # or self._is_collided:
|
||||
min_dist = np.min(self.dists)
|
||||
final_dist = self.dists_final[-1]
|
||||
if self.ball_ground_contact_first:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 2, -4
|
||||
else:
|
||||
if not self.ball_in_cup:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -4
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -2
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 0, 1, 0, 0
|
||||
action_cost = 1e-4 * np.mean(action_cost)
|
||||
reward = rew_offset - min_dist_coeff * min_dist ** 2 - final_dist_coeff * final_dist ** 2 - \
|
||||
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
|
||||
# release step punishment
|
||||
min_time_bound = 0.1
|
||||
max_time_bound = 1.0
|
||||
release_time = self.release_step * self.dt
|
||||
release_time_rew = int(release_time < min_time_bound) * (-30 - 10 * (release_time - min_time_bound) ** 2) + \
|
||||
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
|
||||
reward += release_time_rew
|
||||
success = self.ball_in_cup
|
||||
else:
|
||||
action_cost = 1e-2 * action_cost
|
||||
reward = - action_cost
|
||||
success = False
|
||||
# ##############################################################################################################
|
||||
infos = {"success": success, "ball_pos": ball_pos.copy(),
|
||||
"ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward,
|
||||
"table_contact_first": int(not self.ball_ground_contact_first),
|
||||
"is_collided": False} # TODO: Check if is collided is needed
|
||||
return reward, infos
|
||||
|
||||
def _check_contacts(self):
|
||||
if not self.ball_table_contact:
|
||||
self.ball_table_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("table_contact_geom")})
|
||||
if not self.ball_cup_contact:
|
||||
self.ball_cup_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id(name) for name in CUP_COLLISION_OBJ})
|
||||
if not self.ball_wall_contact:
|
||||
self.ball_wall_contact = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("wall")})
|
||||
if not self.ball_in_cup:
|
||||
self.ball_in_cup = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("cup_base_table_contact")})
|
||||
if not self.ball_ground_contact_first:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact \
|
||||
and not self.ball_in_cup:
|
||||
self.ball_ground_contact_first = self._check_collision({self.geom_id("ball_geom")},
|
||||
{self.geom_id("ground")})
|
||||
|
||||
# Checks if id_set1 has a collision with id_set2
|
||||
def _check_collision(self, id_set_1, id_set_2):
|
||||
"""
|
||||
If id_set_2 is set to None, it will check for a collision with itself (id_set_1).
|
||||
"""
|
||||
collision_id_set = id_set_2 - id_set_1 if id_set_2 is not None else id_set_1
|
||||
for coni in range(self.data.ncon):
|
||||
con = self.data.contact[coni]
|
||||
if ((con.geom1 in id_set_1 and con.geom2 in collision_id_set) or
|
||||
(con.geom2 in id_set_1 and con.geom1 in collision_id_set)):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
class BeerPongEnvFixedReleaseStep(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
|
||||
class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
def step(self, a):
|
||||
if self._steps < self.release_step:
|
||||
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
|
||||
else:
|
||||
reward = 0
|
||||
done = False
|
||||
while not done:
|
||||
sub_ob, sub_reward, done, sub_infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
|
||||
np.zeros(a.shape))
|
||||
reward += sub_reward
|
||||
infos = sub_infos
|
||||
ob = sub_ob
|
||||
ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# internal steps and thus, the observation also needs to be set correctly
|
||||
return ob, reward, done, infos
|
||||
|
||||
|
||||
# class ALRBeerBongEnvStepBased(ALRBeerBongEnv):
|
||||
# def __init__(self, frame_skip=1, apply_gravity_comp=True, noisy=False, rndm_goal=False, cup_goal_pos=None):
|
||||
# super().__init__(frame_skip, apply_gravity_comp, noisy, rndm_goal, cup_goal_pos)
|
||||
# self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
#
|
||||
# def step(self, a):
|
||||
# if self._steps < self.release_step:
|
||||
# return super(ALRBeerBongEnvStepBased, self).step(a)
|
||||
# else:
|
||||
# reward = 0
|
||||
# done = False
|
||||
# while not done:
|
||||
# sub_ob, sub_reward, done, sub_infos = super(ALRBeerBongEnvStepBased, self).step(np.zeros(a.shape))
|
||||
# if not done or sub_infos['sim_crash']:
|
||||
# reward += sub_reward
|
||||
# else:
|
||||
# ball_pos = self.sim.data.body_xpos[self.sim.model._body_name2id["ball"]].copy()
|
||||
# cup_goal_dist_final = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_final_table"]].copy())
|
||||
# cup_goal_dist_top = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_table"]].copy())
|
||||
# if sub_infos['success']:
|
||||
# dist_rew = -cup_goal_dist_final ** 2
|
||||
# else:
|
||||
# dist_rew = -0.5 * cup_goal_dist_final ** 2 - cup_goal_dist_top ** 2
|
||||
# reward = reward - sub_infos['action_cost'] + dist_rew
|
||||
# infos = sub_infos
|
||||
# ob = sub_ob
|
||||
# ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# # internal steps and thus, the observation also needs to be set correctly
|
||||
# return ob, reward, done, infos
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = BeerPongEnv(frame_skip=2)
|
||||
env.seed(0)
|
||||
# env = ALRBeerBongEnvStepBased(frame_skip=2)
|
||||
# env = ALRBeerBongEnvStepBasedEpisodicReward(frame_skip=2)
|
||||
# env = ALRBeerBongEnvFixedReleaseStep(frame_skip=2)
|
||||
import time
|
||||
|
||||
env.reset()
|
||||
env.render("human")
|
||||
for i in range(600):
|
||||
# ac = 10 * env.action_space.sample()
|
||||
ac = 0.05 * np.ones(7)
|
||||
obs, rew, d, info = env.step(ac)
|
||||
env.render("human")
|
||||
|
||||
if d:
|
||||
print('reward:', rew)
|
||||
print('RESETTING')
|
||||
env.reset()
|
||||
time.sleep(1)
|
||||
env.close()
|
||||
@@ -0,0 +1,212 @@
|
||||
import os
|
||||
|
||||
import mujoco_py.builder
|
||||
import numpy as np
|
||||
from gym import utils
|
||||
from gym.envs.mujoco import MujocoEnv
|
||||
|
||||
from alr_envs.envs.mujoco.beerpong.deprecated.beerpong_reward_staged import BeerPongReward
|
||||
|
||||
|
||||
class BeerPongEnv(MujocoEnv, utils.EzPickle):
|
||||
def __init__(self, frame_skip=2):
|
||||
self._steps = 0
|
||||
# Small Context -> Easier. Todo: Should we do different versions?
|
||||
# self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||
# "beerpong_wo_cup" + ".xml")
|
||||
# self._cup_pos_min = np.array([-0.32, -2.2])
|
||||
# self._cup_pos_max = np.array([0.32, -1.2])
|
||||
|
||||
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "../assets",
|
||||
"beerpong_wo_cup_big_table" + ".xml")
|
||||
self._cup_pos_min = np.array([-1.42, -4.05])
|
||||
self._cup_pos_max = np.array([1.42, -1.25])
|
||||
|
||||
self._start_pos = np.array([0.0, 1.35, 0.0, 1.18, 0.0, -0.786, -1.59])
|
||||
self._start_vel = np.zeros(7)
|
||||
|
||||
self.release_step = 100 # time step of ball release
|
||||
self.ep_length = 600 // frame_skip
|
||||
|
||||
self.reward_function = BeerPongReward()
|
||||
self.repeat_action = frame_skip
|
||||
self.model = None
|
||||
self.site_id = lambda x: self.model.site_name2id(x)
|
||||
self.body_id = lambda x: self.model.body_name2id(x)
|
||||
|
||||
MujocoEnv.__init__(self, self.xml_path, frame_skip=1)
|
||||
utils.EzPickle.__init__(self)
|
||||
|
||||
@property
|
||||
def start_pos(self):
|
||||
return self._start_pos
|
||||
|
||||
@property
|
||||
def start_vel(self):
|
||||
return self._start_vel
|
||||
|
||||
def reset(self):
|
||||
self.reward_function.reset()
|
||||
return super().reset()
|
||||
|
||||
def reset_model(self):
|
||||
init_pos_all = self.init_qpos.copy()
|
||||
init_pos_robot = self.start_pos
|
||||
init_vel = np.zeros_like(init_pos_all)
|
||||
|
||||
self._steps = 0
|
||||
|
||||
start_pos = init_pos_all
|
||||
start_pos[0:7] = init_pos_robot
|
||||
|
||||
# TODO: Ask Max why we need to set the state twice.
|
||||
self.set_state(start_pos, init_vel)
|
||||
start_pos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.set_state(start_pos, init_vel)
|
||||
xy = self.np_random.uniform(self._cup_pos_min, self._cup_pos_max)
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = xy
|
||||
xyz[-1] = 0.840
|
||||
self.sim.model.body_pos[self.body_id("cup_table")] = xyz
|
||||
return self._get_obs()
|
||||
|
||||
def step(self, a):
|
||||
crash = False
|
||||
for _ in range(self.repeat_action):
|
||||
applied_action = a + self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
|
||||
try:
|
||||
self.do_simulation(applied_action, self.frame_skip)
|
||||
self.reward_function.initialize(self)
|
||||
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
|
||||
if self._steps < self.release_step:
|
||||
self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
|
||||
self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
|
||||
crash = False
|
||||
except mujoco_py.builder.MujocoException:
|
||||
crash = True
|
||||
|
||||
ob = self._get_obs()
|
||||
|
||||
if not crash:
|
||||
reward, reward_infos = self.reward_function.compute_reward(self, applied_action)
|
||||
is_collided = reward_infos['is_collided']
|
||||
done = is_collided or self._steps == self.ep_length - 1
|
||||
self._steps += 1
|
||||
else:
|
||||
reward = -30
|
||||
done = True
|
||||
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
|
||||
|
||||
infos = dict(
|
||||
reward=reward,
|
||||
action=a,
|
||||
q_pos=self.sim.data.qpos[0:7].ravel().copy(),
|
||||
q_vel=self.sim.data.qvel[0:7].ravel().copy(), sim_crash=crash,
|
||||
)
|
||||
infos.update(reward_infos)
|
||||
return ob, reward, done, infos
|
||||
|
||||
def _get_obs(self):
|
||||
theta = self.sim.data.qpos.flat[:7]
|
||||
theta_dot = self.sim.data.qvel.flat[:7]
|
||||
ball_pos = self.data.get_body_xpos("ball").copy()
|
||||
cup_goal_diff_final = ball_pos - self.data.get_site_xpos("cup_goal_final_table").copy()
|
||||
cup_goal_diff_top = ball_pos - self.data.get_site_xpos("cup_goal_table").copy()
|
||||
return np.concatenate([
|
||||
np.cos(theta),
|
||||
np.sin(theta),
|
||||
theta_dot,
|
||||
cup_goal_diff_final,
|
||||
cup_goal_diff_top,
|
||||
self.sim.model.body_pos[self.body_id("cup_table")][:2].copy(),
|
||||
[self._steps],
|
||||
])
|
||||
|
||||
@property
|
||||
def dt(self):
|
||||
return super(BeerPongEnv, self).dt * self.repeat_action
|
||||
|
||||
|
||||
class BeerPongEnvFixedReleaseStep(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
|
||||
class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
|
||||
def __init__(self, frame_skip=2):
|
||||
super().__init__(frame_skip)
|
||||
self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
|
||||
def step(self, a):
|
||||
if self._steps < self.release_step:
|
||||
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
|
||||
else:
|
||||
reward = 0
|
||||
done = False
|
||||
while not done:
|
||||
sub_ob, sub_reward, done, sub_infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
|
||||
np.zeros(a.shape))
|
||||
reward += sub_reward
|
||||
infos = sub_infos
|
||||
ob = sub_ob
|
||||
ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# internal steps and thus, the observation also needs to be set correctly
|
||||
return ob, reward, done, infos
|
||||
|
||||
|
||||
# class ALRBeerBongEnvStepBased(ALRBeerBongEnv):
|
||||
# def __init__(self, frame_skip=1, apply_gravity_comp=True, noisy=False, rndm_goal=False, cup_goal_pos=None):
|
||||
# super().__init__(frame_skip, apply_gravity_comp, noisy, rndm_goal, cup_goal_pos)
|
||||
# self.release_step = 62 # empirically evaluated for frame_skip=2!
|
||||
#
|
||||
# def step(self, a):
|
||||
# if self._steps < self.release_step:
|
||||
# return super(ALRBeerBongEnvStepBased, self).step(a)
|
||||
# else:
|
||||
# reward = 0
|
||||
# done = False
|
||||
# while not done:
|
||||
# sub_ob, sub_reward, done, sub_infos = super(ALRBeerBongEnvStepBased, self).step(np.zeros(a.shape))
|
||||
# if not done or sub_infos['sim_crash']:
|
||||
# reward += sub_reward
|
||||
# else:
|
||||
# ball_pos = self.sim.data.body_xpos[self.sim.model._body_name2id["ball"]].copy()
|
||||
# cup_goal_dist_final = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_final_table"]].copy())
|
||||
# cup_goal_dist_top = np.linalg.norm(ball_pos - self.sim.data.site_xpos[
|
||||
# self.sim.model._site_name2id["cup_goal_table"]].copy())
|
||||
# if sub_infos['success']:
|
||||
# dist_rew = -cup_goal_dist_final ** 2
|
||||
# else:
|
||||
# dist_rew = -0.5 * cup_goal_dist_final ** 2 - cup_goal_dist_top ** 2
|
||||
# reward = reward - sub_infos['action_cost'] + dist_rew
|
||||
# infos = sub_infos
|
||||
# ob = sub_ob
|
||||
# ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
|
||||
# # internal steps and thus, the observation also needs to be set correctly
|
||||
# return ob, reward, done, infos
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = BeerPongEnv(frame_skip=2)
|
||||
env.seed(0)
|
||||
# env = ALRBeerBongEnvStepBased(frame_skip=2)
|
||||
# env = ALRBeerBongEnvStepBasedEpisodicReward(frame_skip=2)
|
||||
# env = ALRBeerBongEnvFixedReleaseStep(frame_skip=2)
|
||||
import time
|
||||
|
||||
env.reset()
|
||||
env.render("human")
|
||||
for i in range(600):
|
||||
# ac = 10 * env.action_space.sample()
|
||||
ac = 0.05 * np.ones(7)
|
||||
obs, rew, d, info = env.step(ac)
|
||||
env.render("human")
|
||||
|
||||
if d:
|
||||
print('reward:', rew)
|
||||
print('RESETTING')
|
||||
env.reset()
|
||||
time.sleep(1)
|
||||
env.close()
|
||||
@@ -0,0 +1,183 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
class BeerPongReward:
|
||||
def __init__(self):
|
||||
|
||||
self.robot_collision_objects = ["wrist_palm_link_convex_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p1_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p2_geom",
|
||||
"wrist_pitch_link_convex_decomposition_p3_geom",
|
||||
"wrist_yaw_link_convex_decomposition_p1_geom",
|
||||
"wrist_yaw_link_convex_decomposition_p2_geom",
|
||||
"forearm_link_convex_decomposition_p1_geom",
|
||||
"forearm_link_convex_decomposition_p2_geom",
|
||||
"upper_arm_link_convex_decomposition_p1_geom",
|
||||
"upper_arm_link_convex_decomposition_p2_geom",
|
||||
"shoulder_link_convex_decomposition_p1_geom",
|
||||
"shoulder_link_convex_decomposition_p2_geom",
|
||||
"shoulder_link_convex_decomposition_p3_geom",
|
||||
"base_link_convex_geom", "table_contact_geom"]
|
||||
|
||||
self.cup_collision_objects = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "cup_geom_table6",
|
||||
"cup_geom_table7", "cup_geom_table8", "cup_geom_table9", "cup_geom_table10",
|
||||
"cup_geom_table15",
|
||||
"cup_geom_table16",
|
||||
"cup_geom_table17", "cup_geom1_table8",
|
||||
]
|
||||
|
||||
self.dists = None
|
||||
self.dists_final = None
|
||||
self.action_costs = None
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
# IDs
|
||||
self.ball_collision_id = None
|
||||
self.table_collision_id = None
|
||||
self.wall_collision_id = None
|
||||
self.cup_table_collision_id = None
|
||||
self.ground_collision_id = None
|
||||
self.cup_collision_ids = None
|
||||
self.robot_collision_ids = None
|
||||
self.reset()
|
||||
self.is_initialized = False
|
||||
|
||||
def reset(self):
|
||||
self.dists = []
|
||||
self.dists_final = []
|
||||
self.action_costs = []
|
||||
self.ball_ground_contact_first = False
|
||||
self.ball_table_contact = False
|
||||
self.ball_wall_contact = False
|
||||
self.ball_cup_contact = False
|
||||
self.ball_in_cup = False
|
||||
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
|
||||
|
||||
def initialize(self, env):
|
||||
|
||||
if not self.is_initialized:
|
||||
self.is_initialized = True
|
||||
# TODO: Find a more elegant way to acces to the geom ids in each step -> less code
|
||||
self.ball_collision_id = {env.model.geom_name2id("ball_geom")}
|
||||
# self.ball_collision_id = env.model.geom_name2id("ball_geom")
|
||||
self.table_collision_id = {env.model.geom_name2id("table_contact_geom")}
|
||||
# self.table_collision_id = env.model.geom_name2id("table_contact_geom")
|
||||
self.wall_collision_id = {env.model.geom_name2id("wall")}
|
||||
# self.wall_collision_id = env.model.geom_name2id("wall")
|
||||
self.cup_table_collision_id = {env.model.geom_name2id("cup_base_table_contact")}
|
||||
# self.cup_table_collision_id = env.model.geom_name2id("cup_base_table_contact")
|
||||
self.ground_collision_id = {env.model.geom_name2id("ground")}
|
||||
# self.ground_collision_id = env.model.geom_name2id("ground")
|
||||
self.cup_collision_ids = {env.model.geom_name2id(name) for name in self.cup_collision_objects}
|
||||
# self.cup_collision_ids = [env.model.geom_name2id(name) for name in self.cup_collision_objects]
|
||||
self.robot_collision_ids = [env.model.geom_name2id(name) for name in self.robot_collision_objects]
|
||||
|
||||
def compute_reward(self, env, action):
|
||||
|
||||
goal_pos = env.data.get_site_xpos("cup_goal_table")
|
||||
ball_pos = env.data.get_body_xpos("ball")
|
||||
ball_vel = env.data.get_body_xvelp("ball")
|
||||
goal_final_pos = env.data.get_site_xpos("cup_goal_final_table")
|
||||
|
||||
self.check_contacts(env.sim)
|
||||
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
||||
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
||||
self.dist_ground_cup = np.linalg.norm(ball_pos - goal_pos) \
|
||||
if self.ball_ground_contact_first and self.dist_ground_cup == -1 else self.dist_ground_cup
|
||||
action_cost = np.sum(np.square(action))
|
||||
self.action_costs.append(np.copy(action_cost))
|
||||
# # ##################### Reward function which does not force to bounce once on the table (quad dist) #########
|
||||
|
||||
# Is this needed?
|
||||
# self._is_collided = self._check_collision_with_itself(env.sim, self.robot_collision_ids)
|
||||
|
||||
if env._steps == env.ep_length - 1: # or self._is_collided:
|
||||
min_dist = np.min(self.dists)
|
||||
final_dist = self.dists_final[-1]
|
||||
if self.ball_ground_contact_first:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 2, -4
|
||||
else:
|
||||
if not self.ball_in_cup:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -4
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 1, 0.5, 0, -2
|
||||
else:
|
||||
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 0, 1, 0, 0
|
||||
action_cost = 1e-4 * np.mean(action_cost)
|
||||
reward = rew_offset - min_dist_coeff * min_dist ** 2 - final_dist_coeff * final_dist ** 2 - \
|
||||
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
|
||||
# release step punishment
|
||||
min_time_bound = 0.1
|
||||
max_time_bound = 1.0
|
||||
release_time = env.release_step * env.dt
|
||||
release_time_rew = int(release_time < min_time_bound) * (-30 - 10 * (release_time - min_time_bound) ** 2) + \
|
||||
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
|
||||
reward += release_time_rew
|
||||
success = self.ball_in_cup
|
||||
else:
|
||||
action_cost = 1e-2 * action_cost
|
||||
reward = - action_cost
|
||||
success = False
|
||||
# ##############################################################################################################
|
||||
infos = {"success": success, "ball_pos": ball_pos.copy(),
|
||||
"ball_vel": ball_vel.copy(), "action_cost": action_cost, "task_reward": reward,
|
||||
"table_contact_first": int(not self.ball_ground_contact_first),
|
||||
"is_collided": False} # TODO: Check if is collided is needed
|
||||
return reward, infos
|
||||
|
||||
def check_contacts(self, sim):
|
||||
if not self.ball_table_contact:
|
||||
self.ball_table_contact = self._check_collision(sim, self.ball_collision_id, self.table_collision_id)
|
||||
if not self.ball_cup_contact:
|
||||
self.ball_cup_contact = self._check_collision(sim, self.ball_collision_id, self.cup_collision_ids)
|
||||
if not self.ball_wall_contact:
|
||||
self.ball_wall_contact = self._check_collision(sim, self.ball_collision_id, self.wall_collision_id)
|
||||
if not self.ball_in_cup:
|
||||
self.ball_in_cup = self._check_collision(sim, self.ball_collision_id, self.cup_table_collision_id)
|
||||
if not self.ball_ground_contact_first:
|
||||
if not self.ball_table_contact and not self.ball_cup_contact and not self.ball_wall_contact \
|
||||
and not self.ball_in_cup:
|
||||
self.ball_ground_contact_first = self._check_collision(sim, self.ball_collision_id,
|
||||
self.ground_collision_id)
|
||||
|
||||
# Checks if id_set1 has a collision with id_set2
|
||||
def _check_collision(self, sim, id_set_1, id_set_2):
|
||||
"""
|
||||
If id_set_2 is set to None, it will check for a collision with itself (id_set_1).
|
||||
"""
|
||||
collision_id_set = id_set_2 - id_set_1 if id_set_2 is not None else id_set_1
|
||||
for coni in range(sim.data.ncon):
|
||||
con = sim.data.contact[coni]
|
||||
if ((con.geom1 in id_set_1 and con.geom2 in collision_id_set) or
|
||||
(con.geom2 in id_set_1 and con.geom1 in collision_id_set)):
|
||||
return True
|
||||
return False
|
||||
|
||||
# def _check_collision_with_itself(self, sim, collision_ids):
|
||||
# col_1, col_2 = False, False
|
||||
# for j, id in enumerate(collision_ids):
|
||||
# col_1 = self._check_collision_with_set_of_objects(sim, id, collision_ids[:j])
|
||||
# if j != len(collision_ids) - 1:
|
||||
# col_2 = self._check_collision_with_set_of_objects(sim, id, collision_ids[j + 1:])
|
||||
# else:
|
||||
# col_2 = False
|
||||
# collision = True if col_1 or col_2 else False
|
||||
# return collision
|
||||
|
||||
### This function will not be needed if we really do not need to check for collision with itself
|
||||
# def _check_collision_with_set_of_objects(self, sim, id_1, id_list):
|
||||
# for coni in range(0, sim.data.ncon):
|
||||
# con = sim.data.contact[coni]
|
||||
#
|
||||
# collision = con.geom1 in id_list and con.geom2 == id_1
|
||||
# collision_trans = con.geom1 == id_1 and con.geom2 in id_list
|
||||
#
|
||||
# if collision or collision_trans:
|
||||
# return True
|
||||
# return False
|
||||
@@ -0,0 +1,43 @@
|
||||
from typing import Union, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from alr_envs.black_box.raw_interface_wrapper import RawInterfaceWrapper
|
||||
|
||||
|
||||
class MPWrapper(RawInterfaceWrapper):
|
||||
|
||||
@property
|
||||
def context_mask(self) -> np.ndarray:
|
||||
return np.hstack([
|
||||
[False] * 7, # cos
|
||||
[False] * 7, # sin
|
||||
[False] * 7, # joint velocities
|
||||
[False] * 3, # cup_goal_diff_final
|
||||
[False] * 3, # cup_goal_diff_top
|
||||
[True] * 2, # xy position of cup
|
||||
# [False] # env steps
|
||||
])
|
||||
|
||||
@property
|
||||
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qpos[0:7].copy()
|
||||
|
||||
@property
|
||||
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
|
||||
return self.env.data.qvel[0:7].copy()
|
||||
|
||||
# TODO: Fix this
|
||||
def _episode_callback(self, action: np.ndarray, mp) -> Tuple[np.ndarray, Union[np.ndarray, None]]:
|
||||
if mp.learn_tau:
|
||||
self.env.env.release_step = action[0] / self.env.dt # Tau value
|
||||
return action, None
|
||||
else:
|
||||
return action, None
|
||||
|
||||
def set_context(self, context):
|
||||
xyz = np.zeros(3)
|
||||
xyz[:2] = context
|
||||
xyz[-1] = 0.840
|
||||
self.env.env.model.body_pos[self.env.env.cup_table_id] = xyz
|
||||
return self.get_observation_from_step(self.env.env._get_obs())
|
||||
Reference in New Issue
Block a user