format: cpp and python

Author: juelg

examples/teleop/franka.py

@@ -3,9 +3,8 @@
 from time import sleep
 
 import numpy as np
-import rcs
-from rcs._core.common import RPY, Pose, RobotPlatform
 from rcs._core import common
+from rcs._core.common import RPY, Pose, RobotPlatform
 from rcs._core.sim import SimConfig
 from rcs.camera.hw import HardwareCameraSet
 from rcs.envs.base import (
@@ -18,15 +17,17 @@
 from rcs.envs.creators import SimMultiEnvCreator
 from rcs.envs.storage_wrapper import StorageWrapper
 from rcs.envs.utils import default_digit, default_sim_gripper_cfg, default_sim_robot_cfg
+from rcs.operator.gello import GelloArmConfig, GelloConfig, GelloOperator
+from rcs.operator.interface import TeleopLoop
 from rcs.operator.quest import QuestConfig, QuestOperator
 from rcs.utils import SimpleFrameRate
 from rcs_fr3.creators import RCSFR3MultiEnvCreator
 from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 from rcs_realsense.utils import default_realsense
-from rcs.operator.gello import GelloConfig, GelloOperator, GelloArmConfig
-from rcs.operator.interface import TeleopLoop
 from simpub.sim.mj_publisher import MujocoPublisher
 
+import rcs
+
 logger = logging.getLogger(__name__)
 
 
@@ -64,8 +65,10 @@
 DATASET_PATH = "test_data_iris_dual_arm14"
 INSTRUCTION = "build a tower with the blocks in front of you"
 
-robot2world={"right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]),
-            "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758])}
+robot2world = {
+    "right": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[0.89360858, -0.17453293, 0.46425758]),
+    "left": rcs.common.Pose(translation=[0, 0, 0], rpy_vector=[-0.89360858, -0.17453293, -0.46425758]),
+}
 
 config = QuestConfig(mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION)
 # config = GelloConfig(
@@ -94,7 +97,9 @@ def get_env():
             robot_cfg=default_fr3_hw_robot_cfg(async_control=True),
             control_mode=config.operator_class.control_mode[0],
             gripper_cfg=default_fr3_hw_gripper_cfg(async_control=True),
-            max_relative_movement = 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)),
+            max_relative_movement=(
+                0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90))
+            ),
             relative_to=config.operator_class.control_mode[1],
             robot2world=robot2world,
         )

python/rcs/envs/base.py

@@ -167,6 +167,7 @@ class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ...
 CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType
 LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType
 
+
 class ArmWithGripper(TQuatDictType, GripperDictType): ...
 
 
@@ -312,7 +313,9 @@ def close(self):
 class MultiRobotWrapper(gym.Env):
     """Wraps a dictionary of environments to allow for multi robot control."""
 
-    def __init__(self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None):
+    def __init__(
+        self, envs: dict[str, gym.Env] | dict[str, gym.Wrapper], robot2world: dict[str, common.Pose] | None = None
+    ):
         self.envs = envs
         self.unwrapped_multi = cast(dict[str, RobotEnv], {key: env.unwrapped for key, env in envs.items()})
         if robot2world is None:

python/rcs/envs/sim.py

@@ -60,17 +60,16 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo
             self.sim.step_until_convergence()
         # state = self.sim_robot.get_state()
         # if "collision" not in info:
-            # info["collision"] = state.collision
+        # info["collision"] = state.collision
         # else:
-            # info["collision"] = info["collision"] or state.collision
+        # info["collision"] = info["collision"] or state.collision
         # info["ik_success"] = state.ik_success
         info["is_sim_converged"] = self.sim.is_converged()
         # truncate episode if collision
         # obs.update(self.unwrapped.get_obs())
         # return obs, 0, False, info["collision"] or not state.ik_success, info
         return obs, 0, False, False, info
 
