diff --git a/alr_envs/mujoco/__init__.py b/alr_envs/mujoco/__init__.py
index 77588f7..e0ecd85 100644
--- a/alr_envs/mujoco/__init__.py
+++ b/alr_envs/mujoco/__init__.py
@@ -1 +1 @@
-from alr_envs.mujoco.alr_reacher import ALRReacherEnv
\ No newline at end of file
+from alr_envs.mujoco.reacher.alr_reacher import ALRReacherEnv
\ No newline at end of file
diff --git a/alr_envs/mujoco/ball_in_a_cup/__init__.py b/alr_envs/mujoco/ball_in_a_cup/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/ball-in-a-cup_base.xml b/alr_envs/mujoco/ball_in_a_cup/assets/ball-in-a-cup_base.xml
new file mode 100644
index 0000000..b534205
--- /dev/null
+++ b/alr_envs/mujoco/ball_in_a_cup/assets/ball-in-a-cup_base.xml
@@ -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>
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/base_link_convex.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/base_link_convex.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/base_link_convex.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/base_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/base_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/base_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/base_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/base_link_fine.stl
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup.stl
new file mode 100644
index 0000000..bc34058
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split1.stl
new file mode 100644
index 0000000..c80aa61
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split1.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split10.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split10.stl
new file mode 100644
index 0000000..bd5708b
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split10.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split11.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split11.stl
new file mode 100644
index 0000000..ac81da2
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split11.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split12.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split12.stl
new file mode 100644
index 0000000..a18e96e
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split12.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split13.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split13.stl
new file mode 100644
index 0000000..f0e5832
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split13.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split14.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split14.stl
new file mode 100644
index 0000000..41a3e94
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split14.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split15.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split15.stl
new file mode 100644
index 0000000..7a26643
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split15.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split16.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split16.stl
new file mode 100644
index 0000000..155b24e
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split16.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split17.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split17.stl
new file mode 100644
index 0000000..2fe8d95
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split17.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split18.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split18.stl
new file mode 100644
index 0000000..f5287b2
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split18.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split2.stl
new file mode 100644
index 0000000..5c1e50c
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split2.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split3.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split3.stl
new file mode 100644
index 0000000..ef6d547
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split3.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split4.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split4.stl
new file mode 100644
index 0000000..5476296
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split4.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split5.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split5.stl
new file mode 100644
index 0000000..ccfcd42
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split5.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split6.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split6.stl
new file mode 100644
index 0000000..72d6287
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split6.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split7.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split7.stl
new file mode 100644
index 0000000..d4918f2
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split7.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split8.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split8.stl
new file mode 100644
index 0000000..8a0cd84
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split8.stl differ
diff --git a/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split9.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split9.stl
new file mode 100644
index 0000000..4281a69
Binary files /dev/null and b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/cup_split9.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/elbow_link_convex.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/elbow_link_convex.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/elbow_link_convex.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/elbow_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/elbow_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/elbow_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/elbow_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/elbow_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_convex_decomposition_p1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_convex_decomposition_p1.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_convex_decomposition_p2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_convex_decomposition_p2.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/forearm_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/forearm_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p1.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p2.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p3.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p3.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_convex_decomposition_p3.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_convex_decomposition_p3.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_pitch_link_convex.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_pitch_link_convex.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_pitch_link_convex.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_pitch_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_pitch_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_pitch_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/shoulder_pitch_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/shoulder_pitch_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_convex_decomposition_p1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_convex_decomposition_p1.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_convex_decomposition_p2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_convex_decomposition_p2.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/upper_arm_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/upper_arm_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_palm_link_convex.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_palm_link_convex.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_palm_link_convex.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_palm_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_palm_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_palm_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_palm_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_palm_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p1.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p2.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p3.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p3.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_convex_decomposition_p3.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_convex_decomposition_p3.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_pitch_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_pitch_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_convex_decomposition_p1.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_convex_decomposition_p1.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_convex_decomposition_p2.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_convex_decomposition_p2.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_fine.stl b/alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_fine.stl
old mode 100644
new mode 100755
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/wrist_yaw_link_fine.stl
rename to alr_envs/mujoco/ball_in_a_cup/assets/meshes/wrist_yaw_link_fine.stl
diff --git a/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup.py b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup.py
new file mode 100644
index 0000000..093e7f1
--- /dev/null
+++ b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup.py
@@ -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()
+
diff --git a/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_reward.py b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_reward.py
new file mode 100644
index 0000000..69932db
--- /dev/null
+++ b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_reward.py
@@ -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
diff --git a/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_simple.py b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_simple.py
new file mode 100644
index 0000000..9161f17
--- /dev/null
+++ b/alr_envs/mujoco/ball_in_a_cup/ball_in_a_cup_simple.py
@@ -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()
+
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/MUJOCO_LOG.TXT b/alr_envs/mujoco/gym_table_tennis/envs/MUJOCO_LOG.TXT
new file mode 100644
index 0000000..91c2162
--- /dev/null
+++ b/alr_envs/mujoco/gym_table_tennis/envs/MUJOCO_LOG.TXT
@@ -0,0 +1,3 @@
+Mon Jan 25 15:45:30 2021
+ERROR: GLEW initalization error: Missing GL version
+
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_7_motor_actuator.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_7_motor_actuator.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_7_motor_actuator.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_7_motor_actuator.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_barrett_wam_7dof_left.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_barrett_wam_7dof_left.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_barrett_wam_7dof_left.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_barrett_wam_7dof_left.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_barrett_wam_7dof_right.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_barrett_wam_7dof_right.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_barrett_wam_7dof_right.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_barrett_wam_7dof_right.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_table.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_table.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_table.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_table.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_target_ball.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_target_ball.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_target_ball.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_target_ball.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_test_balls.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/include_test_balls.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/include_test_balls.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/include_test_balls.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_convex.stl
new file mode 100644
index 0000000..133b112
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_convex.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_fine.stl
new file mode 100644
index 0000000..047e9df
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/base_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_dist_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_dist_link_convex.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_dist_link_convex.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_dist_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_dist_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_dist_link_fine.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_dist_link_fine.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_dist_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_med_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_med_link_convex.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_med_link_convex.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_med_link_convex.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_med_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_med_link_fine.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_med_link_fine.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_med_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p1.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p2.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p3.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p3.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_convex_decomposition_p3.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_convex_decomposition_p3.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_fine.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_finger_prox_link_fine.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_finger_prox_link_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_fine.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_fine.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_fine.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p1.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p1.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p1.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p2.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p2.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p2.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p3.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p3.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p3.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p3.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p4.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p4.stl
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/meshes/bhand_palm_link_convex_decomposition_p4.stl
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/bhand_palm_link_convex_decomposition_p4.stl
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_convex.stl
new file mode 100644
index 0000000..b34963d
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_convex.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_fine.stl
new file mode 100644
index 0000000..f6a1515
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/elbow_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p1.stl
new file mode 100644
index 0000000..e6aa6b6
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p1.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p2.stl
new file mode 100644
index 0000000..667902e
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_convex_decomposition_p2.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_fine.stl
new file mode 100644
index 0000000..ed66bbb
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/forearm_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p1.stl
new file mode 100644
index 0000000..aba957d
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p1.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p2.stl
new file mode 100644
index 0000000..5cca6a9
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p2.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p3.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p3.stl
new file mode 100644
index 0000000..3343e27
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_convex_decomposition_p3.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_fine.stl
new file mode 100644
index 0000000..ae505fd
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_convex.stl
new file mode 100644
index 0000000..c36cfec
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_convex.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_fine.stl
new file mode 100644
index 0000000..dc633c4
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/shoulder_pitch_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p1.stl
new file mode 100644
index 0000000..82d0093
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p1.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p2.stl
new file mode 100644
index 0000000..7fd5a55
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_convex_decomposition_p2.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_fine.stl
new file mode 100644
index 0000000..76353ae
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/upper_arm_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_convex.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_convex.stl
new file mode 100644
index 0000000..a0386f6
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_convex.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_fine.stl
new file mode 100644
index 0000000..f6b41ad
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_palm_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p1.stl
new file mode 100644
index 0000000..c36f88f
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p1.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p2.stl
new file mode 100644
index 0000000..d00cac1
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p2.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p3.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p3.stl
new file mode 100644
index 0000000..34d1d8b
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_convex_decomposition_p3.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_fine.stl
new file mode 100644
index 0000000..13d2f73
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_pitch_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p1.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p1.stl
new file mode 100644
index 0000000..06e857f
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p1.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p2.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p2.stl
new file mode 100644
index 0000000..48e1bb1
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_convex_decomposition_p2.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_fine.stl b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_fine.stl
new file mode 100644
index 0000000..0d95239
Binary files /dev/null and b/alr_envs/mujoco/gym_table_tennis/envs/assets/meshes/wrist_yaw_link_fine.stl differ
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/right_arm_actuator.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/right_arm_actuator.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/right_arm_actuator.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/right_arm_actuator.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/shared.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/shared.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/shared.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/shared.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/table_tennis_env.xml b/alr_envs/mujoco/gym_table_tennis/envs/assets/table_tennis_env.xml
similarity index 100%
rename from alr_envs/mujoco/gym_table_tennis/envs/robotics/assets/table_tennis/table_tennis_env.xml
rename to alr_envs/mujoco/gym_table_tennis/envs/assets/table_tennis_env.xml
diff --git a/alr_envs/mujoco/gym_table_tennis/envs/table_tennis_env.py b/alr_envs/mujoco/gym_table_tennis/envs/table_tennis_env.py
index 625a122..f155f06 100644
--- a/alr_envs/mujoco/gym_table_tennis/envs/table_tennis_env.py
+++ b/alr_envs/mujoco/gym_table_tennis/envs/table_tennis_env.py
@@ -3,7 +3,7 @@ from gym import spaces
 from gym.envs.robotics import robot_env, utils
 # import xml.etree.ElementTree as ET
 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 pathlib import Path
 import os
