lots of new stuff...
This commit is contained in:
Maximilian Huettenrauch
parent
cab2c249bb
commit
07195fa2dc
@ -1 +1 @@
|
|||||||
from alr_envs.mujoco.alr_reacher import ALRReacherEnv
|
from alr_envs.mujoco.reacher.alr_reacher import ALRReacherEnv
|
||||||
0
alr_envs/mujoco/ball_in_a_cup/__init__.py
Normal file
0
alr_envs/mujoco/ball_in_a_cup/__init__.py
Normal file
366
alr_envs/mujoco/ball_in_a_cup/assets/ball-in-a-cup_base.xml
Normal file
366
alr_envs/mujoco/ball_in_a_cup/assets/ball-in-a-cup_base.xml
Normal file
@ -0,0 +1,366 @@
|
|||||||
|
<mujoco model="wam(v1.31)">
|
||||||
|
<compiler angle="radian" meshdir="meshes/" />
|
||||||
|
<option timestep="0.0005" integrator="Euler" />
|
||||||
|
<size njmax="500" nconmax="100" />
|
||||||
|
<default class="main">
|
||||||
|
<joint limited="true" frictionloss="0.001" />
|
||||||
|
<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" />
|
||||||
|
</asset>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<geom name="ground" size="1.5 2 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" />
|
||||||
|
<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" />
|
||||||
|
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p2" />
|
||||||
|
<geom class="col" pos="0 0 0.186" mesh="shoulder_link_convex_decomposition_p3" />
|
||||||
|
<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" />
|
||||||
|
<geom class="col" pos="0 0 -0.505" mesh="upper_arm_link_convex_decomposition_p2" />
|
||||||
|
<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 name="cup_geom1" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup1" />
|
||||||
|
<geom name="cup_geom2" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup2" />
|
||||||
|
<geom name="cup_geom3" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup3" />
|
||||||
|
<geom name="cup_geom4" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup4" />
|
||||||
|
<geom name="cup_geom5" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup5" />
|
||||||
|
<geom name="cup_geom6" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup6" />
|
||||||
|
<geom name="cup_geom7" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup7" />
|
||||||
|
<geom name="cup_geom8" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup8" />
|
||||||
|
<geom name="cup_geom9" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup9" />
|
||||||
|
<geom name="cup_geom10" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup10" />
|
||||||
|
<geom name="cup_base" pos="0 -0.035 0.1165" euler="-1.57 0 0" type="cylinder" size="0.038 0.0045" solref="-10000 -100"/>
|
||||||
|
<geom name="cup_base_contact" pos="0 -0.025 0.1165" euler="-1.57 0 0" type="cylinder" size="0.03 0.0005" solref="-10000 -100" rgba="0 0 255 1"/>
|
||||||
|
<!-- <geom name="cup_geom11" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup11" />-->
|
||||||
|
<!-- <geom name="cup_geom12" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup12" />-->
|
||||||
|
<!-- <geom name="cup_geom13" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup13" />-->
|
||||||
|
<!-- <geom name="cup_geom14" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup14" />-->
|
||||||
|
|
||||||
|
<geom name="cup_geom15" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup15" />
|
||||||
|
<geom name="cup_geom16" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup16" />
|
||||||
|
<geom name="cup_geom17" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup17" />
|
||||||
|
<geom name="cup_geom18" pos="0 0.05 0.055" euler="-1.57 0 0" solref="-10000 -100" type="mesh" mesh="cup18" />
|
||||||
|
<site name="cup_goal" pos="0 0.05 0.1165" rgba="255 0 0 1"/>
|
||||||
|
<site name="cup_goal_final" pos="0 -0.025 0.1165" rgba="0 255 0 1"/>
|
||||||
|
<body name="B0" pos="0 -0.045 0.1165" quat="0.707388 0 0 -0.706825">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<geom name="G0" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B1" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_1" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_1" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G1" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B2" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_2" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_2" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G2" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B3" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_3" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_3" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G3" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B4" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_4" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_4" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G4" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B5" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_5" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_5" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G5" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B6" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_6" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_6" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G6" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B7" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_7" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_7" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G7" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B8" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_8" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_8" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G8" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B9" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_9" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_9" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G9" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B10" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_10" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_10" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G10" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B11" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_11" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_11" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G11" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B12" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_12" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_12" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G12" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B13" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_13" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_13" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G13" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B14" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_14" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_14" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G14" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B15" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_15" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_15" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G15" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B16" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_16" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_16" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G16" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B17" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_17" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_17" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G17" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B18" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_18" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_18" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G18" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B19" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_19" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_19" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G19" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B20" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_20" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_20" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G20" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B21" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_21" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_21" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G21" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B22" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_22" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_22" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G22" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B23" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_23" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_23" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G23" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B24" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_24" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_24" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G24" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B25" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_25" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_25" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G25" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B26" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_26" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_26" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G26" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B27" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_27" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_27" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G27" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B28" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_28" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_28" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G28" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="B29" pos="0.0107 0 0">
|
||||||
|
<inertial pos="0 0 0" quat="0.707107 0 0.707107 0" mass="7.4927e-05" diaginertia="5.87e-10 5.87e-10 3.74635e-11" />
|
||||||
|
<joint name="J0_29" pos="-0.00535 0 0" axis="0 1 0" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<joint name="J1_29" pos="-0.00535 0 0" axis="0 0 1" group="3" limited="false" damping="0.0001" frictionloss="0" />
|
||||||
|
<geom name="G29" size="0.001 0.00427" quat="0.707107 0 0.707107 0" type="capsule" rgba="0.8 0.2 0.1 1" />
|
||||||
|
<body name="ball">
|
||||||
|
<geom name="ball_geom" type="sphere" size="0.02" mass="0.021" rgba="0.8 0.2 0.1 1"/>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
</body>
|
||||||
|
<site name="context_point" pos="0 0.05 0.1165" euler="-1.57 0 0" size="0.015" rgba="1 0 0 0.6" type="sphere"/>
|
||||||
|
<site name="context_point1" pos="-0.5 -0.85 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point2" pos="-0.5 -0.85 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point3" pos="-0.5 -0.35 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point4" pos="-0.5 -0.35 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point5" pos="0.5 -0.85 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point6" pos="0.5 -0.85 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point7" pos="0.5 -0.35 0.8165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_point8" pos="0.5 -0.35 1.4165" euler="-1.57 0 0" size="0.015" rgba="0 1 0 0.3" type="sphere"/>
|
||||||
|
<site name="context_space" pos="0 -0.6 1.1165" euler="0 0 0" size="0.5 0.25 0.3" rgba="0 0 1 0.15" type="box"/>
|
||||||
|
<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" />
|
||||||
|
</worldbody>
|
||||||
|
|
||||||
|
<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"/>
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint" kp="151.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint" kp="125.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint" kp="122.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint" kp="121.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint" kp="99.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint" kp="103.0"/>-->
|
||||||
|
<!-- <position ctrllimited="true" ctrlrange="-3 3" joint="wam/palm_yaw_joint" kp="99.0"/>-->
|
||||||
|
<!-- <velocity joint="wam/base_yaw_joint" kv="7.0"/>-->
|
||||||
|
<!-- <velocity joint="wam/shoulder_pitch_joint" kv="15.0"/>-->
|
||||||
|
<!-- <velocity joint="wam/shoulder_yaw_joint" kv="5.0"/>-->
|
||||||
|
<!-- <velocity joint="wam/elbow_pitch_joint" kv="2.5"/>-->
|
||||||
|
<!-- <velocity joint="wam/wrist_yaw_joint" kv="0.3"/>-->
|
||||||
|
<!-- <velocity joint="wam/wrist_pitch_joint" kv="0.3"/>-->
|
||||||
|
<!-- <velocity joint="wam/palm_yaw_joint" kv="0.05"/>-->
|
||||||
|
</actuator>
|
||||||
|
</mujoco>
|
||||||
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split1.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split1.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split10.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split10.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split11.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split11.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split12.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split12.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split13.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split13.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split14.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split14.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split15.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split15.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split16.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split16.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split17.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split17.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split18.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split18.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split2.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split2.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split3.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split3.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split4.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split4.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split5.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split5.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split6.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split6.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split7.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split7.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split8.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split8.stl
Normal file
Binary file not shown.
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split9.stl
Normal file
BIN
alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split9.stl
Normal file
Binary file not shown.
117
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup.py
Normal file
117
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup.py
Normal file
@ -0,0 +1,117 @@
|
|||||||
|
from gym.envs.mujoco import mujoco_env
|
||||||
|
from gym import utils
|
||||||
|
import os
|
||||||
|
import numpy as np
|
||||||
|
from alr_envs.mujoco.ball_in_a_cup.ball_in_a_cup_reward import BallInACupReward
|
||||||
|
import mujoco_py
|
||||||
|
|
||||||
|
|
||||||
|
class ALRBallInACupEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
||||||
|
def __init__(self, ):
|
||||||
|
self._steps = 0
|
||||||
|
|
||||||
|
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||||
|
"ball-in-a-cup_base" + ".xml")
|
||||||
|
|
||||||
|
self.sim_time = 8 # seconds
|
||||||
|
self.sim_steps = int(self.sim_time / (0.0005 * 4)) # circular dependency.. sim.dt <-> mujocoenv init <-> reward fct
|
||||||
|
self.reward_function = BallInACupReward(self.sim_steps)
|
||||||
|
|
||||||
|
self.start_pos = np.array([0.0, 0.58760536, 0.0, 1.36004913, 0.0, -0.32072943, -1.57])
|
||||||
|
self._q_pos = []
|
||||||
|
|
||||||
|
utils.EzPickle.__init__(self)
|
||||||
|
mujoco_env.MujocoEnv.__init__(self, os.path.join(os.path.dirname(__file__), "assets", "ball-in-a-cup_base.xml"),
|
||||||
|
frame_skip=4)
|
||||||
|
|
||||||
|
def reset_model(self):
|
||||||
|
start_pos = self.init_qpos.copy()
|
||||||
|
start_pos[0:7] = self.start_pos
|
||||||
|
start_vel = np.zeros_like(start_pos)
|
||||||
|
self.set_state(start_pos, start_vel)
|
||||||
|
self._steps = 0
|
||||||
|
self.reward_function.reset()
|
||||||
|
self._q_pos = []
|
||||||
|
|
||||||
|
def do_simulation(self, ctrl, n_frames):
|
||||||
|
self.sim.data.ctrl[:] = ctrl
|
||||||
|
for _ in range(n_frames):
|
||||||
|
try:
|
||||||
|
self.sim.step()
|
||||||
|
except mujoco_py.builder.MujocoException as e:
|
||||||
|
# print("Error in simulation: " + str(e))
|
||||||
|
# error = True
|
||||||
|
# Copy the current torque as if it would have been applied until the end of the trajectory
|
||||||
|
# for i in range(k + 1, sim_time):
|
||||||
|
# torques.append(trq)
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def step(self, a):
|
||||||
|
# Apply gravity compensation
|
||||||
|
if not np.all(self.sim.data.qfrc_applied[:7] == self.sim.data.qfrc_bias[:7]):
|
||||||
|
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
||||||
|
|
||||||
|
reward_dist = 0.0
|
||||||
|
angular_vel = 0.0
|
||||||
|
# if self._steps >= self.steps_before_reward:
|
||||||
|
# vec = self.get_body_com("fingertip") - self.get_body_com("target")
|
||||||
|
# reward_dist -= self.reward_weight * np.linalg.norm(vec)
|
||||||
|
# angular_vel -= np.linalg.norm(self.sim.data.qvel.flat[:self.n_links])
|
||||||
|
reward_ctrl = - np.square(a).sum()
|
||||||
|
# reward_balance = - self.balance_weight * np.abs(
|
||||||
|
# angle_normalize(np.sum(self.sim.data.qpos.flat[:self.n_links]), type="rad"))
|
||||||
|
#
|
||||||
|
# reward = reward_dist + reward_ctrl + angular_vel + reward_balance
|
||||||
|
# self.do_simulation(a, self.frame_skip)
|
||||||
|
|
||||||
|
crash = self.do_simulation(a, self.frame_skip)
|
||||||
|
|
||||||
|
self._q_pos.append(self.sim.data.qpos[0:7].ravel().copy())
|
||||||
|
|
||||||
|
ob = self._get_obs()
|
||||||
|
|
||||||
|
if not crash:
|
||||||
|
reward, success, collision = self.reward_function.compute_reward(a, self.sim, self._steps)
|
||||||
|
done = success or self._steps == self.sim_steps - 1 or collision
|
||||||
|
self._steps += 1
|
||||||
|
else:
|
||||||
|
reward = -1000
|
||||||
|
done = True
|
||||||
|
return ob, reward, done, dict(reward_dist=reward_dist, reward_ctrl=reward_ctrl,
|
||||||
|
velocity=angular_vel, # reward_balance=reward_balance,
|
||||||
|
# end_effector=self.get_body_com("fingertip").copy(),
|
||||||
|
goal=self.goal if hasattr(self, "goal") else None,
|
||||||
|
traj=self._q_pos)
|
||||||
|
|
||||||
|
def _get_obs(self):
|
||||||
|
theta = self.sim.data.qpos.flat[:7]
|
||||||
|
return np.concatenate([
|
||||||
|
np.cos(theta),
|
||||||
|
np.sin(theta),
|
||||||
|
# self.get_body_com("target"), # only return target to make problem harder
|
||||||
|
[self._steps],
|
||||||
|
])
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
env = ALRBallInACupEnv()
|
||||||
|
env.reset()
|
||||||
|
for i in range(2000):
|
||||||
|
# objective.load_result("/tmp/cma")
|
||||||
|
# test with random actions
|
||||||
|
# ac = 0.0 * env.action_space.sample()
|
||||||
|
ac = env.start_pos
|
||||||
|
# ac[0] += np.pi/2
|
||||||
|
obs, rew, d, info = env.step(ac)
|
||||||
|
env.render()
|
||||||
|
|
||||||
|
print(rew)
|
||||||
|
|
||||||
|
if d:
|
||||||
|
break
|
||||||
|
|
||||||
|
env.close()
|
||||||
|
|
||||||
127
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_reward.py
Normal file
127
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_reward.py
Normal file
@ -0,0 +1,127 @@
|
|||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
class BallInACupReward:
|
||||||
|
def __init__(self, sim_time):
|
||||||
|
self.sim_time = sim_time
|
||||||
|
|
||||||
|
self.collision_objects = ["cup_geom1", "cup_geom2", "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"]
|
||||||
|
|
||||||
|
self.ctxt_id = None
|
||||||
|
self.ball_id = None
|
||||||
|
self.ball_collision_id = None
|
||||||
|
self.goal_id = None
|
||||||
|
self.goal_final_id = None
|
||||||
|
self.collision_ids = None
|
||||||
|
|
||||||
|
self.ball_traj = None
|
||||||
|
self.dists = None
|
||||||
|
self.dists_final = None
|
||||||
|
self.costs = None
|
||||||
|
|
||||||
|
self.reset()
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self.ball_traj = np.zeros(shape=(self.sim_time, 3))
|
||||||
|
self.dists = []
|
||||||
|
self.dists_final = []
|
||||||
|
self.costs = []
|
||||||
|
|
||||||
|
def compute_reward(self, action, sim, step):
|
||||||
|
self.ctxt_id = sim.model._site_name2id['context_point']
|
||||||
|
self.ball_id = sim.model._body_name2id["ball"]
|
||||||
|
self.ball_collision_id = sim.model._geom_name2id["ball_geom"]
|
||||||
|
self.goal_id = sim.model._site_name2id["cup_goal"]
|
||||||
|
self.goal_final_id = sim.model._site_name2id["cup_goal_final"]
|
||||||
|
self.collision_ids = [sim.model._geom_name2id[name] for name in self.collision_objects]
|
||||||
|
|
||||||
|
ball_in_cup = self.check_ball_in_cup(sim, self.ball_collision_id)
|
||||||
|
|
||||||
|
# Compute the current distance from the ball to the inner part of the cup
|
||||||
|
goal_pos = sim.data.site_xpos[self.goal_id]
|
||||||
|
ball_pos = sim.data.body_xpos[self.ball_id]
|
||||||
|
goal_final_pos = sim.data.site_xpos[self.goal_final_id]
|
||||||
|
self.dists.append(np.linalg.norm(goal_pos - ball_pos))
|
||||||
|
self.dists_final.append(np.linalg.norm(goal_final_pos - ball_pos))
|
||||||
|
# dists_ctxt.append(np.linalg.norm(ball_pos - ctxt))
|
||||||
|
self.ball_traj[step, :] = ball_pos
|
||||||
|
|
||||||
|
if self.check_collision(sim):
|
||||||
|
return -1000, False, True
|
||||||
|
|
||||||
|
# self._get_cost(ball_pos, goal_pos, goal_final_pos, action,
|
||||||
|
# sim.data.get_site_xpos('context_point').copy(), step)
|
||||||
|
|
||||||
|
# min_dist = np.min(self.dists)
|
||||||
|
# dist_final = self.dists_final[-1]
|
||||||
|
action_cost = np.sum(np.square(action))
|
||||||
|
|
||||||
|
# cost = self.get_stage_wise_cost(ball_in_cup, min_dist, self.dists_final[-1]) # , self.dists_ctxt[-1])
|
||||||
|
if step == self.sim_time - 1:
|
||||||
|
min_dist = np.min(self.dists)
|
||||||
|
dist_final = self.dists_final[-1]
|
||||||
|
|
||||||
|
cost = 0.5 * min_dist + 0.5 * dist_final
|
||||||
|
# cost = 3 + 2 * (0.5 * min_dist ** 2 + 0.5 * dist_final ** 2)
|
||||||
|
reward = np.exp(-2 * min_dist) - 1e-5 * action_cost
|
||||||
|
success = dist_final < 0.05 and min_dist < 0.05
|
||||||
|
else:
|
||||||
|
cost = 0
|
||||||
|
reward = - 1e-5 * action_cost
|
||||||
|
success = False
|
||||||
|
# action_cost = np.mean(np.sum(np.square(torques), axis=1), axis=0)
|
||||||
|
|
||||||
|
return reward, success, False
|
||||||
|
|
||||||
|
def get_stage_wise_cost(self, ball_in_cup, min_dist, dist_final): #, dist_to_ctxt):
|
||||||
|
# stop_sim = False
|
||||||
|
cost = 3 + 2 * (0.5 * min_dist ** 2 + 0.5 * dist_final ** 2)
|
||||||
|
# if not ball_in_cup:
|
||||||
|
# # cost = 3 + 2*(0.5 * min_dist + 0.5 * dist_final)
|
||||||
|
# cost = 3 + 2*(0.5 * min_dist**2 + 0.5 * dist_final**2)
|
||||||
|
# else:
|
||||||
|
# # cost = 2*dist_to_ctxt
|
||||||
|
# cost = 2*dist_to_ctxt**2
|
||||||
|
# stop_sim = True
|
||||||
|
# # print(dist_to_ctxt-0.02)
|
||||||
|
# print('Context Distance:', dist_to_ctxt)
|
||||||
|
return cost
|
||||||
|
|
||||||
|
def _get_cost(self, ball_pos, goal_pos, goal_pos_final, u, ctxt, t):
|
||||||
|
|
||||||
|
cost = 0
|
||||||
|
if t == self.sim_time*0.8:
|
||||||
|
dist = 0.5*np.linalg.norm(goal_pos-ball_pos)**2 + 0.5*np.linalg.norm(goal_pos_final-ball_pos)**2
|
||||||
|
# dist_ctxt = np.linalg.norm(ctxt-goal_pos)**2
|
||||||
|
cost = dist # +dist_ctxt
|
||||||
|
return cost
|
||||||
|
|
||||||
|
def check_ball_in_cup(self, sim, ball_collision_id):
|
||||||
|
cup_base_collision_id = sim.model._geom_name2id["cup_base_contact"]
|
||||||
|
for coni in range(0, sim.data.ncon):
|
||||||
|
con = sim.data.contact[coni]
|
||||||
|
|
||||||
|
collision = con.geom1 == cup_base_collision_id and con.geom2 == ball_collision_id
|
||||||
|
collision_trans = con.geom1 == ball_collision_id and con.geom2 == cup_base_collision_id
|
||||||
|
|
||||||
|
if collision or collision_trans:
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
def check_collision(self, sim):
|
||||||
|
for coni in range(0, sim.data.ncon):
|
||||||
|
con = sim.data.contact[coni]
|
||||||
|
|
||||||
|
collision = con.geom1 in self.collision_ids and con.geom2 == self.ball_collision_id
|
||||||
|
collision_trans = con.geom1 == self.ball_collision_id and con.geom2 in self.collision_ids
|
||||||
|
|
||||||
|
if collision or collision_trans:
|
||||||
|
return True
|
||||||
|
return False
|
||||||
210
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_simple.py
Normal file
210
alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_simple.py
Normal file
@ -0,0 +1,210 @@
|
|||||||
|
from gym.envs.mujoco import mujoco_env
|
||||||
|
from gym import utils, spaces
|
||||||
|
import os
|
||||||
|
import numpy as np
|
||||||
|
from alr_envs.mujoco.ball_in_a_cup.ball_in_a_cup_reward import BallInACupReward
|
||||||
|
import mujoco_py
|
||||||
|
|
||||||
|
|
||||||
|
class ALRBallInACupEnv(mujoco_env.MujocoEnv, utils.EzPickle):
|
||||||
|
def __init__(self, pd_control=True):
|
||||||
|
self._steps = 0
|
||||||
|
self.pd_control = pd_control
|
||||||
|
|
||||||
|
self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets",
|
||||||
|
"ball-in-a-cup_base" + ".xml")
|
||||||
|
|
||||||
|
self.sim_time = 8 # seconds
|
||||||
|
self.sim_steps = int(self.sim_time / (0.0005 * 4)) # circular dependency.. sim.dt <-> mujocoenv init <-> reward fct
|
||||||
|
self.reward_function = BallInACupReward(self.sim_steps)
|
||||||
|
|
||||||
|
self.start_pos = np.array([0.0, 0.58760536, 0.0, 1.36004913, 0.0, -0.32072943, -1.57])
|
||||||
|
self.start_vel = np.zeros(7)
|
||||||
|
# self.start_pos = np.array([0.58760536, 1.36004913, -0.32072943])
|
||||||
|
self._q_pos = []
|
||||||
|
self._q_vel = []
|
||||||
|
# self.weight_matrix_scale = 50
|
||||||
|
self.p_gains = 1*np.array([200, 300, 100, 100, 10, 10, 2.5])
|
||||||
|
self.d_gains = 1*np.array([7, 15, 5, 2.5, 0.3, 0.3, 0.05])
|
||||||
|
|
||||||
|
self.j_min = np.array([-2.6, -1.985, -2.8, -0.9, -4.55, -1.5707, -2.7])
|
||||||
|
self.j_max = np.array([2.6, 1.985, 2.8, 3.14159, 1.25, 1.5707, 2.7])
|
||||||
|
|
||||||
|
utils.EzPickle.__init__(self)
|
||||||
|
mujoco_env.MujocoEnv.__init__(self, os.path.join(os.path.dirname(__file__), "assets", "ball-in-a-cup_base.xml"),
|
||||||
|
frame_skip=4)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def current_pos(self):
|
||||||
|
return self.sim.data.qpos[0:7].copy()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def current_vel(self):
|
||||||
|
return self.sim.data.qvel[0:7].copy()
|
||||||
|
|
||||||
|
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)
|
||||||
|
ball_id = self.sim.model._body_name2id["ball"]
|
||||||
|
goal_final_id = self.sim.model._site_name2id["cup_goal_final"]
|
||||||
|
# self.set_state(start_pos, start_vel)
|
||||||
|
self._steps = 0
|
||||||
|
self.reward_function.reset()
|
||||||
|
self._q_pos = []
|
||||||
|
self._q_vel = []
|
||||||
|
|
||||||
|
# Reset the system
|
||||||
|
self.sim.data.qpos[:] = init_pos_all
|
||||||
|
self.sim.data.qvel[:] = init_vel
|
||||||
|
self.sim.data.qpos[0:7] = init_pos_robot
|
||||||
|
|
||||||
|
self.sim.step()
|
||||||
|
|
||||||
|
self.sim.data.qpos[:] = init_pos_all
|
||||||
|
self.sim.data.qvel[:] = init_vel
|
||||||
|
self.sim.data.qpos[0:7] = init_pos_robot
|
||||||
|
self.sim.data.body_xpos[ball_id, :] = np.copy(self.sim.data.site_xpos[goal_final_id, :]) - np.array([0., 0., 0.329])
|
||||||
|
|
||||||
|
# Stabilize the system around the initial position
|
||||||
|
for i in range(0, 2000):
|
||||||
|
self.sim.data.qpos[7:] = 0.
|
||||||
|
self.sim.data.qvel[7:] = 0.
|
||||||
|
# self.sim.data.qpos[7] = -0.2
|
||||||
|
cur_pos = self.sim.data.qpos[0:7].copy()
|
||||||
|
cur_vel = self.sim.data.qvel[0:7].copy()
|
||||||
|
trq = self.p_gains * (init_pos_robot - cur_pos) + self.d_gains * (np.zeros_like(init_pos_robot) - cur_vel)
|
||||||
|
self.sim.data.qfrc_applied[0:7] = trq + self.sim.data.qfrc_bias[:7].copy()
|
||||||
|
self.sim.step()
|
||||||
|
# self.render()
|
||||||
|
|
||||||
|
for i in range(0, 2000):
|
||||||
|
cur_pos = self.sim.data.qpos[0:7].copy()
|
||||||
|
cur_vel = self.sim.data.qvel[0:7].copy()
|
||||||
|
trq = self.p_gains * (init_pos_robot - cur_pos) + self.d_gains * (np.zeros_like(init_pos_robot) - cur_vel)
|
||||||
|
self.sim.data.qfrc_applied[0:7] = trq + self.sim.data.qfrc_bias[:7].copy()
|
||||||
|
self.sim.step()
|
||||||
|
# self.render()
|
||||||
|
|
||||||
|
def do_simulation(self, ctrl, n_frames):
|
||||||
|
# cur_pos = self.sim.data.qpos[0:7].copy()
|
||||||
|
# cur_vel = self.sim.data.qvel[0:7].copy()
|
||||||
|
# des_pos = ctrl[:7]
|
||||||
|
# des_vel = ctrl[7:]
|
||||||
|
# trq = self.p_gains * (des_pos - cur_pos) + self.d_gains * (des_vel - cur_vel)
|
||||||
|
if self.pd_control:
|
||||||
|
self.sim.data.qfrc_applied[0:7] = ctrl + self.sim.data.qfrc_bias[:7].copy()
|
||||||
|
else:
|
||||||
|
self.sim.data.ctrl[:] = ctrl
|
||||||
|
|
||||||
|
for _ in range(n_frames):
|
||||||
|
try:
|
||||||
|
self.sim.step()
|
||||||
|
except mujoco_py.builder.MujocoException as e:
|
||||||
|
print("Error in simulation: " + str(e))
|
||||||
|
# error = True
|
||||||
|
# Copy the current torque as if it would have been applied until the end of the trajectory
|
||||||
|
# for i in range(k + 1, sim_time):
|
||||||
|
# torques.append(trq)
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def step(self, a):
|
||||||
|
# Apply gravity compensation
|
||||||
|
# if not np.all(self.sim.data.qfrc_applied[:7] == self.sim.data.qfrc_bias[:7]):
|
||||||
|
# self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
||||||
|
|
||||||
|
reward_dist = 0.0
|
||||||
|
angular_vel = 0.0
|
||||||
|
# if self._steps >= self.steps_before_reward:
|
||||||
|
# vec = self.get_body_com("fingertip") - self.get_body_com("target")
|
||||||
|
# reward_dist -= self.reward_weight * np.linalg.norm(vec)
|
||||||
|
# angular_vel -= np.linalg.norm(self.sim.data.qvel.flat[:self.n_links])
|
||||||
|
reward_ctrl = - np.square(a).sum()
|
||||||
|
# reward_balance = - self.balance_weight * np.abs(
|
||||||
|
# angle_normalize(np.sum(self.sim.data.qpos.flat[:self.n_links]), type="rad"))
|
||||||
|
#
|
||||||
|
# reward = reward_dist + reward_ctrl + angular_vel + reward_balance
|
||||||
|
# self.do_simulation(a, self.frame_skip)
|
||||||
|
|
||||||
|
joint_cons_viol = self.check_traj_in_joint_limits()
|
||||||
|
|
||||||
|
crash = self.do_simulation(a, self.frame_skip)
|
||||||
|
|
||||||
|
self._q_pos.append(self.sim.data.qpos[0:7].ravel().copy())
|
||||||
|
self._q_vel.append(self.sim.data.qvel[0:7].ravel().copy())
|
||||||
|
|
||||||
|
ob = self._get_obs()
|
||||||
|
|
||||||
|
if not crash and not joint_cons_viol:
|
||||||
|
reward, success, collision = self.reward_function.compute_reward(a, self.sim, self._steps)
|
||||||
|
done = success or self._steps == self.sim_steps - 1 or collision
|
||||||
|
self._steps += 1
|
||||||
|
else:
|
||||||
|
reward = -1000
|
||||||
|
done = True
|
||||||
|
return ob, reward, done, dict(reward_dist=reward_dist, reward_ctrl=reward_ctrl,
|
||||||
|
velocity=angular_vel, # reward_balance=reward_balance,
|
||||||
|
# end_effector=self.get_body_com("fingertip").copy(),
|
||||||
|
goal=self.goal if hasattr(self, "goal") else None,
|
||||||
|
traj=self._q_pos)
|
||||||
|
|
||||||
|
def check_traj_in_joint_limits(self):
|
||||||
|
return any(self.current_pos > self.j_max) or any(self.current_pos < self.j_min)
|
||||||
|
|
||||||
|
def check_collision(self):
|
||||||
|
for coni in range(0, sim.data.ncon):
|
||||||
|
con = sim.data.contact[coni]
|
||||||
|
|
||||||
|
collision = con.geom1 in self.collision_ids and con.geom2 == self.ball_collision_id
|
||||||
|
collision_trans = con.geom1 == self.ball_collision_id and con.geom2 in self.collision_ids
|
||||||
|
|
||||||
|
if collision or collision_trans:
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _get_obs(self):
|
||||||
|
theta = self.sim.data.qpos.flat[:7]
|
||||||
|
return np.concatenate([
|
||||||
|
np.cos(theta),
|
||||||
|
np.sin(theta),
|
||||||
|
# self.get_body_com("target"), # only return target to make problem harder
|
||||||
|
[self._steps],
|
||||||
|
])
|
||||||
|
|
||||||
|
def extend_des_pos(self, des_pos):
|
||||||
|
des_pos_full = self.start_pos.copy()
|
||||||
|
des_pos_full[1] = des_pos[0]
|
||||||
|
des_pos_full[3] = des_pos[1]
|
||||||
|
des_pos_full[5] = des_pos[2]
|
||||||
|
return des_pos_full
|
||||||
|
|
||||||
|
def extend_des_vel(self, des_vel):
|
||||||
|
des_vel_full = self.start_vel.copy()
|
||||||
|
des_vel_full[1] = des_vel[0]
|
||||||
|
des_vel_full[3] = des_vel[1]
|
||||||
|
des_vel_full[5] = des_vel[2]
|
||||||
|
return des_vel_full
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
env = ALRBallInACupEnv()
|
||||||
|
env.reset()
|
||||||
|
env.render()
|
||||||
|
for i in range(4000):
|
||||||
|
# objective.load_result("/tmp/cma")
|
||||||
|
# test with random actions
|
||||||
|
# ac = 0.1 * env.action_space.sample()
|
||||||
|
# ac = -np.array([i, i, i]) / 10000 + np.array([env.start_pos[1], env.start_pos[3], env.start_pos[5]])
|
||||||
|
ac = np.array([0., -0.1, 0, 0, 0, 0, 0])
|
||||||
|
# ac[0] += np.pi/2
|
||||||
|
obs, rew, d, info = env.step(ac)
|
||||||
|
env.render()
|
||||||
|
|
||||||
|
print(rew)
|
||||||
|
|
||||||
|
if d:
|
||||||
|
break
|
||||||
|
|
||||||
|
env.close()
|
||||||
|
|
||||||
3
alr_envs/mujoco/gym_table_tennis/envs/MUJOCO_LOG.TXT
Normal file
3
alr_envs/mujoco/gym_table_tennis/envs/MUJOCO_LOG.TXT
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
Mon Jan 25 15:45:30 2021
|
||||||
|
ERROR: GLEW initalization error: Missing GL version
|
||||||
|
|
||||||
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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -3,7 +3,7 @@ from gym import spaces
|
|||||||
from gym.envs.robotics import robot_env, utils
|
from gym.envs.robotics import robot_env, utils
|
||||||
# import xml.etree.ElementTree as ET
|
# import xml.etree.ElementTree as ET
|
||||||
from alr_envs.mujoco.gym_table_tennis.utils.rewards.hierarchical_reward import HierarchicalRewardTableTennis
|
from alr_envs.mujoco.gym_table_tennis.utils.rewards.hierarchical_reward import HierarchicalRewardTableTennis
|
||||||
# import glfw
|
import glfw
|
||||||
from alr_envs.mujoco.gym_table_tennis.utils.experiment import ball_initialize
|
from alr_envs.mujoco.gym_table_tennis.utils.experiment import ball_initialize
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
import os
|
import os
|
||||||
@ -34,18 +34,26 @@ class TableTennisEnv(robot_env.RobotEnv):
|
|||||||
path_cws = Path.cwd()
|
path_cws = Path.cwd()
|
||||||
print(path_cws)
|
print(path_cws)
|
||||||
current_dir = Path(os.path.split(os.path.realpath(__file__))[0])
|
current_dir = Path(os.path.split(os.path.realpath(__file__))[0])
|
||||||
table_tennis_env_xml_path = current_dir / "robotics"/"assets"/"table_tennis"/"table_tennis_env.xml"
|
table_tennis_env_xml_path = current_dir / "assets"/"table_tennis_env.xml"
|
||||||
model_path = str(table_tennis_env_xml_path)
|
model_path = str(table_tennis_env_xml_path)
|
||||||
self.config = config
|
self.config = config
|
||||||
action_space = self.config['trajectory']['args']['action_space']
|
action_space = True # self.config['trajectory']['args']['action_space']
|
||||||
time_step = self.config['mujoco_sim_env']['args']["time_step"]
|
time_step = 0.002 # self.config['mujoco_sim_env']['args']["time_step"]
|
||||||
if initial_qpos is None:
|
if initial_qpos is None:
|
||||||
initial_qpos = self.config['robot_config']['args']['initial_qpos']
|
initial_qpos = {"wam/base_yaw_joint_right": 1.5,
|
||||||
|
"wam/shoulder_pitch_joint_right": 1,
|
||||||
|
"wam/shoulder_yaw_joint_right": 0,
|
||||||
|
"wam/elbow_pitch_joint_right": 1,
|
||||||
|
"wam/wrist_yaw_joint_right": 1,
|
||||||
|
"wam/wrist_pitch_joint_right": 0,
|
||||||
|
"wam/palm_yaw_joint_right": 0}
|
||||||
|
# initial_qpos = [1.5, 1, 0, 1, 1, 0, 0] # self.config['robot_config']['args']['initial_qpos']
|
||||||
|
|
||||||
# TODO should read all configuration in config
|
# TODO should read all configuration in config
|
||||||
assert initial_qpos is not None, "Must initialize the initial q position of robot arm"
|
assert initial_qpos is not None, "Must initialize the initial q position of robot arm"
|
||||||
n_actions = 7
|
n_actions = 7
|
||||||
self.initial_qpos_value = np.array(list(initial_qpos.values())).copy()
|
self.initial_qpos_value = np.array(list(initial_qpos.values())).copy()
|
||||||
|
# self.initial_qpos_value = np.array(initial_qpos)
|
||||||
# # change time step in .xml file
|
# # change time step in .xml file
|
||||||
# tree = ET.parse(model_path)
|
# tree = ET.parse(model_path)
|
||||||
# root = tree.getroot()
|
# root = tree.getroot()
|
||||||
@ -71,6 +79,7 @@ class TableTennisEnv(robot_env.RobotEnv):
|
|||||||
self.n_actions = n_actions
|
self.n_actions = n_actions
|
||||||
self.action = None
|
self.action = None
|
||||||
self.time_step = time_step
|
self.time_step = time_step
|
||||||
|
self._dt = time_step
|
||||||
self.paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
self.paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
||||||
if reward_obj is None:
|
if reward_obj is None:
|
||||||
self.reward_obj = HierarchicalRewardTableTennis()
|
self.reward_obj = HierarchicalRewardTableTennis()
|
||||||
@ -104,7 +113,8 @@ class TableTennisEnv(robot_env.RobotEnv):
|
|||||||
self.reward_obj.hitting(self)
|
self.reward_obj.hitting(self)
|
||||||
# if not hitted, return the highest reward
|
# if not hitted, return the highest reward
|
||||||
if not self.reward_obj.goal_achievement:
|
if not self.reward_obj.goal_achievement:
|
||||||
return self.reward_obj.highest_reward
|
# return self.reward_obj.highest_reward
|
||||||
|
return self.reward_obj.total_reward
|
||||||
# # Stage 2 Right Table Contact
|
# # Stage 2 Right Table Contact
|
||||||
# self.reward_obj.right_table_contact(self)
|
# self.reward_obj.right_table_contact(self)
|
||||||
# if not self.reward_obj.goal_achievement:
|
# if not self.reward_obj.goal_achievement:
|
||||||
@ -119,7 +129,8 @@ class TableTennisEnv(robot_env.RobotEnv):
|
|||||||
# print("self.reward_obj.highest_reward: ", self.reward_obj.highest_reward)
|
# print("self.reward_obj.highest_reward: ", self.reward_obj.highest_reward)
|
||||||
# TODO
|
# TODO
|
||||||
self.reward_obj.target_achievement(self)
|
self.reward_obj.target_achievement(self)
|
||||||
return self.reward_obj.highest_reward
|
# return self.reward_obj.highest_reward
|
||||||
|
return self.reward_obj.total_reward
|
||||||
|
|
||||||
def _reset_sim(self):
|
def _reset_sim(self):
|
||||||
self.sim.set_state(self.initial_state)
|
self.sim.set_state(self.initial_state)
|
||||||
@ -217,10 +228,10 @@ if __name__ == '__main__':
|
|||||||
env.reset()
|
env.reset()
|
||||||
# env.render(mode=render_mode)
|
# env.render(mode=render_mode)
|
||||||
|
|
||||||
for i in range(200):
|
for i in range(500):
|
||||||
# objective.load_result("/tmp/cma")
|
# objective.load_result("/tmp/cma")
|
||||||
# test with random actions
|
# test with random actions
|
||||||
ac = 2 * env.action_space.sample()
|
ac = env.action_space.sample()
|
||||||
# ac[0] += np.pi/2
|
# ac[0] += np.pi/2
|
||||||
obs, rew, d, info = env.step(ac)
|
obs, rew, d, info = env.step(ac)
|
||||||
env.render(mode=render_mode)
|
env.render(mode=render_mode)
|
||||||
|
|||||||
0
alr_envs/mujoco/reacher/__init__.py
Normal file
0
alr_envs/mujoco/reacher/__init__.py
Normal file
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user