-
     def reset(
         self, *, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:

python/rcs/envs/utils.py

@@ -39,8 +39,8 @@ def default_tilburg_hw_hand_cfg(file: str | PathLike | None = None) -> THConfig:
 
 def default_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig:
     cfg = sim.SimGripperConfig()
-    cfg.collision_geoms = [] 
-    cfg.collision_geoms_fingers = [] 
+    cfg.collision_geoms = []
+    cfg.collision_geoms_fingers = []
     # cfg.add_id(idx)
     return cfg
 

python/rcs/operator/gello.py

@@ -3,7 +3,7 @@
 import threading
 import time
 from dataclasses import dataclass, field
-from typing import Any, Dict, List, Optional, Sequence, Tuple, TypedDict, Iterator
+from typing import Any, Dict, Iterator, List, Optional, Sequence, Tuple, TypedDict
 
 import numpy as np
 
@@ -13,17 +13,25 @@
     from dynamixel_sdk.packet_handler import PacketHandler
     from dynamixel_sdk.port_handler import PortHandler
     from dynamixel_sdk.robotis_def import COMM_SUCCESS
+
     HAS_DYNAMIXEL_SDK = True
 except ImportError:
     HAS_DYNAMIXEL_SDK = False
 
 try:
     from pynput import keyboard
+
     HAS_PYNPUT = True
 except ImportError:
     HAS_PYNPUT = False
 
-from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo, JointsDictType, GripperDictType
+from rcs.envs.base import (
+    ArmWithGripper,
+    ControlMode,
+    GripperDictType,
+    JointsDictType,
+    RelativeTo,
+)
 from rcs.operator.interface import BaseOperator, BaseOperatorConfig, TeleopCommands
 from rcs.sim.sim import Sim
 from rcs.utils import SimpleFrameRate
@@ -95,7 +103,7 @@ def __init__(
     def write_value_by_name(self, name: str, values: Sequence[int | None]):
         if len(values) != len(self._ids):
             raise ValueError(f"The length of {name} must match the number of servos")
-        
+
         handler = self._groupSyncWriteHandlers[name]
         value_len = XL330_CONTROL_TABLE[name]["len"]
 
@@ -105,7 +113,7 @@ def write_value_by_name(self, name: str, values: Sequence[int | None]):
                     continue
                 param = [(value >> (8 * i)) & 0xFF for i in range(value_len)]
                 handler.addParam(dxl_id, param)
-            
+
             comm_result = handler.txPacket()
             if comm_result != COMM_SUCCESS:
                 handler.clearParam()
@@ -121,7 +129,7 @@ def read_value_by_name(self, name: str) -> List[int]:
             comm_result = handler.txRxPacket()
             if comm_result != COMM_SUCCESS:
                 raise RuntimeError(f"Failed to sync read {name}: {self._packetHandler.getTxRxResult(comm_result)}")
-            
+
             values = []
             for dxl_id in self._ids:
                 if handler.isAvailable(dxl_id, addr, value_len):
@@ -177,34 +185,24 @@ def close(self):
 
 # --- Gello Hardware Interface Logic ---
 
+
 @dataclass
 class GelloArmConfig:
     com_port: str = "/dev/ttyUSB0"
     num_arm_joints: int = 7
     joint_signs: List[int] = field(default_factory=lambda: [1, -1, 1, -1, 1, 1, 1])
     gripper: bool = True
     gripper_range_rad: List[float] = field(default_factory=lambda: [2.23, 3.22])
-    assembly_offsets: List[float] = field(
-        default_factory=lambda: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000]
-    )
-    dynamixel_kp_p: List[int] = field(
-        default_factory=lambda: [30, 60, 0, 30, 0, 0, 0, 50]
-    )
-    dynamixel_kp_i: List[int] = field(
-        default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0]
-    )
-    dynamixel_kp_d: List[int] = field(
-        default_factory=lambda: [250, 100, 80, 60, 30, 10, 5, 0]
-    )
-    dynamixel_torque_enable: List[int] = field(
-        default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0]
-    )
+    assembly_offsets: List[float] = field(default_factory=lambda: [0.000, 0.000, 3.142, 3.142, 3.142, 4.712, 0.000])
+    dynamixel_kp_p: List[int] = field(default_factory=lambda: [30, 60, 0, 30, 0, 0, 0, 50])
+    dynamixel_kp_i: List[int] = field(default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0])
+    dynamixel_kp_d: List[int] = field(default_factory=lambda: [250, 100, 80, 60, 30, 10, 5, 0])
+    dynamixel_torque_enable: List[int] = field(default_factory=lambda: [0, 0, 0, 0, 0, 0, 0, 0])
     dynamixel_goal_position: List[float] = field(
         default_factory=lambda: [0.0, 0.0, 0.0, -1.571, 0.0, 1.571, 0.0, 3.509]
     )
 
 