@@ -34,18 +34,26 @@ class TableTennisEnv(robot_env.RobotEnv):
             path_cws = Path.cwd()
             print(path_cws)
             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)
         self.config = config
-        action_space = self.config['trajectory']['args']['action_space']
-        time_step = self.config['mujoco_sim_env']['args']["time_step"]
+        action_space = True  # self.config['trajectory']['args']['action_space']
+        time_step = 0.002  # self.config['mujoco_sim_env']['args']["time_step"]
         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
         assert initial_qpos is not None, "Must initialize the initial q position of robot arm"
         n_actions = 7
         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
         # tree = ET.parse(model_path)
         # root = tree.getroot()
@@ -71,6 +79,7 @@ class TableTennisEnv(robot_env.RobotEnv):
         self.n_actions = n_actions
         self.action = None
         self.time_step = time_step
+        self._dt = time_step
         self.paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
         if reward_obj is None:
             self.reward_obj = HierarchicalRewardTableTennis()
@@ -104,7 +113,8 @@ class TableTennisEnv(robot_env.RobotEnv):
         self.reward_obj.hitting(self)
         # if not hitted, return the highest reward
         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
         # self.reward_obj.right_table_contact(self)
         # 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)
         # TODO
         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):
         self.sim.set_state(self.initial_state)
