refactor: Flexible dof and robot meta config

- VectorXd type for flexible joint spaces
- adapted gym spaces to be dof independent
- robot meta config struct to describe meta configs of robot such as dofs
- robot platform enum moved to cpp part

Author: juelg

README.md

@@ -36,8 +36,8 @@ Checkout the python examples that we provide in [python/examples](python/example
 All of these examples work both in the MuJoCo simulation as well as on your hardware FR3.
 Just switch between the following settings in the example script
 ```python
-ROBOT_INSTANCE = RobotInstance.SIMULATION
-# ROBOT_INSTANCE = RobotInstance.HARDWARE
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
+# ROBOT_INSTANCE = RobotPlatform.HARDWARE
 ```
 and add your robot credentials to a `.env` file like this:
 ```env

python/examples/env_cartesian_control.py

@@ -1,7 +1,8 @@
 import logging
 
+from rcsss._core.common import RobotPlatform
 from rcsss.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk
-from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance
+from rcsss.envs.base import ControlMode, RelativeTo
 from rcsss.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator
 from rcsss.envs.utils import (
     default_fr3_hw_gripper_cfg,
@@ -15,7 +16,7 @@
 logger.setLevel(logging.INFO)
 
 ROBOT_IP = "192.168.101.1"
-ROBOT_INSTANCE = RobotInstance.SIMULATION
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
 
 
 """
@@ -43,14 +44,14 @@
 
 def main():
     context_manger: ContextManager
-    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+    if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
         user, pw = load_creds_fr3_desk()
         context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         context_manger = ContextManager()
 
     with context_manger:
-        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
             env_rel = RCSFR3EnvCreator()(
                 ip=ROBOT_IP,
                 control_mode=ControlMode.CARTESIAN_TQuat,

python/examples/env_joint_control.py

@@ -1,8 +1,9 @@
 import logging
 
 import numpy as np
+from rcsss._core.common import RobotPlatform
 from rcsss.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk
-from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance
+from rcsss.envs.base import ControlMode, RelativeTo
 from rcsss.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator
 from rcsss.envs.utils import (
     default_fr3_hw_gripper_cfg,
@@ -16,7 +17,7 @@
 logger.setLevel(logging.INFO)
 
 ROBOT_IP = "192.168.101.1"
-ROBOT_INSTANCE = RobotInstance.SIMULATION
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
 
 
 """
@@ -44,14 +45,14 @@
 
 def main():
     context_manger: FCI | ContextManager
-    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+    if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
         user, pw = load_creds_fr3_desk()
         context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         context_manger = ContextManager()
     with context_manger:
 
-        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
             env_rel = RCSFR3EnvCreator()(
                 ip=ROBOT_IP,
                 control_mode=ControlMode.JOINTS,

python/examples/fr3.py

@@ -4,15 +4,15 @@
 import numpy as np
 import rcsss
 from rcsss import sim
+from rcsss._core.common import RobotPlatform
 from rcsss._core.hw import FR3Config, IKSolver
 from rcsss._core.sim import CameraType
 from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
 from rcsss.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk
-from rcsss.envs.base import RobotInstance
 from rcsss.envs.creators import get_urdf_path
 
 ROBOT_IP = "192.168.101.1"
-ROBOT_INSTANCE = RobotInstance.SIMULATION
+ROBOT_INSTANCE = RobotPlatform.SIMULATION
 # replace this with a path to a robot urdf file if you dont have the utn models
 URDF_PATH = None
 
@@ -55,7 +55,7 @@ def main():
         logger.error("This pip package was not built with the UTN lab models, aborting.")
         sys.exit()
     context_manger: FCI | ContextManager
-    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+    if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
         user, pw = load_creds_fr3_desk()
         context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
@@ -64,7 +64,7 @@ def main():
     with context_manger:
         robot: rcsss.common.Robot
         gripper: rcsss.common.Gripper
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation = sim.Sim(rcsss.scenes["fr3_empty_world"])
             urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False)
             assert urdf_path is not None
@@ -107,15 +107,15 @@ def main():
         # move to home position and open gripper
         robot.move_home()
         gripper.open()
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
         logger.info("Robot is in home position, gripper is open")
 
         # 5cm in x direction
         robot.set_cartesian_position(
             robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0.05, 0, 0]))
         )
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
@@ -124,7 +124,7 @@ def main():
         robot.set_cartesian_position(
             robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0.05, 0]))
         )
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
@@ -133,7 +133,7 @@ def main():
         robot.set_cartesian_position(
             robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.05]))
         )
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
@@ -143,12 +143,12 @@ def main():
             translation=np.array([0, 0, 0]), rpy=rcsss.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90))
         )
         robot.set_cartesian_position(new_pose)
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
-        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
             input(
                 "hold an object 25cm in front of the gripper, the robot is going to grasp it, press enter when you are ready"
             )
@@ -157,37 +157,37 @@ def main():
         robot.set_cartesian_position(
             robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.25]))
         )
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # grasp the object
         gripper.grasp()
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
         # move 25cm backward
         robot.set_cartesian_position(
             robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, -0.25]))
         )
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
             logger.debug(f"IK success: {robot.get_state().ik_success}")  # type: ignore
             logger.debug(f"sim converged: {simulation.is_converged()}")
 
-        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
             input("gripper is going to be open, press enter when you are ready")
 
         # open gripper
         gripper.open()
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
 
         # move back to home position
         robot.move_home()
-        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+        if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation.step_until_convergence()
 
 

python/rcsss/_core/common.pyi

@@ -10,10 +10,12 @@ import numpy
 import pybind11_stubgen.typing_ext
 
 __all__ = [
+    "FR3",
     "FrankaHandTCPOffset",
     "Gripper",
     "GripperConfig",
     "GripperState",
+    "HARDWARE",
     "IK",
     "IdentityRotMatrix",
     "IdentityRotQuatVec",
@@ -22,8 +24,16 @@ __all__ = [
     "RPY",
     "Robot",
     "RobotConfig",
+    "RobotMetaConfig",
+    "RobotPlatform",
     "RobotState",
+    "RobotType",
+    "SIMULATION",
+    "UR5e",
+    "robots_meta_config",
 ]
+M = typing.TypeVar("M", bound=int)
+N = typing.TypeVar("N", bound=int)
 
 class Gripper:
     def get_normalized_width(self) -> float: ...
@@ -45,8 +55,8 @@ class GripperState:
 class IK:
     def __init__(self, urdf_path: str, max_duration_ms: int = 300) -> None: ...
     def ik(
-        self, pose: Pose, q0: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
-    ) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] | None: ...
+        self, pose: Pose, q0: numpy.ndarray[M, numpy.dtype[numpy.float64]], tcp_offset: Pose = ...
+    ) -> numpy.ndarray[M, numpy.dtype[numpy.float64]] | None: ...
 
 class Pose:
     def __getstate__(self) -> typing.Annotated[list[float], pybind11_stubgen.typing_ext.FixedSize(16)]: ...
@@ -127,23 +137,96 @@ class Robot:
     def get_base_pose_in_world_coordinates(self) -> Pose: ...
     def get_cartesian_position(self) -> Pose: ...
     def get_ik(self) -> IK | None: ...
-    def get_joint_position(self) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]: ...
+    def get_joint_position(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
     def get_parameters(self) -> RobotConfig: ...
     def get_state(self) -> RobotState: ...
     def move_home(self) -> None: ...
     def reset(self) -> None: ...
     def set_cartesian_position(self, pose: Pose) -> None: ...
-    def set_joint_position(self, q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]) -> None: ...
+    def set_joint_position(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
     def to_pose_in_robot_coordinates(self, pose_in_world_coordinates: Pose) -> Pose: ...
     def to_pose_in_world_coordinates(self, pose_in_robot_coordinates: Pose) -> Pose: ...
 
 class RobotConfig:
-    pass
+    robot_platform: RobotPlatform
+    robot_type: RobotType
+
+class RobotMetaConfig:
+    @property
+    def dof(self) -> int: ...
+    @property
+    def joint_limits(self) -> numpy.ndarray[tuple[typing.Literal[2], N], numpy.dtype[numpy.float64]]: ...
+    @property
+    def q_home(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
+
+class RobotPlatform:
+    """
+    Members:
+
+      HARDWARE
+
+      SIMULATION
+    """
+
+    HARDWARE: typing.ClassVar[RobotPlatform]  # value = <RobotPlatform.HARDWARE: 1>
+    SIMULATION: typing.ClassVar[RobotPlatform]  # value = <RobotPlatform.SIMULATION: 0>
+    __members__: typing.ClassVar[
+        dict[str, RobotPlatform]
+    ]  # value = {'HARDWARE': <RobotPlatform.HARDWARE: 1>, 'SIMULATION': <RobotPlatform.SIMULATION: 0>}
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
 
 class RobotState:
     pass
 
+class RobotType:
+    """
+    Members:
+
+      FR3
+
+      UR5e
+    """
+
+    FR3: typing.ClassVar[RobotType]  # value = <RobotType.FR3: 0>
+    UR5e: typing.ClassVar[RobotType]  # value = <RobotType.UR5e: 1>
+    __members__: typing.ClassVar[
+        dict[str, RobotType]
+    ]  # value = {'FR3': <RobotType.FR3: 0>, 'UR5e': <RobotType.UR5e: 1>}
+    def __eq__(self, other: typing.Any) -> bool: ...
+    def __getstate__(self) -> int: ...
+    def __hash__(self) -> int: ...
+    def __index__(self) -> int: ...
+    def __init__(self, value: int) -> None: ...
+    def __int__(self) -> int: ...
+    def __ne__(self, other: typing.Any) -> bool: ...
+    def __repr__(self) -> str: ...
+    def __setstate__(self, state: int) -> None: ...
+    def __str__(self) -> str: ...
+    @property
+    def name(self) -> str: ...
+    @property
+    def value(self) -> int: ...
+
 def FrankaHandTCPOffset() -> numpy.ndarray[tuple[typing.Literal[4], typing.Literal[4]], numpy.dtype[numpy.float64]]: ...
 def IdentityRotMatrix() -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ...
 def IdentityRotQuatVec() -> numpy.ndarray[typing.Literal[4], numpy.dtype[numpy.float64]]: ...
 def IdentityTranslation() -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ...
+def robots_meta_config(robot_type: RobotType) -> RobotMetaConfig: ...
+
+FR3: RobotType  # value = <RobotType.FR3: 0>
+HARDWARE: RobotPlatform  # value = <RobotPlatform.HARDWARE: 1>
+SIMULATION: RobotPlatform  # value = <RobotPlatform.SIMULATION: 0>
+UR5e: RobotType  # value = <RobotType.UR5e: 1>

python/rcsss/_core/sim.pyi

@@ -144,7 +144,7 @@ class SimRobot(rcsss._core.common.Robot):
     ) -> None: ...
     def get_parameters(self) -> SimRobotConfig: ...
     def get_state(self) -> SimRobotState: ...
-    def set_joints_hard(self, q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]) -> None: ...
+    def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ...
     def set_parameters(self, cfg: SimRobotConfig) -> bool: ...
 
 class SimRobotConfig(rcsss._core.common.RobotConfig):
@@ -168,9 +168,9 @@ class SimRobotState(rcsss._core.common.RobotState):
     @property
     def is_moving(self) -> bool: ...
     @property
-    def previous_angles(self) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]: ...
+    def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
     @property
-    def target_angles(self) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]: ...
+    def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ...
 
 def open_gui_window(uuid: str) -> None: ...