fix(examples): desk import and max movement

Author: juelg

python/examples/env_cartesian_control.py

@@ -1,11 +1,18 @@
 import logging
 
 from dotenv import dotenv_values
+from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
 from rcsss.control.utils import load_creds_fr3_desk
-from rcsss.desk import FCI, Desk, DummyResourceManager
 from rcsss.envs.base import ControlMode, RobotInstance
-from rcsss.envs.factories import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, fr3_hw_env, fr3_sim_env
-
+from rcsss.envs.factories import (
+    default_fr3_hw_gripper_cfg,
+    default_fr3_hw_robot_cfg,
+    default_fr3_sim_gripper_cfg,
+    default_fr3_sim_robot_cfg,
+    default_mujoco_cameraset_cfg,
+    fr3_hw_env,
+    fr3_sim_env,
+)
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -24,9 +31,7 @@
 def main():
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
         user, pw = load_creds_fr3_desk()
-        resource_manger = FCI(
-            Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False
-        )
+        resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         resource_manger = DummyResourceManager()
 
@@ -38,17 +43,18 @@ def main():
                 robot_cfg=default_fr3_hw_robot_cfg(),
                 collision_guard=True,
                 gripper_cfg=default_fr3_hw_gripper_cfg(),
-                max_relative_movement=0.5
+                max_relative_movement=0.5,
             )
         else:
             env_rel = fr3_sim_env(
                 control_mode=ControlMode.CARTESIAN_TQuart,
                 robot_cfg=default_fr3_sim_robot_cfg(),
-                gripper_cfg=default_fr3_hw_gripper_cfg(),
+                gripper_cfg=default_fr3_sim_gripper_cfg(),
                 camera_set_cfg=default_mujoco_cameraset_cfg(),
-                max_relative_movement=0.5
-                )
+                max_relative_movement=0.5,
+            )
 
+        env_rel.reset()
         print(env_rel.unwrapped.robot.get_cartesian_position())
 
         for _ in range(10):

python/examples/env_joint_control.py

@@ -1,13 +1,21 @@
 import logging
+
 import mujoco
+import numpy as np
 import rcsss
-
 from dotenv import dotenv_values
+from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
 from rcsss.control.utils import load_creds_fr3_desk
-from rcsss.desk import FCI, Desk, DummyResourceManager
 from rcsss.envs.base import ControlMode, RobotInstance
-from rcsss.envs.factories import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, fr3_hw_env, fr3_sim_env
-
+from rcsss.envs.factories import (
+    default_fr3_hw_gripper_cfg,
+    default_fr3_hw_robot_cfg,
+    default_fr3_sim_gripper_cfg,
+    default_fr3_sim_robot_cfg,
+    default_mujoco_cameraset_cfg,
+    fr3_hw_env,
+    fr3_sim_env,
+)
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -26,9 +34,7 @@
 def main():
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
         user, pw = load_creds_fr3_desk()
-        resource_manger = FCI(
-            Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False
-        )
+        resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         resource_manger = DummyResourceManager()
     with resource_manger:
@@ -40,24 +46,22 @@ def main():
                 robot_cfg=default_fr3_hw_robot_cfg(),
                 collision_guard=True,
                 gripper_cfg=default_fr3_hw_gripper_cfg(),
-                max_relative_movement=0.5
+                max_relative_movement=np.deg2rad(5),
             )
         else:
             env_rel = fr3_sim_env(
                 control_mode=ControlMode.JOINTS,
                 robot_cfg=default_fr3_sim_robot_cfg(),
-                gripper_cfg=default_fr3_hw_gripper_cfg(),
+                gripper_cfg=default_fr3_sim_gripper_cfg(),
                 camera_set_cfg=default_mujoco_cameraset_cfg(),
-                max_relative_movement=0.5
-                )
+                max_relative_movement=np.deg2rad(5),
+            )
 
         for _ in range(10):
             obs, info = env_rel.reset()
             for _ in range(3):
                 # sample random relative action and execute it
                 act = env_rel.action_space.sample()
-                # if the first is open, then it does not open
-                act["gripper"] = 1
                 obs, reward, terminated, truncated, info = env_rel.step(act)
                 if truncated or terminated:
                     logger.info("Truncated or terminated!")

python/examples/fr3.py

@@ -8,7 +8,8 @@
 from rcsss._core.hw import FR3Config, IKController
 from rcsss._core.sim import CameraType
 from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
-from rcsss.desk import FCI, Desk, DummyResourceManager
+from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager
+from rcsss.control.utils import load_creds_fr3_desk
 from rcsss.envs.base import RobotInstance
 
 ROBOT_IP = "192.168.101.1"
@@ -37,14 +38,11 @@ def main():
         logger.error("This pip package was not built with the UTN lab models, aborting.")
         sys.exit()
     if ROBOT_INSTANCE == RobotInstance.HARDWARE:
-        creds = dotenv_values()
-        resource_manger = FCI(
-            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False
-        )
+        user, pw = load_creds_fr3_desk()
+        resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         resource_manger = DummyResourceManager()
 
-
     with resource_manger:
         if ROBOT_INSTANCE == RobotInstance.SIMULATION:
             simulation = sim.Sim(rcsss.scenes["fr3_empty_world"])
@@ -60,13 +58,16 @@ def main():
 
             # add camera to have a rendering gui
             cameras = {
-                "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True),
-                "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False),
+                "default_free": SimCameraConfig(
+                    identifier="", type=int(CameraType.default_free), on_screen_render=True
+                ),
+                "wrist": SimCameraConfig(
+                    identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False
+                ),
                 # TODO: odd behavior when not both cameras are used: only last image is shown
             }
             cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20)
             camera_set = SimCameraSet(simulation, cam_cfg)
-            resource_manger = DummyResourceManager()
 
         else:
             robot = rcsss.hw.FR3(ROBOT_IP, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
@@ -80,13 +81,8 @@ def main():
             gripper_cfg.speed = 0.1
             gripper_cfg.force = 30
             gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg)
-            creds = dotenv_values()
-            resource_manger = FCI(
-                Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False
-            )
             input("the robot is going to move, press enter whenever you are ready")
 
-
         # move to home position and open gripper
         robot.move_home()
         gripper.open()