@@ -217,10 +228,10 @@ if __name__ == '__main__':
     env.reset()
     # env.render(mode=render_mode)
 
-    for i in range(200):
+    for i in range(500):
         # objective.load_result("/tmp/cma")
         # test with random actions
-        ac = 2 * env.action_space.sample()
+        ac = env.action_space.sample()
         # ac[0] += np.pi/2
         obs, rew, d, info = env.step(ac)
         env.render(mode=render_mode)
diff --git a/alr_envs/mujoco/reacher/__init__.py b/alr_envs/mujoco/reacher/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/alr_envs/mujoco/alr_reacher.py b/alr_envs/mujoco/reacher/alr_reacher.py
similarity index 80%
rename from alr_envs/mujoco/alr_reacher.py
rename to alr_envs/mujoco/reacher/alr_reacher.py
index 7ae28da..85adf58 100644
--- a/alr_envs/mujoco/alr_reacher.py
+++ b/alr_envs/mujoco/reacher/alr_reacher.py
@@ -29,9 +29,20 @@ class ALRReacherEnv(mujoco_env.MujocoEnv, utils.EzPickle):
         else:
             raise ValueError(f"Invalid number of links {n_links}, only 5 or 7 allowed.")
 
+        self._q_pos = []
+        self._q_vel = []
+
         utils.EzPickle.__init__(self)
         mujoco_env.MujocoEnv.__init__(self, os.path.join(os.path.dirname(__file__), "assets", file_name), 2)
 
