fix: replace dep. gym env with own creator and remove delta control

Author: tenfoldpaper

python/examples/grasp_demo.py

@@ -4,6 +4,7 @@
 import gymnasium as gym
 import mujoco
 import numpy as np
+from rcs.envs.creators import FR3SimplePickUpSimEnvCreator
 from rcs._core.common import Pose
 from rcs.envs.base import GripperWrapper, RobotEnv
 
@@ -50,10 +51,10 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int
         return self.generate_waypoints(end_eff_pose, goal_pose, num_waypoints=num_waypoints)
 
     def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.BINARY_GRIPPER_OPEN) -> dict:
-        for i in range(1, len(waypoints)):
+        for i in range(len(waypoints)):
             # calculate delta action
-            delta_action = waypoints[i] * waypoints[i - 1].inverse()
-            obs = self.step(self._action(delta_action, gripper))
+            # delta_action = waypoints[i] * waypoints[i - 1].inverse()
+            obs = self.step(self._action(waypoints[i], gripper))
         return obs
 
     def approach(self, geom_name: str):
@@ -83,10 +84,14 @@ def pickup(self, geom_name: str):
 
 def main():
     # compatilbe with rcs/SimplePickUpSimDigitHand-v0 and rcs/SimplePickUpSim-v0
-    env = gym.make(
-        "rcs/SimplePickUpSimDigitHand-v0",
+    # env = gym.make(
+    #     "rcs/SimplePickUpSimDigitHand-v0",
+    #     render_mode="human",
+    #     delta_actions=True,
+    # )
+    env = FR3SimplePickUpSimEnvCreator()(
         render_mode="human",
-        delta_actions=True,
+        delta_actions=False,
     )
     env.reset()
     print(env.unwrapped.robot.get_cartesian_position().translation())  # type: ignore