-
 @dataclass
 class DynamixelControlConfig:
     kp_p: List[int] = field(default_factory=list)
@@ -289,8 +287,11 @@ def __init__(self, config: GelloArmConfig):
 
     @staticmethod
     def normalize_joint_positions(raw, offsets, signs):
-        return (np.mod((raw - offsets) * signs - GelloHardware.MID_JOINT_POSITIONS, 2 * np.pi) 
-                - np.pi + GelloHardware.MID_JOINT_POSITIONS)
+        return (
+            np.mod((raw - offsets) * signs - GelloHardware.MID_JOINT_POSITIONS, 2 * np.pi)
+            - np.pi
+            + GelloHardware.MID_JOINT_POSITIONS
+        )
 
     def _initialize_parameters(self):
         for name, value in self._dynamixel_control_config:
@@ -300,25 +301,29 @@ def _initialize_parameters(self):
     def get_joint_and_gripper_positions(self) -> Tuple[np.ndarray, float]:
         joints_raw = self._driver.get_joints()
         arm_joints_raw = joints_raw[: self._num_arm_joints]
-        
+
         arm_joints_delta = (arm_joints_raw - self._prev_arm_joints_raw) * self._joint_signs
         arm_joints = self._prev_arm_joints + arm_joints_delta
         self._prev_arm_joints = arm_joints.copy()
         self._prev_arm_joints_raw = arm_joints_raw.copy()
 
         arm_joints_clipped = np.clip(arm_joints, self.JOINT_POSITION_LIMITS[:, 0], self.JOINT_POSITION_LIMITS[:, 1])
-        
+
         gripper_pos = 0.0
         if self._gripper:
             raw_grp = joints_raw[-1]
-            gripper_pos = (raw_grp - self._gripper_range_rad[0]) / (self._gripper_range_rad[1] - self._gripper_range_rad[0])
+            gripper_pos = (raw_grp - self._gripper_range_rad[0]) / (
+                self._gripper_range_rad[1] - self._gripper_range_rad[0]
+            )
             gripper_pos = max(0.0, min(1.0, gripper_pos))
 
         return arm_joints_clipped, gripper_pos
 
     def _goal_position_to_pulses(self, goals):
         arm_goals = np.array(goals[: self._num_arm_joints])
-        initial_rotations = np.floor_divide(self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, 2 * np.pi)
+        initial_rotations = np.floor_divide(
+            self._initial_arm_joints_raw - self._assembly_offsets - self.MID_JOINT_POSITIONS, 2 * np.pi
+        )
         arm_goals_raw = (initial_rotations * 2 * np.pi + arm_goals + self._assembly_offsets) * self._joint_signs + np.pi
         goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw
         return [self._driver._rad_to_pulses(rad) for rad in goals_raw]
@@ -334,8 +339,6 @@ def close(self):
 # --- RCS Operator Implementation ---
 
 
-
-
 class GelloOperator(BaseOperator):
     control_mode = (ControlMode.JOINTS, RelativeTo.NONE)
 
@@ -353,7 +356,7 @@ def __init__(self, config: GelloConfig, sim: Sim | None = None):
         self._last_joints = {name: None for name in self.controller_names}
         self._last_gripper = {name: 1.0 for name in self.controller_names}
         self._hws: Dict[str, GelloHardware] = {}