+    @property
+    def current_pos(self):
+        return self.sim.data.qpos[0:5].copy()
+
+    @property
+    def current_vel(self):
+        return self.sim.data.qvel[0:5].copy()
+
     def step(self, a):
         self._steps += 1
 
@@ -47,28 +58,36 @@ class ALRReacherEnv(mujoco_env.MujocoEnv, utils.EzPickle):
 
         reward = reward_dist + reward_ctrl + angular_vel + reward_balance
         self.do_simulation(a, self.frame_skip)
+
+        self._q_pos.append(self.sim.data.qpos[0:5].ravel().copy())
+        self._q_vel.append(self.sim.data.qvel[0:5].ravel().copy())
+
         ob = self._get_obs()
         done = False
         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)
+                                      goal=self.goal if hasattr(self, "goal") else None,
+                                      traj=self._q_pos, vel=self._q_vel)
 
     def viewer_setup(self):
         self.viewer.cam.trackbodyid = 0
 
     def reset_model(self):
-        qpos = self.np_random.uniform(low=-0.1, high=0.1, size=self.model.nq) + self.init_qpos
+        qpos = self.init_qpos  # self.np_random.uniform(low=-0.1, high=0.1, size=self.model.nq) + self.init_qpos
         while True:
             self.goal = self.np_random.uniform(low=-self.n_links / 10, high=self.n_links / 10, size=2)
             if np.linalg.norm(self.goal) < self.n_links / 10:
                 break
         qpos[-2:] = self.goal
-        qvel = self.init_qvel + self.np_random.uniform(low=-.005, high=.005, size=self.model.nv)
+        qvel = self.init_qvel  # + self.np_random.uniform(low=-.005, high=.005, size=self.model.nv)
         qvel[-2:] = 0
         self.set_state(qpos, qvel)
         self._steps = 0
 
+        self._q_pos = []
+        self._q_vel = []
+
         return self._get_obs()
 
     def _get_obs(self):