-        
+
         if HAS_PYNPUT:
             self._listener = keyboard.Listener(on_press=self._on_press)
             self._listener.start()
@@ -428,8 +431,9 @@ def close(self):
         if self.is_alive() and threading.current_thread() != self:
             self.join(timeout=1.0)
 
+
 @dataclass(kw_only=True)
 class GelloConfig(BaseOperatorConfig):
     operator_class = GelloOperator
     # Dictionary for multi-arm setups: {"left": GelloArmConfig(...), "right": GelloArmConfig(...)}
-    arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()})
+    arms: Dict[str, GelloArmConfig] = field(default_factory=lambda: {"right": GelloArmConfig()})

python/rcs/operator/interface.py

@@ -1,12 +1,12 @@
-from abc import ABC
 import copy
-from dataclasses import dataclass, field
 import logging
 import threading
 import time
+from abc import ABC
+from dataclasses import dataclass, field
 from time import sleep
-import gymnasium as gym
 
+import gymnasium as gym
 from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo
 from rcs.sim.sim import Sim
 from rcs.utils import SimpleFrameRate
@@ -52,12 +52,14 @@ def consume_action(self) -> dict[str, ArmWithGripper]:
     def close(self):
         pass
 
+
 @dataclass(kw_only=True)
 class BaseOperatorConfig:
     operator_class: BaseOperator
     read_frequency: int = 30
     simulation: bool = True
 
+
 class TeleopLoop:
     """Interface for an operator device"""
 
@@ -83,7 +85,7 @@ def __init__(
             self.key_translation = key_translation
 
         # Absolute operators (RelativeTo.NONE) need an initial sync
-        self._synced = (self.operator.control_mode[1] != RelativeTo.NONE)
+        self._synced = self.operator.control_mode[1] != RelativeTo.NONE
 
     def stop(self):
         self.operator.close()
@@ -108,13 +110,13 @@ def _translate_keys(self, actions):
                 if robot_name in self._last_obs:
                     translated[robot_name] = {
                         "joints": self._last_obs[robot_name]["joints"].copy(),
-                        "gripper": self._last_obs[robot_name].get("gripper", 1.0)
+                        "gripper": self._last_obs[robot_name].get("gripper", 1.0),
                     }
         return translated
 
     def environment_step_loop(self):
         rate_limiter = SimpleFrameRate(self.env_frequency, "env loop")
-        
+
         # 0. Initial Reset to get current positions for untracked robots
         self._last_obs, _ = self.env.reset()
 
@@ -135,15 +137,15 @@ def environment_step_loop(self):
                 sleep(1)  # sleep to let the robot reach the goal
                 self._last_obs, _ = self.env.reset()
                 self.operator.reset_operator_state()
-                self._synced = (self.operator.control_mode[1] != RelativeTo.NONE)
+                self._synced = self.operator.control_mode[1] != RelativeTo.NONE
                 # consume new commands because of potential origin reset
                 continue
 
             elif cmds.failure:
                 print("Command: Failure! Resetting env...")
                 self._last_obs, _ = self.env.reset()
                 self.operator.reset_operator_state()
-                self._synced = (self.operator.control_mode[1] != RelativeTo.NONE)
+                self._synced = self.operator.control_mode[1] != RelativeTo.NONE
                 # consume new commands because of potential origin reset
                 continue
 
@@ -174,7 +176,8 @@ def environment_step_loop(self):
                     robot = self.key_translation[controller]
                     print(f"Command: Resetting origin for {robot}...")
                     assert (
-                        self.operator.control_mode[1] == RelativeTo.CONFIGURED_ORIGIN
+                        self.operator.control_mode[1]
+                        == RelativeTo.CONFIGURED_ORIGIN
                         # TODO the following is a dict and can thus not easily be used like this
                         # and self.env.get_wrapper_attr("relative_to") == RelativeTo.CONFIGURED_ORIGIN
                     ), "both robot env and operator must be configured to relative_to.CONFIGURED_ORIGIN"