diff --git a/alr_envs/mujoco/assets/reacher_5links.xml b/alr_envs/mujoco/reacher/assets/reacher_5links.xml
similarity index 100%
rename from alr_envs/mujoco/assets/reacher_5links.xml
rename to alr_envs/mujoco/reacher/assets/reacher_5links.xml
diff --git a/alr_envs/mujoco/assets/reacher_7links.xml b/alr_envs/mujoco/reacher/assets/reacher_7links.xml
similarity index 100%
rename from alr_envs/mujoco/assets/reacher_7links.xml
rename to alr_envs/mujoco/reacher/assets/reacher_7links.xml
diff --git a/alr_envs/utils/detpmp_env_wrapper.py b/alr_envs/utils/detpmp_env_wrapper.py
new file mode 100644
index 0000000..64986a5
--- /dev/null
+++ b/alr_envs/utils/detpmp_env_wrapper.py
@@ -0,0 +1,88 @@
+from mp_lib import det_promp
+import numpy as np
+import gym
+
+
+class DetPMPEnvWrapperBase(gym.Wrapper):
+    def __init__(self,
+                 env,
+                 num_dof,
+                 num_basis,
+                 width,
+                 start_pos=None,
+                 duration=1,
+                 dt=0.01,
+                 post_traj_time=0.,
+                 policy=None,
+                 weights_scale=1):
+        super(DetPMPEnvWrapperBase, self).__init__(env)
+        self.num_dof = num_dof
+        self.num_basis = num_basis
+        self.dim = num_dof * num_basis
+        self.pmp = det_promp.DeterministicProMP(n_basis=num_basis, width=width, off=0.01)
+        weights = np.zeros(shape=(num_basis, num_dof))
+        self.pmp.set_weights(duration, weights)
+        self.weights_scale = weights_scale
+
+        self.duration = duration
+        self.dt = dt
+        self.post_traj_steps = int(post_traj_time / dt)
+
+        self.start_pos = start_pos
+
+        self.policy = policy
+
+    def __call__(self, params):
+        params = np.atleast_2d(params)
+        observations = []
+        rewards = []
+        dones = []
+        infos = []
+        for p in params:
+            observation, reward, done, info = self.rollout(p)
+            observations.append(observation)
+            rewards.append(reward)
+            dones.append(done)
+            infos.append(info)
+
+        return np.array(rewards), infos
+
+    def rollout(self, params, render=False):
+        """ This function generates a trajectory based on a DMP and then does the usual loop over reset and step"""
+        raise NotImplementedError
+
+
+class DetPMPEnvWrapperPD(DetPMPEnvWrapperBase):
+    """
+    Wrapper for gym environments which creates a trajectory in joint velocity space
+    """
+    def rollout(self, params, render=False):
+        params = np.reshape(params, newshape=(self.num_basis, self.num_dof)) * self.weights_scale
+        self.pmp.set_weights(self.duration, params)
+        t, des_pos, des_vel, des_acc = self.pmp.compute_trajectory(1/self.dt, 1.)
+        des_pos += self.start_pos[None, :]
+
+        if self.post_traj_steps > 0:
+            des_pos = np.vstack([des_pos, np.tile(des_pos[-1, :], [self.post_traj_steps, 1])])
+            des_vel = np.vstack([des_vel, np.zeros(shape=(self.post_traj_steps, self.num_dof))])
+
+        self._trajectory = des_pos
+
+        rews = []
+        infos = []
+
+        self.env.reset()
+
+        for t, pos_vel in enumerate(zip(des_pos, des_vel)):
+            ac = self.policy.get_action(self.env, pos_vel[0], pos_vel[1])
+            obs, rew, done, info = self.env.step(ac)
+            rews.append(rew)
+            infos.append(info)
+            if render:
+                self.env.render(mode="human")
+            if done:
+                break
+
+        reward = np.sum(rews)
+
+        return obs, reward, done, info
diff --git a/alr_envs/utils/dmp_env_wrapper.py b/alr_envs/utils/dmp_env_wrapper.py
index 9b1bdf1..823df83 100644
--- a/alr_envs/utils/dmp_env_wrapper.py
+++ b/alr_envs/utils/dmp_env_wrapper.py
@@ -6,19 +6,31 @@ import gym
 
 
 class DmpEnvWrapperBase(gym.Wrapper):
-    def __init__(self, env, num_dof, num_basis, duration=1, dt=0.01, learn_goal=False):
+    def __init__(self,
+                 env,
+                 num_dof,
+                 num_basis,
+                 start_pos=None,
+                 final_pos=None,
+                 duration=1,
+                 alpha_phase=2,
+                 dt=0.01,
+                 learn_goal=False,
+                 post_traj_time=0.,
+                 policy=None):
         super(DmpEnvWrapperBase, self).__init__(env)
         self.num_dof = num_dof
         self.num_basis = num_basis
         self.dim = num_dof * num_basis
         if learn_goal:
             self.dim += num_dof
-        self.learn_goal = True
+        self.learn_goal = learn_goal
         self.duration = duration   # seconds
         time_steps = int(duration / dt)
         self.t = np.linspace(0, duration, time_steps)
+        self.post_traj_steps = int(post_traj_time / dt)
 
-        phase_generator = ExpDecayPhaseGenerator(alpha_phase=5, duration=duration)
+        phase_generator = ExpDecayPhaseGenerator(alpha_phase=alpha_phase, duration=duration)
         basis_generator = DMPBasisGenerator(phase_generator, duration=duration, num_basis=self.num_basis)
 
         self.dmp = dmps.DMP(num_dof=num_dof,
@@ -28,13 +40,18 @@ class DmpEnvWrapperBase(gym.Wrapper):
                             dt=dt
                             )
 
-        self.dmp.dmp_start_pos = env.start_pos.reshape((1, num_dof))
+        self.dmp.dmp_start_pos = start_pos.reshape((1, num_dof))
 
         dmp_weights = np.zeros((num_basis, num_dof))
-        dmp_goal_pos = np.zeros(num_dof)
+        if learn_goal:
+            dmp_goal_pos = np.zeros(num_dof)
+        else:
+            dmp_goal_pos = final_pos
 
         self.dmp.set_weights(dmp_weights, dmp_goal_pos)
 
+        self.policy = policy
+
     def __call__(self, params):
         params = np.atleast_2d(params)
         observations = []
@@ -48,7 +65,7 @@ class DmpEnvWrapperBase(gym.Wrapper):
             dones.append(done)
             infos.append(info)
 
-        return np.array(rewards)
+        return np.array(rewards), infos
 
     def goal_and_weights(self, params):
         if len(params.shape) > 1:
@@ -71,7 +88,7 @@ class DmpEnvWrapperBase(gym.Wrapper):
         raise NotImplementedError
 
 
-class DmpEnvWrapperAngle(DmpEnvWrapperBase):
+class DmpEnvWrapperPos(DmpEnvWrapperBase):
     """
     Wrapper for gym environments which creates a trajectory in joint angle space
     """
@@ -80,7 +97,12 @@ class DmpEnvWrapperAngle(DmpEnvWrapperBase):
         if hasattr(self.env, "weight_matrix_scale"):
             weight_matrix = weight_matrix * self.env.weight_matrix_scale
         self.dmp.set_weights(weight_matrix, goal_pos)
-        trajectory, velocities = self.dmp.reference_trajectory(self.t)
+        trajectory, _ = self.dmp.reference_trajectory(self.t)
+
+        if self.post_traj_steps > 0:
+            trajectory = np.vstack([trajectory, np.tile(trajectory[-1, :], [self.post_traj_steps, 1])])
+
+        self._trajectory = trajectory
 
         rews = []
 
@@ -95,8 +117,6 @@ class DmpEnvWrapperAngle(DmpEnvWrapperBase):
                 break
 
         reward = np.sum(rews)
-        # done = True
-        info = {}
 
         return obs, reward, done, info
 
@@ -110,7 +130,7 @@ class DmpEnvWrapperVel(DmpEnvWrapperBase):
         if hasattr(self.env, "weight_matrix_scale"):
             weight_matrix = weight_matrix * self.env.weight_matrix_scale
         self.dmp.set_weights(weight_matrix, goal_pos)
-        trajectory, velocities = self.dmp.reference_trajectory(self.t)
+        _, velocities = self.dmp.reference_trajectory(self.t)
 
         rews = []
         infos = []
@@ -129,3 +149,41 @@ class DmpEnvWrapperVel(DmpEnvWrapperBase):
         reward = np.sum(rews)
 
         return obs, reward, done, info
+
+
+class DmpEnvWrapperPD(DmpEnvWrapperBase):
+    """
+    Wrapper for gym environments which creates a trajectory in joint velocity space
+    """
+    def rollout(self, action, render=False):
+        goal_pos, weight_matrix = self.goal_and_weights(action)
+        if hasattr(self.env, "weight_matrix_scale"):
+            weight_matrix = weight_matrix * self.env.weight_matrix_scale
+        self.dmp.set_weights(weight_matrix, goal_pos)
+        trajectory, velocity = self.dmp.reference_trajectory(self.t)
+
+        if self.post_traj_steps > 0:
+            trajectory = np.vstack([trajectory, np.tile(trajectory[-1, :], [self.post_traj_steps, 1])])
+            velocity = np.vstack([velocity, np.zeros(shape=(self.post_traj_steps, self.num_dof))])
+
+        self._trajectory = trajectory
+        self._velocity = velocity
+
+        rews = []
+        infos = []
+
+        self.env.reset()
+
+        for t, pos_vel in enumerate(zip(trajectory, velocity)):
+            ac = self.policy.get_action(self.env, pos_vel[0], pos_vel[1])
+            obs, rew, done, info = self.env.step(ac)
+            rews.append(rew)
+            infos.append(info)
+            if render:
+                self.env.render(mode="human")
+            if done:
+                break
+
+        reward = np.sum(rews)
+
+        return obs, reward, done, info
diff --git a/alr_envs/utils/policies.py b/alr_envs/utils/policies.py
new file mode 100644
index 0000000..5d251fa
--- /dev/null
+++ b/alr_envs/utils/policies.py
@@ -0,0 +1,15 @@
+class PDController:
+    def __init__(self, p_gains, d_gains):
+        self.p_gains = p_gains
+        self.d_gains = d_gains
+
+    def get_action(self, env, des_pos, des_vel):
+        # TODO: make standardized ALRenv such that all of them have current_pos/vel attributes
+        cur_pos = env.current_pos
+        cur_vel = env.current_vel
+        if len(des_pos) != len(cur_pos):
+            des_pos = env.extend_des_pos(des_pos)
+        if len(des_vel) != len(cur_vel):
+            des_vel = env.extend_des_vel(des_vel)
+        trq = self.p_gains * (des_pos - cur_pos) + self.d_gains * (des_vel - cur_vel)
+        return trq
diff --git a/dmp_env_wrapper_example.py b/dmp_env_wrapper_example.py
index 9d699be..fcde324 100644
--- a/dmp_env_wrapper_example.py
+++ b/dmp_env_wrapper_example.py
@@ -1,36 +1,9 @@
 from alr_envs.utils.dmp_env_wrapper import DmpEnvWrapperVel
 from alr_envs.utils.dmp_async_vec_env import DmpAsyncVectorEnv, _worker
 from alr_envs.classic_control.hole_reacher import HoleReacher
-from gym.vector.async_vector_env import AsyncVectorEnv
 import numpy as np
 
 
-# env = gym.make('alr_envs:SimpleReacher-v0')
-# env = HoleReacher(num_links=5,
-#                   allow_self_collision=False,
-#                   allow_wall_collision=True,
-#                   hole_width=0.15,
-#                   hole_depth=1,
-#                   hole_x=1)
-#
-# env = DmpEnvWrapperVel(env,
-#                        num_dof=5,
-#                        num_basis=5,
-#                        duration=2,
-#                        dt=env._dt,
-#                        learn_goal=True)
-#
-# params = np.hstack([50 * np.random.randn(25), np.array([np.pi/2, -np.pi/4, -np.pi/4, -np.pi/4, -np.pi/4])])
-#
-# print(params)
-#
-# env.reset()
-# obs, rew, done, info = env.step(params, render=True)
-#
-# print(env.env._joint_angles)
-#
-# print(rew)
-
 if __name__ == "__main__":
 
     def make_env(rank, seed=0):
diff --git a/dmp_pd_control_example.py b/dmp_pd_control_example.py
new file mode 100644
index 0000000..29e7cda
--- /dev/null
+++ b/dmp_pd_control_example.py
@@ -0,0 +1,48 @@
+from alr_envs.utils.dmp_env_wrapper import DmpEnvWrapperPD
+from alr_envs.utils.policies import PDController
+from alr_envs.utils.dmp_async_vec_env import DmpAsyncVectorEnv, _worker
+from alr_envs.mujoco.reacher.alr_reacher import ALRReacherEnv
+import numpy as np
+
+
+if __name__ == "__main__":
+
+    def make_env(rank, seed=0):
+        """
+        Utility function for multiprocessed env.
+
+        :param env_id: (str) the environment ID
+        :param num_env: (int) the number of environments you wish to have in subprocesses
+        :param seed: (int) the inital seed for RNG
+        :param rank: (int) index of the subprocess
+        """
+        def _init():
+            p_gains = np.array([100, 100, 100, 100, 100])
+            d_gains = np.array([1, 1, 1, 1, 1])
+            policy = PDController(p_gains, d_gains)
+
+            env = ALRReacherEnv()
+
+            env = DmpEnvWrapperPD(env,
+                                  num_dof=5,
+                                  num_basis=5,
+                                  duration=4,
+                                  dt=env.dt,
+                                  learn_goal=False,
+                                  start_pos=env.init_qpos[:5],
+                                  final_pos=env.init_qpos[:5],
+                                  alpha_phase=2,
+                                  policy=policy
+                                  )
+            env.seed(seed + rank)
+            return env
+        return _init
+
+    dim = 25
+    env = make_env(0, 0)()
+
+    params = 10 * np.random.randn(dim)
+
+    out = env.rollout(params, render=True)
+
+    print(out)