diff --git a/.vscode/launch.json b/.vscode/launch.json index c1603427..d93a2b98 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -8,7 +8,7 @@ "name": "Test realsense", "type": "debugpy", "request": "launch", - "module": "rcsss", + "module": "rcs", "console": "integratedTerminal", "args": ["realsense", "test", "config.yaml"], "justMyCode": true, @@ -17,7 +17,7 @@ "name": "Record with realsense", "type": "debugpy", "request": "launch", - "module": "rcsss", + "module": "rcs", "console": "integratedTerminal", "args": ["realsense", "test-record", "config.yaml"], "justMyCode": true, diff --git a/Makefile b/Makefile index ab055ed6..be30e4e1 100644 --- a/Makefile +++ b/Makefile @@ -24,14 +24,14 @@ clangcompile: # Auto generation of CPP binding stub files stubgen: - pybind11-stubgen -o python --numpy-array-use-type-var rcsss + pybind11-stubgen -o python --numpy-array-use-type-var rcs find ./python -name '*.pyi' -print | xargs sed -i '1s/^/# ATTENTION: auto generated from C++ code, use `make stubgen` to update!\n/' - find ./python -not -path "./python/rcsss/_core/*" -name '*.pyi' -delete - find ./python/rcsss/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' - find ./python/rcsss/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' - ruff check --fix python/rcsss/_core - isort python/rcsss/_core - black python/rcsss/_core + find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete + find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/typing\.Literal[\1]/g' + find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/\1/g' + ruff check --fix python/rcs/_core + isort python/rcs/_core + black python/rcs/_core # Python pycheckformat: diff --git a/README.md b/README.md index 9590d6fe..9b0bdefc 100644 --- a/README.md +++ b/README.md @@ -25,10 +25,10 @@ pip install -ve . ``` ## Usage -The python package is called `rcsss` (sss because of the sound of a snake). +The python package is called `rcs`. Import the library in python: ```python -import rcsss +import rcs ``` Checkout the python examples that we provide in [python/examples](python/examples): - [fr3.py](python/examples/fr3.py) shows direct robot control with RCS's python bindings @@ -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 @@ -49,7 +49,7 @@ DESK_PASSWORD=... The package includes a command line interface which define useful commands to handle the FR3 robot without the need to use the Desk Website. To list all available subcommands use: ```shell -python -m rcsss --help +python -m rcs --help ``` ## Development @@ -71,7 +71,7 @@ make pytest ``` ### Stub Files for Python Bindings -We use autogenerated python stub files (`.pyi`) in the [`_core`](python/rcsss/_core/) folder to show our linters the expected types of the C++ Python bindings. +We use autogenerated python stub files (`.pyi`) in the [`_core`](python/rcs/_core/) folder to show our linters the expected types of the C++ Python bindings. If the python bindings in the C++ code have changed you might need to regenerate them by using: ```shell make stubgen diff --git a/cmake/compile_scenes.cmake b/cmake/compile_scenes.cmake index 2c155013..40147ead 100644 --- a/cmake/compile_scenes.cmake +++ b/cmake/compile_scenes.cmake @@ -33,12 +33,12 @@ foreach(scene_dir ${scene_dirs}) list(APPEND mjb_files ${scene_mjb}) # Install scene files - install(FILES ${scene_mjb} DESTINATION rcsss/scenes/${scene_name} COMPONENT python_package) + install(FILES ${scene_mjb} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package) # Install URDF files file(GLOB_RECURSE urdfs ${scene_dir}/*.urdf) foreach(urdf ${urdfs}) - install(FILES ${urdf} DESTINATION rcsss/scenes/${scene_name} COMPONENT python_package) + install(FILES ${urdf} DESTINATION rcs/scenes/${scene_name} COMPONENT python_package) endforeach() endforeach() diff --git a/pyproject.toml b/pyproject.toml index 76cb40d9..be9c8681 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,7 +3,7 @@ requires = [] build-backend = "scikit_build_core.build" [project] -name = "rcsss" +name = "rcs" version = "0.4.0" description="Python Interface for libfranka to control the Franka Research 3 Robot" dependencies = ["websockets>=11.0", @@ -60,7 +60,7 @@ build.verbose = true build.targets = ["_core", "utn_scenes"] logging.level = "INFO" build-dir = "build" -wheel.packages = ["python/rcsss"] +wheel.packages = ["python/rcs"] install.components = ["python_package"] [tool.ruff] @@ -147,5 +147,5 @@ update_changelog_on_bump = true major_version_zero = true version_files = [ "CMakeLists.txt:VERSION", - "python/rcsss/_core/__init__.pyi:__version__", + "python/rcs/_core/__init__.pyi:__version__", ] diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index d8e9c5f7..a544d7a9 100644 --- a/python/examples/env_cartesian_control.py +++ b/python/examples/env_cartesian_control.py @@ -1,10 +1,10 @@ import logging -from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance -from rcsss.envs.factories import fr3_hw_env, fr3_sim_env -from rcsss.envs.utils import ( +from rcs._core.common import RobotPlatform +from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator +from rcs.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, @@ -16,7 +16,7 @@ logger.setLevel(logging.INFO) ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotInstance.SIMULATION +ROBOT_INSTANCE = RobotPlatform.SIMULATION """ @@ -26,35 +26,35 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcsss fr3 unlock +python -m rcs fr3 unlock or put it into guiding mode using: -python -m rcsss fr3 guiding-mode +python -m rcs fr3 guiding-mode When you are done you lock it again using: -python -m rcsss fr3 lock +python -m rcs fr3 lock or even shut it down using: -python -m rcsss fr3 shutdown +python -m rcs fr3 shutdown """ def main(): - resource_manger: FCI | DummyResourceManager - if ROBOT_INSTANCE == RobotInstance.HARDWARE: + context_manger: ContextManager + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: user, pw = load_creds_fr3_desk() - resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) + context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: - resource_manger = DummyResourceManager() + context_manger = ContextManager() - with resource_manger: - if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = fr3_hw_env( + with context_manger: + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSFR3EnvCreator()( ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuart, + control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_hw_robot_cfg(), collision_guard="lab", gripper_cfg=default_fr3_hw_gripper_cfg(), @@ -62,8 +62,8 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = fr3_sim_env( - control_mode=ControlMode.CARTESIAN_TQuart, + env_rel = RCSSimEnvCreator()( + control_mode=ControlMode.CARTESIAN_TQuat, robot_cfg=default_fr3_sim_robot_cfg(), collision_guard=False, gripper_cfg=default_fr3_sim_gripper_cfg(), @@ -79,14 +79,14 @@ def main(): for _ in range(10): for _ in range(10): # move 1cm in x direction (forward) and close gripper - act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} + act = {"tquat": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") return for _ in range(10): # move 1cm in negative x direction (backward) and open gripper - act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} + act = {"tquat": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1} obs, reward, terminated, truncated, info = env_rel.step(act) if truncated or terminated: logger.info("Truncated or terminated!") diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index 4c69e3e2..dbcd1df1 100644 --- a/python/examples/env_joint_control.py +++ b/python/examples/env_joint_control.py @@ -1,11 +1,11 @@ import logging import numpy as np -from rcsss.control.fr3_desk import FCI, Desk, DummyResourceManager -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.base import ControlMode, RelativeTo, RobotInstance -from rcsss.envs.factories import fr3_hw_env, fr3_sim_env -from rcsss.envs.utils import ( +from rcs._core.common import RobotPlatform +from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs.envs.base import ControlMode, RelativeTo +from rcs.envs.creators import RCSFR3EnvCreator, RCSSimEnvCreator +from rcs.envs.utils import ( default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg, default_fr3_sim_gripper_cfg, @@ -17,7 +17,7 @@ logger.setLevel(logging.INFO) ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotInstance.SIMULATION +ROBOT_INSTANCE = RobotPlatform.SIMULATION """ @@ -27,33 +27,33 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcsss fr3 unlock +python -m rcs fr3 unlock or put it into guiding mode using: -python -m rcsss fr3 guiding-mode +python -m rcs fr3 guiding-mode When you are done you lock it again using: -python -m rcsss fr3 lock +python -m rcs fr3 lock or even shut it down using: -python -m rcsss fr3 shutdown +python -m rcs fr3 shutdown """ def main(): - resource_manger: FCI | DummyResourceManager - if ROBOT_INSTANCE == RobotInstance.HARDWARE: + context_manger: FCI | ContextManager + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: user, pw = load_creds_fr3_desk() - resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) + context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: - resource_manger = DummyResourceManager() - with resource_manger: + context_manger = ContextManager() + with context_manger: - if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = fr3_hw_env( + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: + env_rel = RCSFR3EnvCreator()( ip=ROBOT_IP, control_mode=ControlMode.JOINTS, robot_cfg=default_fr3_hw_robot_cfg(), @@ -63,7 +63,7 @@ def main(): relative_to=RelativeTo.LAST_STEP, ) else: - env_rel = fr3_sim_env( + env_rel = RCSSimEnvCreator()( control_mode=ControlMode.JOINTS, collision_guard=False, robot_cfg=default_fr3_sim_robot_cfg(), diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 62d0f853..40ad0a0e 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -2,18 +2,17 @@ import sys import numpy as np -import rcsss -from rcsss import sim -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, Desk, DummyResourceManager -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.base import RobotInstance -from rcsss.envs.factories import get_urdf_path +import rcs +from rcs import sim +from rcs._core.common import RobotPlatform +from rcs._core.hw import FR3Config, IKSolver +from rcs._core.sim import CameraType +from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk +from rcs.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 @@ -35,49 +34,49 @@ When you use a real FR3 you first need to unlock its joints using the following cli script: -python -m rcsss fr3 unlock +python -m rcs fr3 unlock or put it into guiding mode using: -python -m rcsss fr3 guiding-mode +python -m rcs fr3 guiding-mode When you are done you lock it again using: -python -m rcsss fr3 lock +python -m rcs fr3 lock or even shut it down using: -python -m rcsss fr3 shutdown +python -m rcs fr3 shutdown """ def main(): - if "lab" not in rcsss.scenes: + if "lab" not in rcs.scenes: logger.error("This pip package was not built with the UTN lab models, aborting.") sys.exit() - resource_manger: FCI | DummyResourceManager - if ROBOT_INSTANCE == RobotInstance.HARDWARE: + context_manger: FCI | ContextManager + if ROBOT_INSTANCE == RobotPlatform.HARDWARE: user, pw = load_creds_fr3_desk() - resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) + context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) else: - resource_manger = DummyResourceManager() + context_manger = ContextManager() - with resource_manger: - robot: rcsss.common.Robot - gripper: rcsss.common.Gripper - if ROBOT_INSTANCE == RobotInstance.SIMULATION: - simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) + with context_manger: + robot: rcs.common.Robot + gripper: rcs.common.Gripper + if ROBOT_INSTANCE == RobotPlatform.SIMULATION: + simulation = sim.Sim(rcs.scenes["fr3_empty_world"]) urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None - ik = rcsss.common.IK(urdf_path) - robot = rcsss.sim.FR3(simulation, "0", ik) - cfg = sim.FR3Config() - cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + ik = rcs.common.IK(urdf_path) + robot = rcs.sim.SimRobot(simulation, "0", ik) + cfg = sim.SimRobotConfig() + cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) cfg.realtime = False robot.set_parameters(cfg) - gripper_cfg_sim = sim.FHConfig() - gripper = sim.FrankaHand(simulation, "0", gripper_cfg_sim) + gripper_cfg_sim = sim.SimGripperConfig() + gripper = sim.SimGripper(simulation, "0", gripper_cfg_sim) # add camera to have a rendering gui cameras = { @@ -91,104 +90,104 @@ def main(): else: urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None - ik = rcsss.common.IK(urdf_path) - robot = rcsss.hw.FR3(ROBOT_IP, ik) + ik = rcs.common.IK(urdf_path) + robot = rcs.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() - robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - robot_cfg.ik_solver = IKSolver.rcs + robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) + robot_cfg.ik_solver = IKSolver.rcs_ik robot.set_parameters(robot_cfg) - gripper_cfg_hw = rcsss.hw.FHConfig() + gripper_cfg_hw = rcs.hw.FHConfig() gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1 gripper_cfg_hw.speed = 0.1 gripper_cfg_hw.force = 30 - gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) + gripper = rcs.hw.FrankaHand(ROBOT_IP, gripper_cfg_hw) 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() - 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])) + robot.get_cartesian_position() * rcs.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()}") # 5cm in y direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0.05, 0])) + robot.get_cartesian_position() * rcs.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()}") # 5cm in z direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.05])) + robot.get_cartesian_position() * rcs.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()}") # rotate the arm 90 degrees around the inverted y and z axis - new_pose = robot.get_cartesian_position() * rcsss.common.Pose( - translation=np.array([0, 0, 0]), rpy=rcsss.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90)) + new_pose = robot.get_cartesian_position() * rcs.common.Pose( + translation=np.array([0, 0, 0]), rpy=rcs.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" ) # move 25cm towards the gripper direction robot.set_cartesian_position( - robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.25])) + robot.get_cartesian_position() * rcs.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])) + robot.get_cartesian_position() * rcs.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() diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 89653a46..03b2bb14 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -4,8 +4,8 @@ import gymnasium as gym import mujoco import numpy as np -from rcsss._core.common import Pose -from rcsss.envs.base import FR3Env, GripperWrapper +from rcs._core.common import Pose +from rcs.envs.base import GripperWrapper, RobotEnv logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -14,7 +14,7 @@ class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self.unwrapped: FR3Env = cast(FR3Env, self.env.unwrapped) + self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) self.home_pose = self.unwrapped.robot.get_cartesian_position() def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index d4817132..3abcc18b 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -4,8 +4,8 @@ import gymnasium as gym import mujoco import numpy as np -from rcsss._core.common import Pose -from rcsss.envs.base import FR3Env, GripperWrapper +from rcs._core.common import Pose +from rcs.envs.base import GripperWrapper, RobotEnv logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) @@ -14,7 +14,7 @@ class PickUpDemo: def __init__(self, env: gym.Env): self.env = env - self.unwrapped: FR3Env = cast(FR3Env, self.env.unwrapped) + self.unwrapped: RobotEnv = cast(RobotEnv, self.env.unwrapped) self.home_pose = self.unwrapped.robot.get_cartesian_position() def _action(self, pose: Pose, gripper: float) -> dict[str, Any]: diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py new file mode 100644 index 00000000..c33e6bb0 --- /dev/null +++ b/python/rcs/__init__.py @@ -0,0 +1,48 @@ +"""Robot control stack python bindings.""" + +import pathlib +import site + +from gymnasium import register +from rcs import camera, control, envs, sim +from rcs._core import __version__, common, hw +from rcs.envs.creators import ( + FR3LabPickUpSimDigitHandEnvCreator, + FR3SimplePickUpSimDigitHandEnvCreator, + FR3SimplePickUpSimEnvCreator, + RCSFR3DefaultEnvCreator, + RCSFR3EnvCreator, +) + +# available mujoco scenes +scenes = { + path.stem: path / "scene.mjb" for path in (pathlib.Path(site.getsitepackages()[0]) / "rcs" / "scenes").glob("*") +} + +# make submodules available +__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] + +# register gymnasium environments +register( + id="rcs/SimplePickUpSim-v0", + entry_point=FR3SimplePickUpSimEnvCreator(), +) + +register( + id="rcs/SimplePickUpSimDigitHand-v0", + entry_point=FR3SimplePickUpSimDigitHandEnvCreator(), +) +register( + id="rcs/LabPickUpSimDigitHand-v0", + entry_point=FR3LabPickUpSimDigitHandEnvCreator(), +) + +register( + id="rcs/FR3Env-v0", + entry_point=RCSFR3EnvCreator(), +) + +register( + id="rcs/FR3Default-v0", + entry_point=RCSFR3DefaultEnvCreator(), +) diff --git a/python/rcsss/__main__.py b/python/rcs/__main__.py similarity index 54% rename from python/rcsss/__main__.py rename to python/rcs/__main__.py index 77d9b253..a71d4e71 100644 --- a/python/rcsss/__main__.py +++ b/python/rcs/__main__.py @@ -1,4 +1,4 @@ -from rcsss.cli.main import main +from rcs.cli.main import main if __name__ == "__main__": main() diff --git a/python/rcsss/_core/__init__.pyi b/python/rcs/_core/__init__.pyi similarity index 100% rename from python/rcsss/_core/__init__.pyi rename to python/rcs/_core/__init__.pyi diff --git a/python/rcsss/_core/common.pyi b/python/rcs/_core/common.pyi similarity index 61% rename from python/rcsss/_core/common.pyi rename to python/rcs/_core/common.pyi index f0e1f252..ef6ded08 100644 --- a/python/rcsss/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -10,33 +10,35 @@ import numpy import pybind11_stubgen.typing_ext __all__ = [ + "FR3", "FrankaHandTCPOffset", - "GConfig", - "GState", "Gripper", + "GripperConfig", + "GripperState", + "HARDWARE", "IK", "IdentityRotMatrix", - "IdentityRotQuartVec", + "IdentityRotQuatVec", "IdentityTranslation", - "NRobotsWithGripper", "Pose", - "RConfig", "RPY", - "RState", "Robot", - "RobotWithGripper", + "RobotConfig", + "RobotMetaConfig", + "RobotPlatform", + "RobotState", + "RobotType", + "SIMULATION", + "UR5e", + "robots_meta_config", ] - -class GConfig: - pass - -class GState: - pass +M = typing.TypeVar("M", bound=int) +N = typing.TypeVar("N", bound=int) class Gripper: def get_normalized_width(self) -> float: ... - def get_parameters(self) -> GConfig: ... - def get_state(self) -> GState: ... + def get_parameters(self) -> GripperConfig: ... + def get_state(self) -> GripperState: ... def grasp(self) -> None: ... def is_grasped(self) -> bool: ... def open(self) -> None: ... @@ -44,30 +46,17 @@ class Gripper: def set_normalized_width(self, width: float, force: float = 0) -> None: ... def shut(self) -> None: ... +class GripperConfig: + pass + +class GripperState: + pass + 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: ... - -class NRobotsWithGripper: - def __init__(self, robots_with_gripper: list[RobotWithGripper]) -> None: ... - def get_cartesian_position(self, idxs: list[int]) -> list[Pose]: ... - def get_joint_position( - self, idxs: list[int] - ) -> list[numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]]: ... - def get_parameters_g(self, idxs: list[int]) -> list[GConfig | None]: ... - def get_parameters_r(self, idxs: list[int]) -> list[RConfig]: ... - def get_state_g(self, idxs: list[int]) -> list[GState | None]: ... - def get_state_r(self, idxs: list[int]) -> list[RState]: ... - def grasp(self, idxs: list[int]) -> None: ... - def move_home(self, idxs: list[int]) -> None: ... - def open(self, idxs: list[int]) -> None: ... - def set_cartesian_position(self, idxs: list[int], pose: list[Pose]) -> None: ... - def set_joint_position( - self, idxs: list[int], q: list[numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]] - ) -> None: ... - def shut(self, idxs: list[int]) -> 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)]: ... @@ -125,9 +114,6 @@ class Pose: def translation(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float64]]: ... def xyzrpy(self) -> numpy.ndarray[typing.Literal[6], numpy.dtype[numpy.float64]]: ... -class RConfig: - pass - class RPY: pitch: float roll: float @@ -147,27 +133,100 @@ class RPY: self, ) -> numpy.ndarray[tuple[typing.Literal[3], typing.Literal[3]], numpy.dtype[numpy.float64]]: ... -class RState: - pass - 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_parameters(self) -> RConfig: ... - def get_state(self) -> RState: ... + 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 RobotWithGripper: - def __init__(self, robot: Robot, gripper: Gripper | None) -> None: ... +class RobotConfig: + 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 = + SIMULATION: typing.ClassVar[RobotPlatform] # value = + __members__: typing.ClassVar[ + dict[str, RobotPlatform] + ] # value = {'HARDWARE': , 'SIMULATION': } + 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 = + UR5e: typing.ClassVar[RobotType] # value = + __members__: typing.ClassVar[ + dict[str, RobotType] + ] # value = {'FR3': , 'UR5e': } + 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 IdentityRotQuartVec() -> numpy.ndarray[typing.Literal[4], 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 = +HARDWARE: RobotPlatform # value = +SIMULATION: RobotPlatform # value = +UR5e: RobotType # value = diff --git a/python/rcsss/_core/hw/__init__.pyi b/python/rcs/_core/hw/__init__.pyi similarity index 73% rename from python/rcsss/_core/hw/__init__.pyi rename to python/rcs/_core/hw/__init__.pyi index e80e1eda..995f33e4 100644 --- a/python/rcsss/_core/hw/__init__.pyi +++ b/python/rcs/_core/hw/__init__.pyi @@ -7,7 +7,7 @@ from __future__ import annotations import typing import numpy -import rcsss._core.common +import rcs._core.common from . import exceptions @@ -21,11 +21,11 @@ __all__ = [ "FrankaHand", "IKSolver", "exceptions", - "franka", - "rcs", + "franka_ik", + "rcs_ik", ] -class FHConfig(rcsss._core.common.GConfig): +class FHConfig(rcs._core.common.GripperConfig): async_control: bool epsilon_inner: float epsilon_outer: float @@ -34,7 +34,7 @@ class FHConfig(rcsss._core.common.GConfig): speed: float def __init__(self) -> None: ... -class FHState(rcsss._core.common.GState): +class FHState(rcs._core.common.GripperState): def __init__(self) -> None: ... @property def bool_state(self) -> bool: ... @@ -51,8 +51,8 @@ class FHState(rcsss._core.common.GState): @property def width(self) -> float: ... -class FR3(rcsss._core.common.Robot): - def __init__(self, ip: str, ik: rcsss._core.common.IK | None = None) -> None: ... +class FR3(rcs._core.common.Robot): + def __init__(self, ip: str, ik: rcs._core.common.IK | None = None) -> None: ... def automatic_error_recovery(self) -> None: ... def controller_set_joint_position( self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]] @@ -60,11 +60,11 @@ class FR3(rcsss._core.common.Robot): def double_tap_robot_to_continue(self) -> None: ... def get_parameters(self) -> FR3Config: ... def get_state(self) -> FR3State: ... - def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcsss._core.common.Pose) -> None: ... + def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... def set_cartesian_position_ik( - self, pose: rcsss._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5 + self, pose: rcs._core.common.Pose, max_time: float, elbow: float | None, max_force: float | None = 5 ) -> None: ... - def set_cartesian_position_internal(self, pose: rcsss._core.common.Pose) -> None: ... + def set_cartesian_position_internal(self, pose: rcs._core.common.Pose) -> None: ... def set_default_robot_behavior(self) -> None: ... def set_guiding_mode( self, @@ -80,14 +80,14 @@ class FR3(rcsss._core.common.Robot): def stop_control_thread(self) -> None: ... def zero_torque_guiding(self) -> None: ... -class FR3Config(rcsss._core.common.RConfig): +class FR3Config(rcs._core.common.RobotConfig): async_control: bool ik_solver: IKSolver load_parameters: FR3Load | None - nominal_end_effector_frame: rcsss._core.common.Pose | None + nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float - tcp_offset: rcsss._core.common.Pose - world_to_robot: rcsss._core.common.Pose | None + tcp_offset: rcs._core.common.Pose + world_to_robot: rcs._core.common.Pose | None def __init__(self) -> None: ... class FR3Load: @@ -96,10 +96,10 @@ class FR3Load: load_mass: float def __init__(self) -> None: ... -class FR3State(rcsss._core.common.RState): +class FR3State(rcs._core.common.RobotState): def __init__(self) -> None: ... -class FrankaHand(rcsss._core.common.Gripper): +class FrankaHand(rcs._core.common.Gripper): def __init__(self, ip: str, cfg: FHConfig) -> None: ... def get_parameters(self) -> FHConfig: ... def get_state(self) -> FHState: ... @@ -111,16 +111,16 @@ class IKSolver: """ Members: - franka + franka_ik - rcs + rcs_ik """ __members__: typing.ClassVar[ dict[str, IKSolver] - ] # value = {'franka': , 'rcs': } - franka: typing.ClassVar[IKSolver] # value = - rcs: typing.ClassVar[IKSolver] # value = + ] # value = {'franka_ik': , 'rcs_ik': } + franka_ik: typing.ClassVar[IKSolver] # value = + rcs_ik: typing.ClassVar[IKSolver] # value = def __eq__(self, other: typing.Any) -> bool: ... def __getstate__(self) -> int: ... def __hash__(self) -> int: ... @@ -136,5 +136,5 @@ class IKSolver: @property def value(self) -> int: ... -franka: IKSolver # value = -rcs: IKSolver # value = +franka_ik: IKSolver # value = +rcs_ik: IKSolver # value = diff --git a/python/rcsss/_core/hw/exceptions.pyi b/python/rcs/_core/hw/exceptions.pyi similarity index 100% rename from python/rcsss/_core/hw/exceptions.pyi rename to python/rcs/_core/hw/exceptions.pyi diff --git a/python/rcsss/_core/sim.pyi b/python/rcs/_core/sim.pyi similarity index 77% rename from python/rcsss/_core/sim.pyi rename to python/rcs/_core/sim.pyi index eb50d591..0da44b92 100644 --- a/python/rcsss/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -7,21 +7,21 @@ from __future__ import annotations import typing import numpy -import rcsss._core.common +import rcs._core.common __all__ = [ "CameraType", - "FHConfig", - "FHState", - "FR3", - "FR3Config", - "FR3State", "FrameSet", - "FrankaHand", "Sim", "SimCameraConfig", "SimCameraSet", "SimCameraSetConfig", + "SimGripper", + "SimGripperConfig", + "SimGripperState", + "SimRobot", + "SimRobotConfig", + "SimRobotState", "default_free", "fixed", "free", @@ -65,60 +65,6 @@ class CameraType: @property def value(self) -> int: ... -class FHConfig(rcsss._core.common.GConfig): - epsilon_inner: float - epsilon_outer: float - ignored_collision_geoms: list[str] - seconds_between_callbacks: float - def __init__(self) -> None: ... - -class FHState(rcsss._core.common.GState): - def __init__(self) -> None: ... - @property - def collision(self) -> bool: ... - @property - def is_moving(self) -> bool: ... - @property - def last_commanded_width(self) -> float: ... - @property - def last_width(self) -> float: ... - @property - def max_unnormalized_width(self) -> float: ... - -class FR3(rcsss._core.common.Robot): - def __init__( - self, sim: Sim, id: str, ik: rcsss._core.common.IK, register_convergence_callback: bool = True - ) -> None: ... - def get_parameters(self) -> FR3Config: ... - def get_state(self) -> FR3State: ... - def set_joints_hard(self, q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]) -> None: ... - def set_parameters(self, cfg: FR3Config) -> bool: ... - -class FR3Config(rcsss._core.common.RConfig): - joint_rotational_tolerance: float - realtime: bool - seconds_between_callbacks: float - tcp_offset: rcsss._core.common.Pose - trajectory_trace: bool - def __init__(self) -> None: ... - -class FR3State(rcsss._core.common.RState): - def __init__(self) -> None: ... - @property - def collision(self) -> bool: ... - @property - def ik_success(self) -> bool: ... - @property - def inverse_tcp_offset(self) -> rcsss._core.common.Pose: ... - @property - def is_arrived(self) -> bool: ... - @property - def is_moving(self) -> bool: ... - @property - def previous_angles(self) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]: ... - @property - def target_angles(self) -> numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]: ... - class FrameSet: def __init__(self) -> None: ... @property @@ -128,12 +74,6 @@ class FrameSet: @property def timestamp(self) -> float: ... -class FrankaHand(rcsss._core.common.Gripper): - def __init__(self, sim: Sim, id: str, cfg: FHConfig) -> None: ... - def get_parameters(self) -> FHConfig: ... - def get_state(self) -> FHState: ... - def set_parameters(self, cfg: FHConfig) -> bool: ... - class Sim: def __init__(self, mjmdl: int, mjdata: int) -> None: ... def _start_gui_server(self, id: str) -> None: ... @@ -172,6 +112,66 @@ class SimCameraSetConfig: @frame_rate.setter def frame_rate(self, arg0: int) -> None: ... +class SimGripper(rcs._core.common.Gripper): + def __init__(self, sim: Sim, id: str, cfg: SimGripperConfig) -> None: ... + def get_parameters(self) -> SimGripperConfig: ... + def get_state(self) -> SimGripperState: ... + def set_parameters(self, cfg: SimGripperConfig) -> bool: ... + +class SimGripperConfig(rcs._core.common.GripperConfig): + epsilon_inner: float + epsilon_outer: float + ignored_collision_geoms: list[str] + seconds_between_callbacks: float + def __init__(self) -> None: ... + +class SimGripperState(rcs._core.common.GripperState): + def __init__(self) -> None: ... + @property + def collision(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property + def last_commanded_width(self) -> float: ... + @property + def last_width(self) -> float: ... + @property + def max_unnormalized_width(self) -> float: ... + +class SimRobot(rcs._core.common.Robot): + def __init__( + self, sim: Sim, id: str, ik: rcs._core.common.IK, register_convergence_callback: bool = True + ) -> None: ... + def get_parameters(self) -> SimRobotConfig: ... + def get_state(self) -> SimRobotState: ... + def set_joints_hard(self, q: numpy.ndarray[M, numpy.dtype[numpy.float64]]) -> None: ... + def set_parameters(self, cfg: SimRobotConfig) -> bool: ... + +class SimRobotConfig(rcs._core.common.RobotConfig): + joint_rotational_tolerance: float + realtime: bool + seconds_between_callbacks: float + tcp_offset: rcs._core.common.Pose + trajectory_trace: bool + def __init__(self) -> None: ... + +class SimRobotState(rcs._core.common.RobotState): + def __init__(self) -> None: ... + @property + def collision(self) -> bool: ... + @property + def ik_success(self) -> bool: ... + @property + def inverse_tcp_offset(self) -> rcs._core.common.Pose: ... + @property + def is_arrived(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property + def previous_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + @property + def target_angles(self) -> numpy.ndarray[M, numpy.dtype[numpy.float64]]: ... + def open_gui_window(uuid: str) -> None: ... default_free: CameraType # value = diff --git a/python/rcsss/camera/__init__.py b/python/rcs/camera/__init__.py similarity index 100% rename from python/rcsss/camera/__init__.py rename to python/rcs/camera/__init__.py diff --git a/python/rcsss/camera/hw.py b/python/rcs/camera/hw.py similarity index 99% rename from python/rcsss/camera/hw.py rename to python/rcs/camera/hw.py index 4c45717e..98388259 100644 --- a/python/rcsss/camera/hw.py +++ b/python/rcs/camera/hw.py @@ -9,7 +9,7 @@ import cv2 import numpy as np from pydantic import Field -from rcsss.camera.interface import ( +from rcs.camera.interface import ( BaseCameraConfig, BaseCameraSetConfig, Frame, diff --git a/python/rcsss/camera/interface.py b/python/rcs/camera/interface.py similarity index 100% rename from python/rcsss/camera/interface.py rename to python/rcs/camera/interface.py diff --git a/python/rcsss/camera/kinect.py b/python/rcs/camera/kinect.py similarity index 94% rename from python/rcsss/camera/kinect.py rename to python/rcs/camera/kinect.py index 71b7037c..bfe9c095 100644 --- a/python/rcsss/camera/kinect.py +++ b/python/rcs/camera/kinect.py @@ -1,7 +1,7 @@ import k4a import numpy as np -from rcsss.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig -from rcsss.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame +from rcs.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig +from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame class KinectConfig(HWCameraSetConfig): diff --git a/python/rcsss/camera/realsense.py b/python/rcs/camera/realsense.py similarity index 98% rename from python/rcsss/camera/realsense.py rename to python/rcs/camera/realsense.py index 53ced077..1f0fac63 100644 --- a/python/rcsss/camera/realsense.py +++ b/python/rcs/camera/realsense.py @@ -2,8 +2,8 @@ import numpy as np import pyrealsense2 as rs -from rcsss.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig -from rcsss.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame +from rcs.camera.hw import BaseHardwareCameraSet, HWCameraSetConfig +from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame # class RealSenseConfig(BaseCameraConfig): # dict with readable name and serial number diff --git a/python/rcsss/camera/sim.py b/python/rcs/camera/sim.py similarity index 91% rename from python/rcsss/camera/sim.py rename to python/rcs/camera/sim.py index cd7fa6c0..2ec82c27 100644 --- a/python/rcsss/camera/sim.py +++ b/python/rcs/camera/sim.py @@ -2,14 +2,14 @@ from datetime import datetime import numpy as np -import rcsss +import rcs from pydantic import Field -from rcsss._core.sim import CameraType -from rcsss._core.sim import FrameSet as _FrameSet -from rcsss._core.sim import SimCameraConfig as _SimCameraConfig -from rcsss._core.sim import SimCameraSet as _SimCameraSet -from rcsss._core.sim import SimCameraSetConfig as _SimCameraSetConfig -from rcsss.camera.interface import ( +from rcs._core.sim import CameraType +from rcs._core.sim import FrameSet as _FrameSet +from rcs._core.sim import SimCameraConfig as _SimCameraConfig +from rcs._core.sim import SimCameraSet as _SimCameraSet +from rcs._core.sim import SimCameraSetConfig as _SimCameraSetConfig +from rcs.camera.interface import ( BaseCameraConfig, BaseCameraSetConfig, CameraFrame, @@ -34,7 +34,7 @@ class SimCameraSet(_SimCameraSet): Implements BaseCameraSet """ - def __init__(self, sim: rcsss.sim.Sim, cfg: SimCameraSetConfig): + def __init__(self, sim: rcs.sim.Sim, cfg: SimCameraSetConfig): self._logger = logging.getLogger(__name__) self._cfg = cfg cameras: dict[str, _SimCameraConfig] = {} @@ -62,7 +62,7 @@ def get_type(t): cpp_set_cfg.max_buffer_frames = cfg.max_buffer_frames super().__init__(sim, cpp_set_cfg) - self._sim: rcsss.sim.Sim + self._sim: rcs.sim.Sim def get_latest_frames(self) -> FrameSet | None: """Should return the latest frame from the camera with the given name.""" diff --git a/python/rcsss/cli/__init__.py b/python/rcs/cli/__init__.py similarity index 100% rename from python/rcsss/cli/__init__.py rename to python/rcs/cli/__init__.py diff --git a/python/rcsss/cli/main.py b/python/rcs/cli/main.py similarity index 83% rename from python/rcsss/cli/main.py rename to python/rcs/cli/main.py index edb5eb8a..7247f9a2 100644 --- a/python/rcsss/cli/main.py +++ b/python/rcs/cli/main.py @@ -4,13 +4,13 @@ from typing import Annotated, Optional import pyrealsense2 as rs -import rcsss -import rcsss.control.fr3_desk +import rcs +import rcs.control.fr3_desk import typer -from rcsss.camera.realsense import RealSenseCameraSet -from rcsss.control.record import PoseList -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.factories import get_urdf_path +from rcs.camera.realsense import RealSenseCameraSet +from rcs.control.fr3_desk import load_creds_fr3_desk +from rcs.control.record import PoseList +from rcs.envs.creators import get_urdf_path logger = logging.getLogger(__name__) @@ -57,7 +57,7 @@ def home( ): """Moves the FR3 to home position""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.home(ip, user, pw, shut, unlock) + rcs.control.fr3_desk.home(ip, user, pw, shut, unlock) @fr3_app.command() @@ -67,7 +67,7 @@ def info( ): """Prints info about the robots current joint position and end effector pose, optionally also the gripper.""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.info(ip, user, pw, include_gripper) + rcs.control.fr3_desk.info(ip, user, pw, include_gripper) @fr3_app.command() @@ -76,7 +76,7 @@ def lock( ): """Locks the robot.""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.lock(ip, user, pw) + rcs.control.fr3_desk.lock(ip, user, pw) @fr3_app.command() @@ -85,8 +85,8 @@ def unlock( ): """Prepares the robot by unlocking the joints and putting the robot into the FCI mode.""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.unlock(ip, user, pw) - with rcsss.control.fr3_desk.Desk(ip, user, pw) as d: + rcs.control.fr3_desk.unlock(ip, user, pw) + with rcs.control.fr3_desk.Desk(ip, user, pw) as d: d.activate_fci() @@ -99,12 +99,12 @@ def fci( """Puts the robot into FCI mode, optionally unlocks the robot. Waits for ctrl+c to exit.""" user, pw = load_creds_fr3_desk() try: - with rcsss.control.fr3_desk.FCI(rcsss.control.fr3_desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False): + with rcs.control.fr3_desk.FCI(rcs.control.fr3_desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False): while True: sleep(1) except KeyboardInterrupt: if shutdown: - rcsss.control.fr3_desk.shutdown(ip, user, pw) + rcs.control.fr3_desk.shutdown(ip, user, pw) @fr3_app.command() @@ -115,7 +115,7 @@ def guiding_mode( ): """Enables or disables guiding mode.""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.guiding_mode(ip, user, pw, disable, unlock) + rcs.control.fr3_desk.guiding_mode(ip, user, pw, disable, unlock) @fr3_app.command() @@ -124,7 +124,7 @@ def shutdown( ): """Shuts the robot down""" user, pw = load_creds_fr3_desk() - rcsss.control.fr3_desk.shutdown(ip, user, pw) + rcs.control.fr3_desk.shutdown(ip, user, pw) @fr3_app.command() @@ -144,7 +144,7 @@ def record( if lpaths is not None and len(lpaths) > 0: with ExitStack() as stack: for r_ip in name2ip.values(): - stack.enter_context(rcsss.control.fr3_desk.Desk.fci(r_ip, username=user, password=pw, unlock=True)) + stack.enter_context(rcs.control.fr3_desk.Desk.fci(r_ip, username=user, password=pw, unlock=True)) p = PoseList.load(name2ip, lpaths, urdf_path=urdf_path) input("Press any key to replay") @@ -152,7 +152,7 @@ def record( else: with ExitStack() as stack: gms = [ - rcsss.control.fr3_desk.Desk.guiding_mode(r_ip, username=user, password=pw, unlock=True) + rcs.control.fr3_desk.Desk.guiding_mode(r_ip, username=user, password=pw, unlock=True) for r_ip in name2ip.values() ] for gm in gms: diff --git a/python/rcsss/control/__init__.py b/python/rcs/control/__init__.py similarity index 100% rename from python/rcsss/control/__init__.py rename to python/rcs/control/__init__.py diff --git a/python/rcsss/control/fr3_desk.py b/python/rcs/control/fr3_desk.py similarity index 95% rename from python/rcsss/control/fr3_desk.py rename to python/rcs/control/fr3_desk.py index f2826ddd..3cedad71 100644 --- a/python/rcsss/control/fr3_desk.py +++ b/python/rcs/control/fr3_desk.py @@ -1,5 +1,4 @@ import base64 -import dataclasses import hashlib import json as json_module import logging @@ -7,18 +6,20 @@ import ssl import threading import time +from dataclasses import dataclass from typing import Callable, Literal from urllib import parse -import rcsss +import rcs import requests -from rcsss.envs.factories import default_fr3_hw_gripper_cfg +from dotenv import load_dotenv +from rcs.envs.creators import default_fr3_hw_gripper_cfg from requests.packages import urllib3 # type: ignore[attr-defined] from websockets.sync.client import connect _logger = logging.getLogger("desk") -TOKEN_PATH = "~/.rcsss/token.conf" +TOKEN_PATH = "~/.rcs/token.conf" """ Path to the configuration file holding known control tokens. If :py:class:`Desk` is used to connect to a control unit's @@ -27,15 +28,22 @@ """ +def load_creds_fr3_desk() -> tuple[str, str]: + load_dotenv() + assert "DESK_USERNAME" in os.environ, "DESK_USERNAME not set in .env file or environment var." + assert "DESK_PASSWORD" in os.environ, "DESK_PASSWORD not set in .env file or environment var." + return os.environ["DESK_USERNAME"], os.environ["DESK_PASSWORD"] + + def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False): with Desk.fci(ip, username, password, unlock=unlock): - f = rcsss.hw.FR3(ip) - config = rcsss.hw.FR3Config() + f = rcs.hw.FR3(ip) + config = rcs.hw.FR3Config() config.speed_factor = 0.2 - config.ik_solver = rcsss.hw.IKSolver.franka + config.ik_solver = rcs.hw.IKSolver.franka_ik f.set_parameters(config) - config_hand = rcsss.hw.FHConfig() - g = rcsss.hw.FrankaHand(ip, config_hand) + config_hand = rcs.hw.FHConfig() + g = rcs.hw.FrankaHand(ip, config_hand) if shut: g.shut() else: @@ -45,8 +53,8 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False def info(ip: str, username: str, password: str, include_hand: bool = False): with Desk.fci(ip, username, password): - f = rcsss.hw.FR3(ip) - config = rcsss.hw.FR3Config() + f = rcs.hw.FR3(ip) + config = rcs.hw.FR3Config() f.set_parameters(config) print("Robot info:") print("Current cartesian position:") @@ -55,7 +63,7 @@ def info(ip: str, username: str, password: str, include_hand: bool = False): print(f.get_joint_position()) if include_hand: config_hand = default_fr3_hw_gripper_cfg() - g = rcsss.hw.FrankaHand(ip, config_hand) + g = rcs.hw.FrankaHand(ip, config_hand) print("Gripper info:") print("Current normalized width:") print(g.get_normalized_width()) @@ -90,7 +98,7 @@ def shutdown(ip: str, username: str, password: str): d.shutdown() -@dataclasses.dataclass +@dataclass class Token: """ Represents a Desk token owned by a user. @@ -101,7 +109,7 @@ class Token: token: str = "" -class DummyResourceManager: +class ContextManager: def __enter__(self): pass @@ -109,7 +117,7 @@ def __exit__(self, *args): pass -class Desk: +class Desk(ContextManager): """ Connects to the control unit running the web-based Desk interface to manage the robot. Use this class to interact with the Desk @@ -504,7 +512,7 @@ def stop_listen(self) -> None: self._listen_thread.join() -class FCI: +class FCI(ContextManager): """ Can be used as a context manager to activate the Franka Control Interface (FCI). """ @@ -535,7 +543,7 @@ def __exit__(self, *args): self.desk.__exit__() -class GuidingMode: +class GuidingMode(ContextManager): """ Can be used as a context manager to enable or disable guiding mode. """ diff --git a/python/rcsss/envs/__init__.py b/python/rcs/envs/__init__.py similarity index 100% rename from python/rcsss/envs/__init__.py rename to python/rcs/envs/__init__.py diff --git a/python/rcsss/envs/base.py b/python/rcs/envs/base.py similarity index 82% rename from python/rcsss/envs/base.py rename to python/rcs/envs/base.py index c38f7f6b..24554f3f 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcs/envs/base.py @@ -7,14 +7,14 @@ import gymnasium as gym import numpy as np -from rcsss import common, sim -from rcsss._core.common import Robot -from rcsss.camera.interface import BaseCameraSet -from rcsss.envs.space_utils import ( +from rcs import common +from rcs.camera.interface import BaseCameraSet +from rcs.envs.space_utils import ( ActObsInfoWrapper, RCSpaceType, Vec6Type, Vec7Type, + VecType, get_space, get_space_keys, ) @@ -30,7 +30,7 @@ class TRPYDictType(RCSpaceType): gym.spaces.Box( low=np.array([-0.855, -0.855, 0, -np.deg2rad(180), -np.deg2rad(180), -np.deg2rad(180)]), high=np.array([0.855, 0.855, 1.188, np.deg2rad(180), np.deg2rad(180), np.deg2rad(180)]), - dtype=np.float32, + dtype=np.float64, ), ] @@ -38,33 +38,33 @@ class TRPYDictType(RCSpaceType): class LimitedTRPYRelDictType(RCSpaceType): xyzrpy: Annotated[ Vec6Type, - lambda max_cart_mov: gym.spaces.Box( - low=np.array(3 * [-max_cart_mov] + 3 * [-np.deg2rad(180)]), - high=np.array(3 * [max_cart_mov] + 3 * [np.deg2rad(180)]), - dtype=np.float32, + lambda max_cart_mov, max_angle_mov: gym.spaces.Box( + low=np.array(3 * [-max_cart_mov] + 3 * [-max_angle_mov]), + high=np.array(3 * [max_cart_mov] + 3 * [max_angle_mov]), + dtype=np.float64, ), "cart_limits", ] -class TQuartDictType(RCSpaceType): - tquart: Annotated[ +class TQuatDictType(RCSpaceType): + tquat: Annotated[ Vec7Type, gym.spaces.Box( low=np.array([-0.855, -0.855, 0] + [-1] + [-np.inf] * 3), high=np.array([0.855, 0.855, 1.188] + [1] + [np.inf] * 3), - dtype=np.float32, + dtype=np.float64, ), ] -class LimitedTQuartRelDictType(RCSpaceType): - tquart: Annotated[ +class LimitedTQuatRelDictType(RCSpaceType): + tquat: Annotated[ Vec7Type, lambda max_cart_mov: gym.spaces.Box( low=np.array(3 * [-max_cart_mov] + [-1] + [-np.inf] * 3), high=np.array(3 * [max_cart_mov] + [1] + [np.inf] * 3), - dtype=np.float32, + dtype=np.float64, ), "cart_limits", ] @@ -72,22 +72,23 @@ class LimitedTQuartRelDictType(RCSpaceType): class JointsDictType(RCSpaceType): joints: Annotated[ - Vec7Type, - gym.spaces.Box( - low=np.array([-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895]), - high=np.array([2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895]), - dtype=np.float32, + VecType, + lambda low, high: gym.spaces.Box( + low=np.array(low), + high=np.array(high), + dtype=np.float64, ), + "joint_limits", ] class LimitedJointsRelDictType(RCSpaceType): joints: Annotated[ - Vec7Type, - lambda max_joint_mov: gym.spaces.Box( - low=np.array(7 * [-max_joint_mov]), - high=np.array(7 * [max_joint_mov]), - dtype=np.float32, + VecType, + lambda max_joint_mov, dof=7: gym.spaces.Box( + low=np.array(dof * [-max_joint_mov]), + high=np.array(dof * [max_joint_mov]), + dtype=np.float64, ), "joint_limits", ] @@ -119,38 +120,40 @@ class CameraDictType(RCSpaceType): # joining works with inheritance but need to inherit from protocol again -class ArmObsType(TQuartDictType, JointsDictType, TRPYDictType): ... - +class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... -CartOrJointContType: TypeAlias = TQuartDictType | JointsDictType | TRPYDictType -LimitedCartOrJointContType: TypeAlias = LimitedTQuartRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType +CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType +LimitedCartOrJointContType: TypeAlias = LimitedTQuatRelDictType | LimitedJointsRelDictType | LimitedTRPYRelDictType -class ObsArmsGr(ArmObsType, GripperDictType): - pass +class ControlMode(Enum): + JOINTS = auto() + CARTESIAN_TRPY = auto() + CARTESIAN_TQuat = auto() -class ObsArmsGrCam(ArmObsType, GripperDictType, CameraDictType): - pass +def get_dof(robot: common.Robot) -> int: + """Returns the number of degrees of freedom of the robot.""" + return common.robots_meta_config(robot.get_parameters().robot_type).dof -class ObsArmsGrCamCG(ArmObsType, GripperDictType, CameraDictType): - pass +def get_joint_limits(robot: common.Robot) -> tuple[np.ndarray, np.ndarray]: + """Returns the joint limits of the robot. -class ControlMode(Enum): - JOINTS = auto() - CARTESIAN_TRPY = auto() - CARTESIAN_TQuart = auto() + The first element is the lower limit, the second element is the upper limit. + """ + limits = common.robots_meta_config(robot.get_parameters().robot_type).joint_limits + return limits[0], limits[1] -class RobotInstance(Enum): - HARDWARE = auto() - SIMULATION = auto() +def get_home_position(robot: common.Robot) -> np.ndarray: + """Returns the home position of the robot.""" + return common.robots_meta_config(robot.get_parameters().robot_type).q_home -class FR3Env(gym.Env): - """Joint Gym Environment for Franka Research 3. +class RobotEnv(gym.Env): + """Joint Gym Environment for a single robot arm. Top view of on the robot. Robot faces into x direction. z direction faces upwards. (Right handed coordinate axis) @@ -160,24 +163,25 @@ class FR3Env(gym.Env): y """ - def __init__(self, robot: Robot, control_mode: ControlMode): + def __init__(self, robot: common.Robot, control_mode: ControlMode): self.robot = robot self._control_mode_overrides = [control_mode] self.action_space: gym.spaces.Dict self.observation_space: gym.spaces.Dict + low, high = get_joint_limits(self.robot) if control_mode == ControlMode.JOINTS: - self.action_space = get_space(JointsDictType) + self.action_space = get_space(JointsDictType, params={"joint_limits": {"low": low, "high": high}}) elif control_mode == ControlMode.CARTESIAN_TRPY: self.action_space = get_space(TRPYDictType) - elif control_mode == ControlMode.CARTESIAN_TQuart: - self.action_space = get_space(TQuartDictType) + elif control_mode == ControlMode.CARTESIAN_TQuat: + self.action_space = get_space(TQuatDictType) else: msg = "Control mode not recognized!" raise ValueError(msg) - self.observation_space = get_space(ArmObsType) + self.observation_space = get_space(ArmObsType, params={"joint_limits": {"low": low, "high": high}}) self.joints_key = get_space_keys(JointsDictType)[0] self.trpy_key = get_space_keys(TRPYDictType)[0] - self.tquart_key = get_space_keys(TQuartDictType)[0] + self.tquat_key = get_space_keys(TQuatDictType)[0] self.prev_action: dict | None = None def get_unwrapped_control_mode(self, idx: int) -> ControlMode: @@ -199,7 +203,7 @@ def override_control_mode(self, control_mode: ControlMode): def get_obs(self) -> ArmObsType: return ArmObsType( - tquart=np.concatenate( + tquat=np.concatenate( [self.robot.get_cartesian_position().translation(), self.robot.get_cartesian_position().rotation_q()] ), joints=self.robot.get_joint_position(), @@ -209,8 +213,8 @@ def get_obs(self) -> ArmObsType: def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bool, dict]: action_dict = cast(dict, action) if ( - self.get_base_control_mode() == ControlMode.CARTESIAN_TQuart - and self.tquart_key not in action_dict + self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat + and self.tquat_key not in action_dict or self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and self.trpy_key not in action_dict or self.get_base_control_mode() == ControlMode.JOINTS @@ -231,12 +235,12 @@ def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bo self.robot.set_cartesian_position( common.Pose(translation=action_dict[self.trpy_key][:3], rpy_vector=action_dict[self.trpy_key][3:]) ) - elif self.get_base_control_mode() == ControlMode.CARTESIAN_TQuart and ( + elif self.get_base_control_mode() == ControlMode.CARTESIAN_TQuat and ( self.prev_action is None - or not np.allclose(action_dict[self.tquart_key], self.prev_action[self.tquart_key], atol=1e-03, rtol=0) + or not np.allclose(action_dict[self.tquat_key], self.prev_action[self.tquat_key], atol=1e-03, rtol=0) ): self.robot.set_cartesian_position( - common.Pose(translation=action_dict[self.tquart_key][:3], quaternion=action_dict[self.tquart_key][3:]) + common.Pose(translation=action_dict[self.tquat_key][:3], quaternion=action_dict[self.tquat_key][3:]) ) self.prev_action = copy.deepcopy(action_dict) return self.get_obs(), 0, False, False, {} @@ -275,12 +279,12 @@ def __init__( max_mov: float | tuple[float, float] | None = None, ): super().__init__(env) - self.unwrapped: FR3Env + self.unwrapped: RobotEnv self.action_space: gym.spaces.Dict self.relative_to = relative_to if ( self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY - or self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuart + or self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat ): if max_mov is None: max_mov = (self.DEFAULT_MAX_CART_MOV, self.DEFAULT_MAX_CART_ROT) @@ -314,32 +318,41 @@ def __init__( if self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY: assert isinstance(self.max_mov, tuple) self.action_space.spaces.update( - get_space(LimitedTRPYRelDictType, params={"cart_limits": {"max_cart_mov": self.max_mov[0]}}).spaces + get_space( + LimitedTRPYRelDictType, + params={"cart_limits": {"max_cart_mov": self.max_mov[0], "max_angle_mov": self.max_mov[1]}}, + ).spaces ) elif self.unwrapped.get_control_mode() == ControlMode.JOINTS: self.action_space.spaces.update( - get_space(LimitedJointsRelDictType, params={"joint_limits": {"max_joint_mov": self.max_mov}}).spaces + get_space( + LimitedJointsRelDictType, + params={"joint_limits": {"max_joint_mov": self.max_mov, "dof": get_dof(self.unwrapped.robot)}}, + ).spaces ) - elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuart: + elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat: assert isinstance(self.max_mov, tuple) self.action_space.spaces.update( - get_space(LimitedTQuartRelDictType, params={"cart_limits": {"max_cart_mov": self.max_mov[0]}}).spaces + get_space( + LimitedTQuatRelDictType, + params={"cart_limits": {"max_cart_mov": self.max_mov[0]}}, + ).spaces ) else: msg = "Control mode not recognized!" raise ValueError(msg) self.joints_key = get_space_keys(LimitedJointsRelDictType)[0] self.trpy_key = get_space_keys(LimitedTRPYRelDictType)[0] - self.tquart_key = get_space_keys(LimitedTQuartRelDictType)[0] + self.tquat_key = get_space_keys(LimitedTQuatRelDictType)[0] self.initial_obs: dict[str, Any] | None = None - self._origin: common.Pose | Vec7Type | None = None - self._last_action: common.Pose | Vec7Type | None = None + self._origin: common.Pose | VecType | None = None + self._last_action: common.Pose | VecType | None = None - def set_origin(self, origin: common.Pose | Vec7Type): + def set_origin(self, origin: common.Pose | VecType): if self.unwrapped.get_control_mode() == ControlMode.JOINTS: assert isinstance( origin, np.ndarray - ), "Invalid origin type. If control mode is joints, origin must be Vec7Type." + ), "Invalid origin type. If control mode is joints, origin must be VecType." self._origin = copy.deepcopy(origin) else: assert isinstance( @@ -369,7 +382,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: if self.unwrapped.get_control_mode() == ControlMode.JOINTS and self.joints_key in action: assert isinstance(self._origin, np.ndarray), "Invalid origin type give the control mode." assert isinstance(self.max_mov, float) - joint_space = cast(gym.spaces.Box, get_space(JointsDictType).spaces[self.joints_key]) + low, high = get_joint_limits(self.unwrapped.robot) # TODO: should we also clip euqally for all joints? if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None: limited_joints = np.clip(action[self.joints_key], -self.max_mov, self.max_mov) @@ -379,9 +392,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: limited_joints_diff = np.clip(joints_diff, -self.max_mov, self.max_mov) limited_joints = limited_joints_diff + self._last_action self._last_action = limited_joints - action.update( - JointsDictType(joints=np.clip(self._origin + limited_joints, joint_space.low, joint_space.high)) - ) + action.update(JointsDictType(joints=np.clip(self._origin + limited_joints, low, high))) elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY and self.trpy_key in action: assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode." @@ -427,16 +438,16 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: ) ) ) - elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuart and self.tquart_key in action: + elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuat and self.tquat_key in action: assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode." assert isinstance(self.max_mov, tuple) - pose_space = cast(gym.spaces.Box, get_space(TQuartDictType).spaces[self.tquart_key]) + pose_space = cast(gym.spaces.Box, get_space(TQuatDictType).spaces[self.tquat_key]) if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None: clipped_pose_offset = ( common.Pose( - translation=action[self.tquart_key][:3], - quaternion=action[self.tquart_key][3:], + translation=action[self.tquat_key][:3], + quaternion=action[self.tquat_key][3:], ) .limit_translation_length(self.max_mov[0]) .limit_rotation_angle(self.max_mov[1]) @@ -446,8 +457,8 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: assert isinstance(self._last_action, common.Pose) pose_diff = ( common.Pose( - translation=action[self.tquart_key][:3], - quaternion=action[self.tquart_key][3:], + translation=action[self.tquat_key][:3], + quaternion=action[self.tquat_key][3:], ) * self._last_action.inverse() ) @@ -463,8 +474,8 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: ) action.update( - TQuartDictType( - tquart=np.concatenate( + TQuatDictType( + tquat=np.concatenate( [ np.clip(unclipped_pose.translation(), pose_space.low[:3], pose_space.high[:3]), unclipped_pose.rotation_q(), @@ -484,7 +495,7 @@ class CameraSetWrapper(ActObsInfoWrapper): def __init__(self, env, camera_set: BaseCameraSet, include_depth: bool = False): super().__init__(env) - self.unwrapped: FR3Env + self.unwrapped: RobotEnv self.camera_set = camera_set self.include_depth = include_depth @@ -575,7 +586,7 @@ class GripperWrapper(ActObsInfoWrapper): def __init__(self, env, gripper: common.Gripper, binary: bool = True): super().__init__(env) - self.unwrapped: FR3Env + self.unwrapped: RobotEnv self.observation_space: gym.spaces.Dict self.observation_space.spaces.update(get_space(GripperDictType).spaces) self.action_space: gym.spaces.Dict @@ -599,12 +610,6 @@ def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tupl else: observation[self.gripper_key] = self._gripper.get_normalized_width() - # TODO: a cleaner solution would be to put this code into env/sim.py - # similar to sim fr3 has also a sim specific wrapper - if isinstance(self._gripper, sim.FrankaHand): - state = self._gripper.get_state() - if "collision" not in info or not info["collision"]: - info["collision"] = state.collision return observation, info def action(self, action: dict[str, Any]) -> dict[str, Any]: diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py new file mode 100644 index 00000000..43d3e93e --- /dev/null +++ b/python/rcs/envs/creators.py @@ -0,0 +1,336 @@ +import logging +from os import PathLike +from typing import Type + +import gymnasium as gym +import numpy as np +import rcs +from gymnasium.envs.registration import EnvCreator +from rcs import sim +from rcs._core.sim import CameraType +from rcs.camera.hw import BaseHardwareCameraSet +from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig +from rcs.envs.base import ( + CameraSetWrapper, + ControlMode, + GripperWrapper, + RelativeActionSpace, + RelativeTo, + RobotEnv, +) +from rcs.envs.hw import FR3HW +from rcs.envs.sim import ( + CamRobot, + CollisionGuard, + FR3Sim, + GripperWrapperSim, + PickCubeSuccessWrapper, + RandomCubePos, + SimWrapper, +) +from rcs.envs.space_utils import VecType +from rcs.envs.utils import ( + default_fr3_hw_gripper_cfg, + default_fr3_hw_robot_cfg, + default_fr3_sim_gripper_cfg, + default_fr3_sim_robot_cfg, + default_realsense, + get_urdf_path, +) + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class RCSHardwareEnvCreator(EnvCreator): + pass + + +class RCSFR3EnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + ip: str, + control_mode: ControlMode, + robot_cfg: rcs.hw.FR3Config, + collision_guard: str | PathLike | None = None, + gripper_cfg: rcs.hw.FHConfig | None = None, + camera_set: BaseHardwareCameraSet | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + urdf_path: str | PathLike | None = None, + ) -> gym.Env: + """ + Creates a hardware environment for the FR3 robot. + + Args: + ip (str): IP address of the robot. + control_mode (ControlMode): Control mode for the robot. + robot_cfg (rcs.hw.FR3Config): Configuration for the FR3 robot. + collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) + or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. + gripper_cfg (rcs.hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. + camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. + max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts + translational movement in meters. If tuple, it restricts both translational (in meters) and rotational + (in radians) movements. If None, no restriction is applied. + relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. + urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced + which requires a UTN compatible lab scene to be present. If no URDF file is found, the environment will + still work but set_cartesian methods might throw an error. A URDF file is needed for collision guarding. + + Returns: + gym.Env: The configured hardware environment for the FR3 robot. + """ + urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=collision_guard is not None) + ik = rcs.common.IK(str(urdf_path)) if urdf_path is not None else None + robot = rcs.hw.FR3(ip, ik) + robot.set_parameters(robot_cfg) + + env: gym.Env = RobotEnv(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) + + env = FR3HW(env) + if gripper_cfg is not None: + gripper = rcs.hw.FrankaHand(ip, gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + + if camera_set is not None: + camera_set.start() + camera_set.wait_for_frames() + logger.info("CameraSet started") + env = CameraSetWrapper(env, camera_set) + + if collision_guard is not None: + assert urdf_path is not None + env = CollisionGuard.env_from_xml_paths( + env, + str(rcs.scenes.get(str(collision_guard), collision_guard)), + urdf_path, + gripper=True, + check_home_collision=False, + control_mode=control_mode, + tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), + sim_gui=True, + truncate_on_collision=False, + ) + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env + + +class RCSFR3DefaultEnvCreator(RCSHardwareEnvCreator): + def __call__( # type: ignore + self, + robot_ip: str, + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + delta_actions: bool = True, + camera_config: dict[str, str] | None = None, + gripper: bool = True, + ) -> gym.Env: + camera_set = default_realsense(camera_config) + return RCSFR3EnvCreator()( + ip=robot_ip, + camera_set=camera_set, + control_mode=control_mode, + robot_cfg=default_fr3_hw_robot_cfg(), + collision_guard=None, + gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None, + max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, + relative_to=RelativeTo.LAST_STEP, + ) + + +class RCSSimEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + control_mode: ControlMode, + robot_cfg: rcs.sim.SimRobotConfig, + collision_guard: bool = False, + gripper_cfg: rcs.sim.SimGripperConfig | None = None, + camera_set_cfg: SimCameraSetConfig | None = None, + max_relative_movement: float | tuple[float, float] | None = None, + relative_to: RelativeTo = RelativeTo.LAST_STEP, + urdf_path: str | PathLike | None = None, + mjcf: str | PathLike = "fr3_empty_world", + sim_wrapper: Type[SimWrapper] | None = None, + ) -> gym.Env: + """ + Creates a simulation environment for the FR3 robot. + + Args: + control_mode (ControlMode): Control mode for the robot. + robot_cfg (rcs.sim.SimRobotConfig): Configuration for the FR3 robot. + collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. + gripper_cfg (rcs.sim.SimGripperConfig | None): Configuration for the gripper. If None, no gripper is used. + camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. + max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts + translational movement in meters. If tuple, it restricts both translational (in meters) and rotational + (in radians) movements. If None, no restriction is applied. + relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. + urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced + which requires a UTN compatible lab scene to be present. + mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". + sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful + for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. + + Returns: + gym.Env: The configured simulation environment for the FR3 robot. + """ + urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=False) + assert urdf_path is not None + if mjcf not in rcs.scenes: + logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") + mjb_file = rcs.scenes.get(str(mjcf), mjcf) + simulation = sim.Sim(mjb_file) + + ik = rcs.common.IK(urdf_path) + robot = rcs.sim.SimRobot(simulation, "0", ik) + robot.set_parameters(robot_cfg) + env: gym.Env = RobotEnv(robot, control_mode) + env = FR3Sim(env, simulation, sim_wrapper) + + if camera_set_cfg is not None: + camera_set = SimCameraSet(simulation, camera_set_cfg) + env = CameraSetWrapper(env, camera_set, include_depth=True) + + if gripper_cfg is not None: + gripper = sim.SimGripper(simulation, "0", gripper_cfg) + env = GripperWrapper(env, gripper, binary=True) + env = GripperWrapperSim(env, gripper) + + if collision_guard: + env = CollisionGuard.env_from_xml_paths( + env, + str(rcs.scenes.get(str(mjcf), mjcf)), + urdf_path, + gripper=gripper_cfg is not None, + check_home_collision=False, + control_mode=control_mode, + tcp_offset=rcs.common.Pose(rcs.common.FrankaHandTCPOffset()), + sim_gui=True, + truncate_on_collision=True, + ) + if max_relative_movement is not None: + env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) + + return env + + +class SimTaskEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + mjcf: str, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + delta_actions: bool = True, + camera_cfg: SimCameraSetConfig | None = None, + ) -> gym.Env: + + env_rel = RCSSimEnvCreator()( + control_mode=control_mode, + robot_cfg=default_fr3_sim_robot_cfg(mjcf), + collision_guard=False, + gripper_cfg=default_fr3_sim_gripper_cfg(), + camera_set_cfg=camera_cfg, + max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, + relative_to=RelativeTo.LAST_STEP, + mjcf=mjcf, + sim_wrapper=RandomCubePos, + ) + env_rel = PickCubeSuccessWrapper(env_rel) + if render_mode == "human": + env_rel.get_wrapper_attr("sim").open_gui() + + return env_rel + + +class FR3SimplePickUpSimEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 0, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = { + "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), + "bird_eye": SimCameraConfig(identifier="bird_eye_cam", type=int(CameraType.fixed)), + "side": SimCameraConfig(identifier="side_view", type=int(CameraType.fixed)), + "right_side": SimCameraConfig(identifier="right_side", type=int(CameraType.fixed)), + "left_side": SimCameraConfig(identifier="left_side", type=int(CameraType.fixed)), + "front": SimCameraConfig(identifier="front", type=int(CameraType.fixed)), + } + + camera_cfg = SimCameraSetConfig( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + return SimTaskEnvCreator()("fr3_simple_pick_up", render_mode, control_mode, delta_actions, camera_cfg) + + +class FR3SimplePickUpSimDigitHandEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 0, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = {"wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed))} + + camera_cfg = SimCameraSetConfig( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + return SimTaskEnvCreator()( + "fr3_simple_pick_up_digit_hand", render_mode, control_mode, delta_actions, camera_cfg + ) + + +class FR3LabPickUpSimDigitHandEnvCreator(EnvCreator): + def __call__( # type: ignore + self, + cam_robot_joints: VecType, + render_mode: str = "human", + control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, + resolution: tuple[int, int] | None = None, + frame_rate: int = 0, + delta_actions: bool = True, + ) -> gym.Env: + if resolution is None: + resolution = (256, 256) + + cameras = { + "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), + "side": SimCameraConfig(identifier="wrist_1", type=int(CameraType.fixed)), + } + + camera_cfg = SimCameraSetConfig( + cameras=cameras, + resolution_width=resolution[0], + resolution_height=resolution[1], + frame_rate=frame_rate, + physical_units=True, + ) + env_rel = SimTaskEnvCreator()( + "lab_simple_pick_up_digit_hand", + render_mode, + control_mode, + delta_actions, + camera_cfg, + ) + return CamRobot(env_rel, cam_robot_joints, "lab_simple_pick_up_digit_hand") diff --git a/python/rcsss/envs/hw.py b/python/rcs/envs/hw.py similarity index 90% rename from python/rcsss/envs/hw.py rename to python/rcs/envs/hw.py index d55f00db..12ea76b3 100644 --- a/python/rcsss/envs/hw.py +++ b/python/rcs/envs/hw.py @@ -2,8 +2,8 @@ from typing import Any, SupportsFloat, cast import gymnasium as gym -from rcsss import hw -from rcsss.envs.base import FR3Env +from rcs import hw +from rcs.envs.base import RobotEnv _logger = logging.getLogger(__name__) @@ -11,7 +11,7 @@ class FR3HW(gym.Wrapper): def __init__(self, env): super().__init__(env) - self.unwrapped: FR3Env + self.unwrapped: RobotEnv assert isinstance(self.unwrapped.robot, hw.FR3), "Robot must be a hw.FR3 instance." self.hw_robot = cast(hw.FR3, self.unwrapped.robot) @@ -22,7 +22,7 @@ def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool, _logger.error("FrankaControlException: %s", e) self.hw_robot.automatic_error_recovery() # TODO: this does not work if some wrappers are in between - # FR3HW and FR3Env + # FR3HW and RobotEnv return dict(self.unwrapped.get_obs()), 0, False, True, {} def reset( diff --git a/python/rcsss/envs/sim.py b/python/rcs/envs/sim.py similarity index 72% rename from python/rcsss/envs/sim.py rename to python/rcs/envs/sim.py index 3112ae7e..eae055b6 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -3,19 +3,25 @@ import gymnasium as gym import numpy as np -import rcsss -from rcsss import sim -from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper +import rcs +from rcs import sim +from rcs.envs.base import ControlMode, GripperWrapper, RobotEnv +from rcs.envs.space_utils import ActObsInfoWrapper, VecType +from rcs.envs.utils import default_fr3_sim_robot_cfg logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) class SimWrapper(gym.Wrapper): + """A sub class of this wrapper can be passed to FR3Sim to assure that its code is called before + step_until_convergence() is called. + """ + def __init__(self, env: gym.Env, simulation: sim.Sim): super().__init__(env) - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.unwrapped: RobotEnv + assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." self.sim = simulation @@ -25,9 +31,9 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non if sim_wrapper is not None: env = sim_wrapper(env, simulation) super().__init__(env) - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - self.sim_robot = cast(sim.FR3, self.unwrapped.robot) + self.unwrapped: RobotEnv + assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." + self.sim_robot = cast(sim.SimRobot, self.unwrapped.robot) self.sim = simulation def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]: @@ -52,6 +58,18 @@ def reset( return obs, info +class GripperWrapperSim(ActObsInfoWrapper): + def __init__(self, env, gripper: sim.SimGripper): + super().__init__(env) + self._gripper = gripper + + def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: + state = self._gripper.get_state() + if "collision" not in info or not info["collision"]: + info["collision"] = state.collision + return observation, info + + class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]): """ - Gripper Wrapper has to be added before this as it removes the gripper action @@ -69,7 +87,7 @@ def __init__( truncate_on_collision: bool = True, ): super().__init__(env) - self.unwrapped: FR3Env + self.unwrapped: RobotEnv self.collision_env = collision_env self.sim = simulation self.last_obs: tuple[dict[str, Any], dict[str, Any]] | None = None @@ -93,7 +111,7 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, b if self.to_joint_control: fr3_env = self.collision_env.unwrapped - assert isinstance(fr3_env, FR3Env), "Collision env must be an FR3Env instance." + assert isinstance(fr3_env, RobotEnv), "Collision env must be an RobotEnv instance." action[self.unwrapped.joints_key] = fr3_env.robot.get_joint_position() if info["collision"]: @@ -135,16 +153,16 @@ def env_from_xml_paths( id: str = "0", gripper: bool = True, check_home_collision: bool = True, - tcp_offset: rcsss.common.Pose | None = None, + tcp_offset: rcs.common.Pose | None = None, control_mode: ControlMode | None = None, sim_gui: bool = True, truncate_on_collision: bool = True, ) -> "CollisionGuard": - assert isinstance(env.unwrapped, FR3Env) + assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) - ik = rcsss.common.IK(urdf, max_duration_ms=300) - robot = rcsss.sim.FR3(simulation, id, ik) - cfg = sim.FR3Config() + ik = rcs.common.IK(urdf, max_duration_ms=300) + robot = rcs.sim.SimRobot(simulation, id, ik) + cfg = sim.SimRobotConfig() cfg.realtime = False if tcp_offset is not None: cfg.tcp_offset = tcp_offset @@ -159,12 +177,13 @@ def env_from_xml_paths( to_joint_control = True else: control_mode = env.unwrapped.get_control_mode() - c_env: gym.Env = FR3Env(robot, control_mode) + c_env: gym.Env = RobotEnv(robot, control_mode) c_env = FR3Sim(c_env, simulation) if gripper: - gripper_cfg = sim.FHConfig() - fh = sim.FrankaHand(simulation, id, gripper_cfg) + gripper_cfg = sim.SimGripperConfig() + fh = sim.SimGripper(simulation, id, gripper_cfg) c_env = GripperWrapper(c_env, fh) + c_env = GripperWrapperSim(c_env, fh) return cls( env=env, simulation=simulation, @@ -186,7 +205,7 @@ def reset( self.sim.step(1) iso_cube = np.array([0.498, 0.0, 0.226]) - iso_cube_pose = rcsss.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) + iso_cube_pose = rcs.common.Pose(translation=np.array(iso_cube), rpy_vector=np.array([0, 0, 0])) iso_cube = self.unwrapped.robot.to_pose_in_world_coordinates(iso_cube_pose).translation() pos_z = 0.826 pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 @@ -204,8 +223,8 @@ class PickCubeSuccessWrapper(gym.Wrapper): def __init__(self, env): super().__init__(env) - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." + self.unwrapped: RobotEnv + assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." self.sim = env.get_wrapper_attr("sim") def step(self, action: dict[str, Any]): @@ -221,3 +240,28 @@ def step(self, action: dict[str, Any]): reward = -diff_cube_home - diff_ee_cube return obs, reward, success, truncated, info + + +class CamRobot(gym.Wrapper): + """Use this wrapper in lab setups where the second arm is used as a camera holder.""" + + def __init__(self, env, cam_robot_joints: VecType, scene: str = "lab_simple_pick_up_digit_hand"): + super().__init__(env) + self.unwrapped: RobotEnv + assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." + self.sim = env.get_wrapper_attr("sim") + self.cam_robot = rcs.sim.SimRobot( + self.sim, "1", env.unwrapped.robot.get_ik(), register_convergence_callback=False + ) + self.cam_robot.set_parameters(default_fr3_sim_robot_cfg(scene)) + self.cam_robot_joints = cam_robot_joints + + def step(self, action: dict): + self.cam_robot.set_joints_hard(self.cam_robot_joints) + obs, reward, done, truncated, info = super().step(action) + return obs, reward, done, truncated, info + + def reset(self, *, seed=None, options=None): + re = super().reset(seed=seed, options=options) + self.cam_robot.reset() + return re diff --git a/python/rcsss/envs/space_utils.py b/python/rcs/envs/space_utils.py similarity index 99% rename from python/rcsss/envs/space_utils.py rename to python/rcs/envs/space_utils.py index cf9c28b1..62909c53 100644 --- a/python/rcsss/envs/space_utils.py +++ b/python/rcs/envs/space_utils.py @@ -6,6 +6,7 @@ Type, TypeAlias, TypedDict, + TypeVar, get_args, get_origin, get_type_hints, @@ -14,6 +15,8 @@ import gymnasium as gym import numpy as np +M = TypeVar("M", bound=int) +VecType: TypeAlias = np.ndarray[M, np.dtype[np.float64]] Vec7Type: TypeAlias = np.ndarray[Literal[7], np.dtype[np.float64]] Vec3Type: TypeAlias = np.ndarray[Literal[3], np.dtype[np.float64]] Vec6Type: TypeAlias = np.ndarray[Literal[6], np.dtype[np.float64]] diff --git a/python/rcsss/envs/utils.py b/python/rcs/envs/utils.py similarity index 77% rename from python/rcsss/envs/utils.py rename to python/rcs/envs/utils.py index 19bba80f..2701efca 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -4,20 +4,20 @@ import mujoco as mj import numpy as np -import rcsss -from rcsss import sim -from rcsss._core.hw import FR3Config, IKSolver -from rcsss._core.sim import CameraType -from rcsss.camera.interface import BaseCameraConfig -from rcsss.camera.realsense import RealSenseCameraSet, RealSenseSetConfig -from rcsss.camera.sim import SimCameraConfig, SimCameraSetConfig +import rcs +from rcs import sim +from rcs._core.hw import FR3Config, IKSolver +from rcs._core.sim import CameraType +from rcs.camera.interface import BaseCameraConfig +from rcs.camera.realsense import RealSenseCameraSet, RealSenseSetConfig +from rcs.camera.sim import SimCameraConfig, SimCameraSetConfig logger = logging.getLogger(__name__) logger.setLevel(logging.INFO) -def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.FR3Config: - cfg = sim.FR3Config() +def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.SimRobotConfig: + cfg = sim.SimRobotConfig() cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False return cfg @@ -25,15 +25,15 @@ def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.FR3Co def default_fr3_hw_robot_cfg(async_control: bool = False): robot_cfg = FR3Config() - robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) robot_cfg.speed_factor = 0.1 - robot_cfg.ik_solver = IKSolver.rcs + robot_cfg.ik_solver = IKSolver.rcs_ik robot_cfg.async_control = async_control return robot_cfg def default_fr3_hw_gripper_cfg(async_control: bool = False): - gripper_cfg = rcsss.hw.FHConfig() + gripper_cfg = rcs.hw.FHConfig() gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 gripper_cfg.force = 30 @@ -58,7 +58,7 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No def default_fr3_sim_gripper_cfg(): - return sim.FHConfig() + return sim.SimGripperConfig() def default_mujoco_cameraset_cfg(): @@ -82,28 +82,28 @@ def get_tcp_offset(mjcf: str | Path): mjcf (str | PathLike): Path to the mjcf file. Returns: - rcsss.common.Pose: The tcp offset. + rcs.common.Pose: The tcp offset. """ if isinstance(mjcf, str): mjcf = Path(mjcf) - mjmdl = rcsss.scenes.get(str(mjcf), mjcf) + mjmdl = rcs.scenes.get(str(mjcf), mjcf) if mjmdl.suffix == ".xml": model = mj.MjModel.from_xml_path(str(mjmdl)) elif mjmdl.suffix == ".mjb": model = mj.MjModel.from_binary_path(str(mjmdl)) try: tcp_offset = np.array(model.numeric("tcp_offset").data) - pose_offset = rcsss.common.Pose(translation=tcp_offset) - return rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) * pose_offset + pose_offset = rcs.common.Pose(translation=tcp_offset) + return rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) * pose_offset except KeyError: msg = "No tcp offset found in the model. Using the default tcp offset." logging.warning(msg) - return rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) + return rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: - if urdf_path is None and "lab" in rcsss.scenes: - urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" + if urdf_path is None and "lab" in rcs.scenes: + urdf_path = rcs.scenes["lab"].parent / "fr3.urdf" assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." logger.info("Using automatic found urdf.") elif urdf_path is None and not allow_none_if_not_found: diff --git a/python/rcsss/sim.py b/python/rcs/sim.py similarity index 73% rename from python/rcsss/sim.py rename to python/rcs/sim.py index 4200b589..ae7fa550 100644 --- a/python/rcsss/sim.py +++ b/python/rcs/sim.py @@ -6,11 +6,18 @@ from typing import Optional import mujoco as mj -from rcsss._core.sim import FR3, FHConfig, FHState, FR3Config, FR3State, FrankaHand -from rcsss._core.sim import Sim as _Sim -from rcsss._core.sim import open_gui_window as _open_gui_window - -__all__ = ["Sim", "FR3", "FR3Config", "FR3State", "FHConfig", "FHState", "FrankaHand"] +from rcs._core.sim import Sim as _Sim +from rcs._core.sim import ( + SimGripper, + SimGripperConfig, + SimGripperState, + SimRobot, + SimRobotConfig, + SimRobotState, +) +from rcs._core.sim import open_gui_window as _open_gui_window + +__all__ = ["Sim", "SimRobot", "SimRobotConfig", "SimRobotState", "SimGripper", "SimGripperConfig", "SimGripperState"] logger = getLogger(__name__) @@ -36,7 +43,7 @@ def __init__(self, mjmdl: str | PathLike): def open_gui(self): if self._gui_uuid is None: - self._gui_uuid = "rcsss_" + str(uuid.uuid4()) + self._gui_uuid = "rcs_" + str(uuid.uuid4()) self._start_gui_server(self._gui_uuid) self._mp_context.Process(target=_start_gui, args=(self._gui_uuid,), daemon=True).start() diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py deleted file mode 100644 index 63d4cf6f..00000000 --- a/python/rcsss/__init__.py +++ /dev/null @@ -1,42 +0,0 @@ -"""Robot control stack python bindings.""" - -import pathlib -import site - -from gymnasium import register -from rcsss import camera, control, envs, sim -from rcsss._core import __version__, common, hw -from rcsss.envs.factories import ( - FR3LabPickUpSimDigitHand, - FR3Real, - FR3SimplePickUpSim, - FR3SimplePickUpSimDigitHand, -) - -# available mujoco scenes -scenes = { - path.stem: path / "scene.mjb" for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*") -} - -# make submodules available -__all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] - -# register gymnasium environments -register( - id="rcs/SimplePickUpSim-v0", - entry_point=FR3SimplePickUpSim(), -) - -register( - id="rcs/SimplePickUpSimDigitHand-v0", - entry_point=FR3SimplePickUpSimDigitHand(), -) -register( - id="rcs/LabPickUpSimDigitHand-v0", - entry_point=FR3LabPickUpSimDigitHand(), -) - -register( - id="rcs/FR3Real-v0", - entry_point=FR3Real(), -) diff --git a/python/rcsss/control/keyboard_control.py b/python/rcsss/control/keyboard_control.py deleted file mode 100644 index 11d8d049..00000000 --- a/python/rcsss/control/keyboard_control.py +++ /dev/null @@ -1,127 +0,0 @@ -import logging -from threading import Lock -from time import sleep - -import numpy as np -from pynput import keyboard -from rcsss.control.fr3_desk import FCI, Desk -from rcsss.control.utils import load_creds_fr3_desk -from rcsss.envs.base import ControlMode, RelativeTo -from rcsss.envs.factories import fr3_hw_env -from rcsss.envs.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -ROBOT_IP = "192.168.101.1" -VALLID_KEYS = {"w", "a", "s", "d", "i", "k", "g"} -KEYMAP = {"w": (0, +1), "a": (1, -1), "s": (0, -1), "d": (1, +1), "i": (2, +1), "k": (2, -1)} - - -class RobotControl: - def __init__(self, env): - self._env = env - self._lock = Lock() - self._gripper_state = 1 - self._mode_rpy = False - self._keys = set() - self._exit = False - - self._tstep = 0.1 - self._astep = 0.1 - self._action_scale = 1.0 - self._scale_step = 0.1 - - def on_press(self, key: keyboard.Key): - if hasattr(key, "char") and key.char in VALLID_KEYS and key.char != "g": - with self._lock: - self._keys.add(key.char) - - def on_release(self, key: keyboard.Key): - with self._lock: - if key is keyboard.Key.alt_l: - self._mode_rpy = not self._mode_rpy - if self._mode_rpy: - print("Switching mode to RPY") - else: - print("Switching mode to XYZ") - elif key is keyboard.Key.page_up: - self._action_scale = min(1.0, self._action_scale + self._scale_step) - print(f"New speed factor: {self._action_scale:.2f}") - elif key is keyboard.Key.page_down: - self._action_scale = max(0.0, self._action_scale - self._scale_step) - print(f"New speed factor: {self._action_scale:.2f}") - elif key is keyboard.Key.esc: - self._exit = True - print("Exiting...") - elif hasattr(key, "char") and key.char in VALLID_KEYS: - if key.char == "g": - self._gripper_state = not self._gripper_state - else: - self._keys.remove(key.char) - - def get_keyboard_action(self): - action = np.zeros(7) - done = False - - with self._lock: - if not self._mode_rpy: - for k in self._keys: - i, sgn = KEYMAP[k] - action[i] = sgn * self._tstep - else: - # Rotation - offset = 3 - for k in self._keys: - i, sgn = KEYMAP[k] - action[offset + i] = sgn * self._astep - - action[:6] *= self._action_scale - action[6] = self._gripper_state - done = self._exit - - return action, done - - def step(self, action) -> tuple[bool, list[str], dict]: - # TODO Check if the model indicates when an action is finished. - obs, _, _, truncated, info = self._env.step({"xyzrpy": action[:6], "gripper": action[6]}) - return obs - - def loop(self): - # Initialize the environment and obtain the initial observation - _, _ = self._env.reset() - - sleep(5) - _, _, _, truncated, info = self._env.step({"xyzrpy": np.array([0, 0, 0, 0, 0, 0]), "gripper": 1}) - - print("Enter action (WASD: x/y plane, IK: z - ALT for mode switch - WASD: roll, pitch, IK: yaw - g: gripper)") - done = False - - while not done: - action, done = self.get_keyboard_action() - _ = self.step(action) - - -def main(): - user, pw = load_creds_fr3_desk() - d = Desk(ROBOT_IP, user, pw) - with FCI(d, unlock=False, lock_when_done=False): - env = fr3_hw_env( - ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TRPY, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=0.2, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - controller = RobotControl(env) - listener = keyboard.Listener(on_press=controller.on_press, on_release=controller.on_release) - listener.start() - controller.loop() - listener.stop() - - -if __name__ == "__main__": - main() diff --git a/python/rcsss/control/liveplot.py b/python/rcsss/control/liveplot.py deleted file mode 100644 index a99b3813..00000000 --- a/python/rcsss/control/liveplot.py +++ /dev/null @@ -1,69 +0,0 @@ -from collections import deque -from itertools import product -from socket import AF_INET, SOCK_DGRAM, socket -from struct import unpack -from typing import Optional - -import matplotlib.pyplot as plt -import numpy as np -from scipy.spatial.transform import Rotation as R - -host = "127.0.0.1" -port = 54322 -FMT = "!" + 7 * "d" + "i" - -# buffsize = 30 -buffsize = 300 -# Order of buffers: x, y, z, rx, ry, rz - -titles = [["x", "rx"], ["y", "ry"], ["z", "rz"]] -colors = [["r", "r"], ["b", "b"], ["g", "g"]] -buffers: list[list[deque]] = [[deque(maxlen=buffsize) for j in range(2)] for i in range(3)] -plots: list[list[Optional[plt.Figure]]] = [[None for j in range(2)] for i in range(3)] - -fig, axs = plt.subplots(3, 2) -for i, j in product(range(3), range(2)): - buffer = buffers[i][j] - (plots[i][j],) = axs[i][j].plot(np.arange(len(buffer)), buffer, c=colors[i][j]) - axs[i][j].set_title(titles[i][j]) - axs[i][j].set_xticklabels([]) - -fig.tight_layout() -plt.show(block=False) - - -with socket(AF_INET, SOCK_DGRAM) as sock: - sock.settimeout(2) - sock.bind((host, port)) - while True: - try: - unpacked = unpack(FMT, sock.recv(7 * 8 + 4)) - except TimeoutError: - continue - - last_controller_pose_raw = np.ctypeslib.as_array(unpacked[:7]) - rotation = last_controller_pose_raw[:4] - translation = last_controller_pose_raw[4:] - - rot_q = R.from_quat(rotation) - rot_euler = rot_q.as_euler("xyz", degrees=True) - - buffers[0][0].append(translation[0]) - buffers[0][1].append(rot_euler[0]) - buffers[1][0].append(translation[1]) - buffers[1][1].append(rot_euler[1]) - buffers[2][0].append(translation[2]) - buffers[2][1].append(rot_euler[2]) - - for i, j in product(range(3), range(2)): - buffer = buffers[i][j] - # mypy fails to coerce type after assert so we just ignore - plots[i][j].set_xdata(np.arange(len(buffer))) # type: ignore - plots[i][j].set_ydata(buffer) # type: ignore - axs[i][j].relim() - axs[i][j].autoscale_view() - axs[i][j].set_title(titles[i][j] + ": {:.2f}".format(buffer[-1])) - - plt.draw() - plt.pause(0.001) - # plt.pause(0.5) diff --git a/python/rcsss/control/record.py b/python/rcsss/control/record.py deleted file mode 100644 index b8be3410..00000000 --- a/python/rcsss/control/record.py +++ /dev/null @@ -1,311 +0,0 @@ -import logging -from abc import ABC, abstractmethod -from time import sleep -from typing import cast - -import numpy as np -from rcsss import common, hw - -np.set_printoptions(precision=27) - -_logger = logging.getLogger("record") - - -class Pose(ABC): - @abstractmethod - def replay(self, robot: dict[str, hw.FR3], gripper: dict[str, hw.FrankaHand]): - pass - - @abstractmethod - def __str__(self) -> str: - pass - - @classmethod - @abstractmethod - def from_str(cls, line: str) -> "Pose": - pass - - -class JointPose(Pose): - def __init__(self, name: str, pose: np.ndarray | None = None): - self.pose = pose - self.name = name - - def record(self, robot: dict[str, hw.FR3]): - self.pose = robot[self.name].get_joint_position() - - def replay(self, robot: dict[str, hw.FR3], _: dict[str, hw.FrankaHand]): - if self.pose is not None: - robot[self.name].set_joint_position(self.pose) - - def __str__(self) -> str: - if self.pose is None: - return f"JointPose;{self.name};None" - ps = np.array2string(self.pose).replace("\n", "") - return f"JointPose;{self.name};{ps}" - - @classmethod - def from_str(cls, line: str) -> "Pose": - l = line.replace("\n", "").split(";") - return cls(l[1], np.fromstring(l[2].strip("[]"), sep=" ")) - - -class GripperPose(Pose): - SPEED = 0.1 - FORCE = 5 - EPSILON = 0.2 - - def __init__(self, name: str, pose: float | None = None): - self.pose = pose - self.name = name - - def record(self, shut: bool, gripper: dict[str, hw.FrankaHand]): - if shut: - config = hw.FHConfig() - config.speed = self.SPEED - config.force = self.FORCE - config.grasping_width = 0 - config.epsilon_inner = self.EPSILON - config.epsilon_outer = self.EPSILON - gripper[self.name].set_parameters(config) - gripper[self.name].grasp() - self.pose = 0 # gripper[self.name].getState()[1] - - else: - gripper[self.name].open() - self.pose = None - - def replay(self, _: dict[str, hw.FR3], gripper: dict[str, hw.FrankaHand]): - if self.pose: - config = hw.FHConfig() - config.speed = self.SPEED - config.force = self.FORCE - config.grasping_width = self.pose - config.epsilon_inner = self.EPSILON - config.epsilon_outer = self.EPSILON - - gripper[self.name].set_parameters(config) - gripper[self.name].grasp() - else: - gripper[self.name].open() - - def __str__(self) -> str: - return f"GripperPose;{self.name};{self.pose}" - - @classmethod - def from_str(cls, line: str) -> Pose: - l = line.replace("\n", "").split(";") - return cls(l[1], float(l[2]) if l[2] != "None" else None) - - -class WaitForInput(Pose): - def __init__(self): - pass - - def record(self): - pass - - def replay(self, *args): # noqa: ARG002 - input("Press enter to continue") - - def __str__(self) -> str: - return "WaitForInput" - - @classmethod - def from_str(cls, line: str) -> Pose: # noqa: ARG003 - return cls() - - -class WaitForDoubleTab(Pose): - def __init__(self, name: str): - self.name = name - - def record(self): - pass - - def replay(self, robot: dict[str, hw.FR3], _: dict[str, hw.FrankaHand]): - robot[self.name].double_tap_robot_to_continue() - - def __str__(self) -> str: - return f"WaitForDoubleTab;{self.name}" - - @classmethod - def from_str(cls, line: str) -> "Pose": - l = line.replace("\n", "").split(";") - return cls(l[1]) - - -class Sleep(Pose): - def __init__(self, t): - self.t = t - - def record(self): - pass - - def replay(self, *args): # noqa: ARG002 - sleep(self.t) - - def __str__(self) -> str: - return f"Sleep;{self.t}" - - @classmethod - def from_str(cls, line: str) -> Pose: - l = line.replace("\n", "").split(";") - return cls(float(l[1])) - - -def check_pose(pose: np.ndarray): - if np.all(pose <= np.array([2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895])) and np.all( - np.array([-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895]) <= pose - ): - return True - return False - - -class ChnageSpeedFactor(Pose): - def __init__(self, speed: float, name: str): - self.speed = min(max(0, speed), 1) - self.name = name - - def record(self): - pass - - def replay(self, robot: dict[str, hw.FR3], _): - config: hw.FR3Config = cast(hw.FR3Config, robot[self.name].get_parameters()) - config.speed_factor = self.speed - robot[self.name].set_parameters(config) - - def __str__(self) -> str: - return f"ChnageSpeedFactor;{self.name};{self.speed}" - - @classmethod - def from_str(cls, line: str) -> Pose: - l = line.replace("\n", "").split(";") - return cls(float(l[2]), l[1]) - - -class PoseList: - """Records and replays a list of poses on multiple robots.""" - - def __init__( - self, - name2ip: dict[str, str], - speed_factor: float = 0.2, - poses: list[Pose] | None = None, - urdf_path: str | None = None, - ): - self.ik = common.IK(urdf_path) if urdf_path else None - self.r: dict[str, hw.FR3] = {key: hw.FR3(ip, self.ik) for key, ip in name2ip.items()} - # TODO: this config should be given to the constructor - cfg = hw.FHConfig() - cfg.epsilon_inner = 0.1 - cfg.epsilon_outer = 0.1 - cfg.force = 20 - self.g: dict[str, hw.FrankaHand] = {key: hw.FrankaHand(ip, cfg) for key, ip in name2ip.items()} - self.r_ip: dict[str, hw.FR3] = {ip: self.r[key] for key, ip in name2ip.items()} - self.g_ip: dict[str, hw.FrankaHand] = {ip: self.g[key] for key, ip in name2ip.items()} - self.ip2name: dict[str, str] = {ip: name for name, ip in name2ip.items()} - self.name2ip: dict[str, str] = name2ip - self._button_recording = False - - self.poses: list[Pose] = [ChnageSpeedFactor(speed_factor, key) for key in self.r] if poses is None else poses - - self.m: dict[str, tuple[str, np.ndarray]] = {} - - @classmethod - def load(cls, name2ip: dict[str, str], filenames: list[str], urdf_path: str | None = None): - poses = [] - for filename in filenames: - - def get_class(line: str) -> type[Pose]: - pose_dict: dict[str, type[Pose]] = { - "JointPose": JointPose, - "GripperPose": GripperPose, - "WaitForInput": WaitForInput, - "Sleep": Sleep, - "ChnageSpeedFactor": ChnageSpeedFactor, - "WaitForDoubleTab": WaitForDoubleTab, - } - first = line.split(";")[0].replace("\n", "") - return pose_dict[first] - - with open(filename, "r") as f: - poses += [get_class(line).from_str(line) for line in f.readlines()] - - return cls(poses=poses, name2ip=name2ip, urdf_path=urdf_path) - - def save(self, filename): - with open(filename, "w") as f: - f.write("\n".join([str(pose) for pose in self.poses])) - - def start_button_recording(self): - self._button_recording = True - - def button_callback(self, r_ip: str, buttons: list[str]): - if "check" in buttons: - if not check_pose(self.r_ip[r_ip].get_joint_position()): - _logger.warning("REJECTED due to joint constraints") - return - j = JointPose(name=self.ip2name[r_ip]) - j.record(self.r) - self.poses.append(j) - elif "up" in buttons: - g = GripperPose(name=self.ip2name[r_ip]) - g.record(True, self.g) - self.poses.append(g) - elif "down" in buttons: - g = GripperPose(name=self.ip2name[r_ip]) - g.record(False, self.g) - self.poses.append(g) - elif "cross" in buttons: - self._button_recording = False - - def record(self): - while True: - i = input( - "Press p to record a pose, press s to shut the gripper, press r to release the gripper, press w to have a wait for input pose\n" - ) - if i.split(" ")[0] == "p": - if not check_pose(self.r[i.split(" ")[1]].get_joint_position()): - _logger.warning("REJECTED due to joint constraints") - continue - j = JointPose(name=i.split(" ")[1]) - j.record(self.r) - self.poses.append(j) - elif i.split(" ")[0] == "s": - g = GripperPose(name=i.split(" ")[1]) - g.record(True, self.g) - self.poses.append(g) - elif i.split(" ")[0] == "r": - g = GripperPose(name=i.split(" ")[1]) - g.record(False, self.g) - self.poses.append(g) - elif i == "w": - w = WaitForInput() - self.poses.append(w) - elif i == "wd": - wd = WaitForDoubleTab(name=i.split(" ")[1]) - self.poses.append(wd) - elif i.split(" ")[0] == "sl": - sl = Sleep(float(i.split(" ")[1])) - self.poses.append(sl) - elif i.split(" ")[0] == "sp": - sp = ChnageSpeedFactor(name=i.split(" ")[1], speed=float(i.split(" ")[2])) - self.poses.append(sp) - elif i == "q": - break - elif i.split(" ")[0] == "re": - # re robotname savename - self.m[i.split(" ")[2]] = ( - i.split(" ")[1], - self.r[i.split(" ")[1]].get_joint_position(), - ) - elif i in self.m: - j = JointPose(name=self.m[i][0], pose=self.m[i][1]) - self.poses.append(j) - else: - _logger.warning("Invalid input") - - def replay(self): - for pose in self.poses: - pose.replay(self.r, self.g) diff --git a/python/rcsss/control/utils.py b/python/rcsss/control/utils.py deleted file mode 100644 index d025cd86..00000000 --- a/python/rcsss/control/utils.py +++ /dev/null @@ -1,10 +0,0 @@ -import os - -from dotenv import load_dotenv - - -def load_creds_fr3_desk() -> tuple[str, str]: - load_dotenv() - assert "DESK_USERNAME" in os.environ, "DESK_USERNAME not set in .env file or environment var." - assert "DESK_PASSWORD" in os.environ, "DESK_PASSWORD not set in .env file or environment var." - return os.environ["DESK_USERNAME"], os.environ["DESK_PASSWORD"] diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py deleted file mode 100644 index 90a63b41..00000000 --- a/python/rcsss/envs/factories.py +++ /dev/null @@ -1,347 +0,0 @@ -import logging -from os import PathLike -from typing import Type - -import gymnasium as gym -import numpy as np -import rcsss -from gymnasium.envs.registration import EnvCreator -from rcsss import sim -from rcsss._core.sim import CameraType -from rcsss.camera.hw import BaseHardwareCameraSet -from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig -from rcsss.envs.base import ( - CameraSetWrapper, - ControlMode, - FR3Env, - GripperWrapper, - RelativeActionSpace, - RelativeTo, -) -from rcsss.envs.hw import FR3HW -from rcsss.envs.sim import ( - CollisionGuard, - FR3Sim, - PickCubeSuccessWrapper, - RandomCubePos, - SimWrapper, -) -from rcsss.envs.space_utils import Vec7Type -from rcsss.envs.utils import ( - default_fr3_hw_gripper_cfg, - default_fr3_hw_robot_cfg, - default_fr3_sim_gripper_cfg, - default_fr3_sim_robot_cfg, - default_realsense, - get_urdf_path, -) - -logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) - - -def fr3_hw_env( - ip: str, - control_mode: ControlMode, - robot_cfg: rcsss.hw.FR3Config, - collision_guard: str | PathLike | None = None, - gripper_cfg: rcsss.hw.FHConfig | None = None, - camera_set: BaseHardwareCameraSet | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, -) -> gym.Env: - """ - Creates a hardware environment for the FR3 robot. - - Args: - ip (str): IP address of the robot. - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcsss.hw.FR3Config): Configuration for the FR3 robot. - collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present) - or the path to a mujoco scene for collision guarding. If None, collision guarding is not used. - gripper_cfg (rcsss.hw.FHConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set (BaseHardwareCameraSet | None): Camera set to be used. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. If no URDF file is found, the environment will - still work but set_cartesian methods might throw an error. A URDF file is needed for collision guarding. - - Returns: - gym.Env: The configured hardware environment for the FR3 robot. - """ - urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=collision_guard is not None) - ik = rcsss.common.IK(str(urdf_path)) if urdf_path is not None else None - robot = rcsss.hw.FR3(ip, ik) - robot.set_parameters(robot_cfg) - - env: gym.Env = FR3Env(robot, ControlMode.JOINTS if collision_guard is not None else control_mode) - - env = FR3HW(env) - if gripper_cfg is not None: - gripper = rcsss.hw.FrankaHand(ip, gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) - - if camera_set is not None: - camera_set.start() - camera_set.wait_for_frames() - logger.info("CameraSet started") - env = CameraSetWrapper(env, camera_set) - - if collision_guard is not None: - assert urdf_path is not None - env = CollisionGuard.env_from_xml_paths( - env, - str(rcsss.scenes.get(str(collision_guard), collision_guard)), - urdf_path, - gripper=True, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=False, - ) - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env - - -def fr3_sim_env( - control_mode: ControlMode, - robot_cfg: rcsss.sim.FR3Config, - collision_guard: bool = False, - gripper_cfg: rcsss.sim.FHConfig | None = None, - camera_set_cfg: SimCameraSetConfig | None = None, - max_relative_movement: float | tuple[float, float] | None = None, - relative_to: RelativeTo = RelativeTo.LAST_STEP, - urdf_path: str | PathLike | None = None, - mjcf: str | PathLike = "fr3_empty_world", - sim_wrapper: Type[SimWrapper] | None = None, -) -> gym.Env: - """ - Creates a simulation environment for the FR3 robot. - - Args: - control_mode (ControlMode): Control mode for the robot. - robot_cfg (rcsss.sim.FR3Config): Configuration for the FR3 robot. - collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding. - gripper_cfg (rcsss.sim.FHConfig | None): Configuration for the gripper. If None, no gripper is used. - camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used. - max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts - translational movement in meters. If tuple, it restricts both translational (in meters) and rotational - (in radians) movements. If None, no restriction is applied. - relative_to (RelativeTo): Specifies whether the movement is relative to a configured origin or the last step. - urdf_path (str | PathLike | None): Path to the URDF file. If None, the URDF file is automatically deduced - which requires a UTN compatible lab scene to be present. - mjcf (str | PathLike): Path to the Mujoco scene XML file. Defaults to "fr3_empty_world". - sim_wrapper (Type[SimWrapper] | None): Wrapper to be applied before the simulation wrapper. This is useful - for reset management e.g. resetting objects in the scene with correct observations. Defaults to None. - - Returns: - gym.Env: The configured simulation environment for the FR3 robot. - """ - urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=False) - assert urdf_path is not None - if mjcf not in rcsss.scenes: - logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") - mjb_file = rcsss.scenes.get(str(mjcf), mjcf) - simulation = sim.Sim(mjb_file) - - ik = rcsss.common.IK(urdf_path) - robot = rcsss.sim.FR3(simulation, "0", ik) - robot.set_parameters(robot_cfg) - env: gym.Env = FR3Env(robot, control_mode) - env = FR3Sim(env, simulation, sim_wrapper) - - if camera_set_cfg is not None: - camera_set = SimCameraSet(simulation, camera_set_cfg) - env = CameraSetWrapper(env, camera_set, include_depth=True) - - if gripper_cfg is not None: - gripper = sim.FrankaHand(simulation, "0", gripper_cfg) - env = GripperWrapper(env, gripper, binary=True) - - if collision_guard: - env = CollisionGuard.env_from_xml_paths( - env, - str(rcsss.scenes.get(str(mjcf), mjcf)), - urdf_path, - gripper=gripper_cfg is not None, - check_home_collision=False, - control_mode=control_mode, - tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()), - sim_gui=True, - truncate_on_collision=True, - ) - if max_relative_movement is not None: - env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to) - - return env - - -class FR3Real(EnvCreator): - def __call__( # type: ignore - self, - robot_ip: str, - control_mode: str = "xyzrpy", - delta_actions: bool = True, - camera_config: dict[str, str] | None = None, - gripper: bool = True, - ) -> gym.Env: - camera_set = default_realsense(camera_config) - return fr3_hw_env( - ip=robot_ip, - camera_set=camera_set, - control_mode=( - ControlMode.CARTESIAN_TRPY - if control_mode == "xyzrpy" - else ControlMode.JOINTS if control_mode == "joints" else ControlMode.CARTESIAN_TQuart - ), - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard=None, - gripper_cfg=default_fr3_hw_gripper_cfg() if gripper else None, - max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, - relative_to=RelativeTo.LAST_STEP, - ) - - -def make_sim_task_envs( - mjcf: str, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - delta_actions: bool = True, - camera_cfg: SimCameraSetConfig | None = None, -) -> gym.Env: - - env_rel = fr3_sim_env( - control_mode=control_mode, - robot_cfg=default_fr3_sim_robot_cfg(mjcf), - collision_guard=False, - gripper_cfg=default_fr3_sim_gripper_cfg(), - camera_set_cfg=camera_cfg, - max_relative_movement=(0.2, np.deg2rad(45)) if delta_actions else None, - relative_to=RelativeTo.LAST_STEP, - mjcf=mjcf, - sim_wrapper=RandomCubePos, - ) - env_rel = PickCubeSuccessWrapper(env_rel) - if render_mode == "human": - env_rel.get_wrapper_attr("sim").open_gui() - - return env_rel - - -class FR3SimplePickUpSim(EnvCreator): - def __call__( # type: ignore - self, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - resolution: tuple[int, int] | None = None, - frame_rate: int = 0, - delta_actions: bool = True, - ) -> gym.Env: - if resolution is None: - resolution = (256, 256) - - cameras = { - "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), - "bird_eye": SimCameraConfig(identifier="bird_eye_cam", type=int(CameraType.fixed)), - "side": SimCameraConfig(identifier="side_view", type=int(CameraType.fixed)), - "right_side": SimCameraConfig(identifier="right_side", type=int(CameraType.fixed)), - "left_side": SimCameraConfig(identifier="left_side", type=int(CameraType.fixed)), - "front": SimCameraConfig(identifier="front", type=int(CameraType.fixed)), - } - - camera_cfg = SimCameraSetConfig( - cameras=cameras, - resolution_width=resolution[0], - resolution_height=resolution[1], - frame_rate=frame_rate, - physical_units=True, - ) - return make_sim_task_envs("simple_pick_up", render_mode, control_mode, delta_actions, camera_cfg) - - -class FR3SimplePickUpSimDigitHand(EnvCreator): - def __call__( # type: ignore - self, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - resolution: tuple[int, int] | None = None, - frame_rate: int = 0, - delta_actions: bool = True, - ) -> gym.Env: - if resolution is None: - resolution = (256, 256) - - cameras = {"wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed))} - - camera_cfg = SimCameraSetConfig( - cameras=cameras, - resolution_width=resolution[0], - resolution_height=resolution[1], - frame_rate=frame_rate, - physical_units=True, - ) - return make_sim_task_envs("fr3_simple_pick_up_digit_hand", render_mode, control_mode, delta_actions, camera_cfg) - - -class CamRobot(gym.Wrapper): - - def __init__(self, env, cam_robot_joints: Vec7Type): - super().__init__(env) - self.unwrapped: FR3Env - assert isinstance(self.unwrapped.robot, sim.FR3), "Robot must be a sim.FR3 instance." - self.sim = env.get_wrapper_attr("sim") - self.cam_robot = rcsss.sim.FR3(self.sim, "1", env.unwrapped.robot.get_ik(), register_convergence_callback=False) - self.cam_robot.set_parameters(default_fr3_sim_robot_cfg("lab_simple_pick_up_digit_hand")) - self.cam_robot_joints = cam_robot_joints - - def step(self, action: dict): - self.cam_robot.set_joints_hard(self.cam_robot_joints) - obs, reward, done, truncated, info = super().step(action) - return obs, reward, done, truncated, info - - def reset(self, *, seed=None, options=None): - re = super().reset(seed=seed, options=options) - self.cam_robot.reset() - return re - - -class FR3LabPickUpSimDigitHand(EnvCreator): - def __call__( # type: ignore - self, - cam_robot_joints: Vec7Type, - render_mode: str = "human", - control_mode: ControlMode = ControlMode.CARTESIAN_TRPY, - resolution: tuple[int, int] | None = None, - frame_rate: int = 0, - delta_actions: bool = True, - ) -> gym.Env: - if resolution is None: - resolution = (256, 256) - - cameras = { - "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), - "side": SimCameraConfig(identifier="wrist_1", type=int(CameraType.fixed)), - } - - camera_cfg = SimCameraSetConfig( - cameras=cameras, - resolution_width=resolution[0], - resolution_height=resolution[1], - frame_rate=frame_rate, - physical_units=True, - ) - env_rel = make_sim_task_envs( - "lab_simple_pick_up_digit_hand", - render_mode, - control_mode, - delta_actions, - camera_cfg, - ) - return CamRobot(env_rel, cam_robot_joints) diff --git a/python/tests/test_common.py b/python/tests/test_common.py index 087a0bed..03e3fdd0 100644 --- a/python/tests/test_common.py +++ b/python/tests/test_common.py @@ -2,7 +2,7 @@ import numpy as np import pytest -from rcsss import common +from rcs import common class TestPose: diff --git a/python/tests/test_envs.py b/python/tests/test_envs.py index bacc9200..096c2e47 100644 --- a/python/tests/test_envs.py +++ b/python/tests/test_envs.py @@ -2,7 +2,7 @@ import gymnasium as gym import numpy as np -from rcsss.envs.space_utils import RCSpaceType, get_space, get_space_keys +from rcs.envs.space_utils import RCSpaceType, get_space, get_space_keys class SimpleSpace(RCSpaceType): diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 9806d9ec..3acbac81 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -2,17 +2,17 @@ import numpy as np import pytest -import rcsss -from rcsss.envs.base import ( +import rcs +from rcs.envs.base import ( ControlMode, - FR3Env, GripperDictType, JointsDictType, - TQuartDictType, + RobotEnv, + TQuatDictType, TRPYDictType, ) -from rcsss.envs.factories import fr3_sim_env -from rcsss.envs.utils import ( +from rcs.envs.creators import RCSSimEnvCreator +from rcs.envs.utils import ( default_fr3_sim_gripper_cfg, default_fr3_sim_robot_cfg, default_mujoco_cameraset_cfg, @@ -40,11 +40,11 @@ class TestSimEnvs: def assert_no_pose_change(self, info: dict, initial_obs: dict, final_obs: dict): assert info["ik_success"] assert info["is_sim_converged"] - out_pose = rcsss.common.Pose( - translation=np.array(final_obs["tquart"][:3]), quaternion=np.array(final_obs["tquart"][3:]) + out_pose = rcs.common.Pose( + translation=np.array(final_obs["tquat"][:3]), quaternion=np.array(final_obs["tquat"][3:]) ) - expected_pose = rcsss.common.Pose( - translation=np.array(initial_obs["tquart"][:3]), quaternion=np.array(initial_obs["tquart"][3:]) + expected_pose = rcs.common.Pose( + translation=np.array(initial_obs["tquat"][:3]), quaternion=np.array(initial_obs["tquat"][3:]) ) assert out_pose.is_close(expected_pose, 1e-2, 1e-3) @@ -64,7 +64,7 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None ) # Test double reset. Regression test. A lot can go wrong when resetting. @@ -75,7 +75,7 @@ def test_zero_action_trpy(self, cfg): """ Test that a zero action does not change the state significantly """ - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None ) obs_initial, _ = env.reset() @@ -88,29 +88,29 @@ def test_non_zero_action_trpy(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None ) obs_initial, _ = env.reset() # action to be performed x_pos_change = 0.2 - initial_tquart = obs_initial["tquart"].copy() - t = initial_tquart[:3] + initial_tquat = obs_initial["tquat"].copy() + t = initial_tquat[:3] # shift the translation in x t[0] += x_pos_change - q = initial_tquart[3:] - initial_pose = rcsss.common.Pose(translation=np.array(t), quaternion=np.array(q)) + q = initial_tquat[3:] + initial_pose = rcs.common.Pose(translation=np.array(t), quaternion=np.array(q)) xyzrpy = np.concatenate([t, initial_pose.rotation_rpy().as_vector()], axis=0) non_zero_action = TRPYDictType(xyzrpy=np.array(xyzrpy)) non_zero_action.update(GripperDictType(gripper=0)) expected_obs = obs_initial.copy() - expected_obs["tquart"][0] += x_pos_change + expected_obs["tquat"][0] += x_pos_change obs, _, _, _, info = env.step(non_zero_action) self.assert_no_pose_change(info, expected_obs, obs) def test_relative_zero_action_trpy(self, cfg, gripper_cfg): # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() @@ -122,7 +122,7 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg): def test_relative_non_zero_action(self, cfg, gripper_cfg): # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() @@ -131,7 +131,7 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg): non_zero_action = TRPYDictType(xyzrpy=np.array([x_pos_change, 0, 0, 0, 0, 0])) non_zero_action.update(GripperDictType(gripper=0)) expected_obs = obs_initial.copy() - expected_obs["tquart"][0] += x_pos_change + expected_obs["tquat"][0] += x_pos_change obs, _, _, _, info = env.step(non_zero_action) self.assert_no_pose_change(info, obs_initial, expected_obs) @@ -140,7 +140,7 @@ def test_collision_trpy(self, cfg, gripper_cfg): Check that an obvious collision is detected by sim """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None ) obs, _ = env.reset() @@ -157,7 +157,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, @@ -166,22 +166,22 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg): max_relative_movement=None, ) obs, _ = env.reset() - unwrapped = cast(FR3Env, env.unwrapped) - p1 = unwrapped.robot.get_joint_position() + unwrapped = cast(RobotEnv, env.unwrapped) + p1: np.ndarray = unwrapped.robot.get_joint_position() # an obvious below ground collision action obs["xyzrpy"][0] = 0.4 obs["xyzrpy"][2] = -0.05 collision_action = TRPYDictType(xyzrpy=obs["xyzrpy"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) - p2 = unwrapped.robot.get_joint_position() + p2: np.ndarray = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move assert np.allclose(p1, p2) -class TestSimEnvsTquart(TestSimEnvs): - """This class is for testing Tquart sim env functionalities""" +class TestSimEnvsTquat(TestSimEnvs): + """This class is for testing Tquat sim env functionalities""" def test_reset(self, cfg, gripper_cfg, cam_cfg): """ @@ -190,8 +190,8 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, + env = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, @@ -201,63 +201,63 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): env.reset() env.reset() - def test_non_zero_action_tquart(self, cfg): + def test_non_zero_action_tquat(self, cfg): """ - Test that a zero action does not change the state significantly in the tquart configuration + Test that a zero action does not change the state significantly in the tquat configuration """ # env creation - env = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None + env = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None ) obs_initial, _ = env.reset() # action to be performed - t = obs_initial["tquart"][:3] - q = obs_initial["tquart"][3:] + t = obs_initial["tquat"][:3] + q = obs_initial["tquat"][3:] x_pos_change = 0.3 # updating the x action by 30cm t[0] += x_pos_change - non_zero_action = TQuartDictType(tquart=np.concatenate([t, q], axis=0)) + non_zero_action = TQuatDictType(tquat=np.concatenate([t, q], axis=0)) expected_obs = obs_initial.copy() - expected_obs["tquart"][0] += x_pos_change + expected_obs["tquat"][0] += x_pos_change _, _, _, _, info = env.step(non_zero_action) self.assert_no_pose_change(info, obs_initial, expected_obs) - def test_zero_action_tquart(self, cfg): + def test_zero_action_tquat(self, cfg): """ - Test that a zero action does not change the state significantly in the tquart configuration + Test that a zero action does not change the state significantly in the tquat configuration """ # env creation - env = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None + env = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None ) obs_initial, info_ = env.reset() - home_action_vec = obs_initial["tquart"] - zero_action = TQuartDictType(tquart=home_action_vec) + home_action_vec = obs_initial["tquat"] + zero_action = TQuatDictType(tquat=home_action_vec) obs, _, _, _, info = env.step(zero_action) self.assert_no_pose_change(info, obs_initial, obs) - def test_relative_zero_action_tquart(self, cfg, gripper_cfg): + def test_relative_zero_action_tquat(self, cfg, gripper_cfg): # env creation - env_rel = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, + env_rel = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5, ) obs_initial, _ = env_rel.reset() - zero_rel_action = TQuartDictType(tquart=np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)) + zero_rel_action = TQuatDictType(tquat=np.array([0, 0, 0, 0, 0, 0, 1.0], dtype=np.float32)) zero_rel_action.update(GripperDictType(gripper=0)) obs, _, _, _, info = env_rel.step(zero_rel_action) self.assert_no_pose_change(info, obs_initial, obs) - def test_collision_tquart(self, cfg, gripper_cfg): + def test_collision_tquat(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by sim """ # env creation - env = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, + env = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, @@ -265,20 +265,20 @@ def test_collision_tquart(self, cfg, gripper_cfg): ) obs, _ = env.reset() # an obvious below ground collision action - obs["tquart"][0] = 0.4 - obs["tquart"][2] = -0.05 - collision_action = TQuartDictType(tquart=obs["tquart"]) + obs["tquat"][0] = 0.4 + obs["tquat"][2] = -0.05 + collision_action = TQuatDictType(tquat=obs["tquat"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) self.assert_collision(info) - def test_collision_guard_tquart(self, cfg, gripper_cfg): + def test_collision_guard_tquat(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = fr3_sim_env( - ControlMode.CARTESIAN_TQuart, + env = RCSSimEnvCreator()( + ControlMode.CARTESIAN_TQuat, cfg, gripper_cfg=gripper_cfg, collision_guard=True, @@ -286,15 +286,15 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg): max_relative_movement=None, ) obs, _ = env.reset() - unwrapped = cast(FR3Env, env.unwrapped) - p1 = unwrapped.robot.get_joint_position() + unwrapped = cast(RobotEnv, env.unwrapped) + p1: np.ndarray = unwrapped.robot.get_joint_position() # an obvious below ground collision action - obs["tquart"][0] = 0.4 - obs["tquart"][2] = -0.05 - collision_action = TQuartDictType(tquart=obs["tquart"]) + obs["tquat"][0] = 0.4 + obs["tquat"][2] = -0.05 + collision_action = TQuatDictType(tquat=obs["tquat"]) collision_action.update(GripperDictType(gripper=0)) _, _, _, _, info = env.step(collision_action) - p2 = unwrapped.robot.get_joint_position() + p2: np.ndarray = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move assert np.allclose(p1, p2) @@ -310,7 +310,7 @@ def test_reset(self, cfg, gripper_cfg, cam_cfg): # TODO: # - test initial pose after reset. # - test initial gripper config. - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None ) # Test double reset. Regression test. A lot can go wrong when resetting. @@ -322,7 +322,9 @@ def test_zero_action_joints(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = fr3_sim_env(ControlMode.JOINTS, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None) + env = RCSSimEnvCreator()( + ControlMode.JOINTS, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None + ) obs_initial, _ = env.reset() # action to be performed zero_action = JointsDictType(joints=np.array(obs_initial["joints"])) @@ -336,7 +338,9 @@ def test_non_zero_action_joints(self, cfg): This is for testing that a certain action leads to the expected change in state """ # env creation - env = fr3_sim_env(ControlMode.JOINTS, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None) + env = RCSSimEnvCreator()( + ControlMode.JOINTS, cfg, gripper_cfg=None, camera_set_cfg=None, max_relative_movement=None + ) obs_initial, _ = env.reset() new_joint_vals = obs_initial["joints"] + np.array([0.1, 0.1, 0.1, 0.1, -0.1, -0.1, 0.1], dtype=np.float32) # action to be performed @@ -351,7 +355,7 @@ def test_collision_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None ) env.reset() @@ -366,7 +370,7 @@ def test_collision_guard_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by sim """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, @@ -375,13 +379,13 @@ def test_collision_guard_joints(self, cfg, gripper_cfg): max_relative_movement=None, ) env.reset() - unwrapped = cast(FR3Env, env.unwrapped) - p1 = unwrapped.robot.get_joint_position() + unwrapped = cast(RobotEnv, env.unwrapped) + p1: np.ndarray = unwrapped.robot.get_joint_position() # the below action is a test_case where there is an obvious collision regardless of the gripper action collision_act = JointsDictType(joints=np.array([0, 1.78, 0, -1.45, 0, 0, 0], dtype=np.float32)) collision_act.update(GripperDictType(gripper=1)) _, _, _, _, info = env.step(collision_act) - p2 = unwrapped.robot.get_joint_position() + p2: np.ndarray = unwrapped.robot.get_joint_position() self.assert_collision(info) # assure that the robot did not move @@ -392,7 +396,7 @@ def test_relative_zero_action_joints(self, cfg, gripper_cfg): Check that an obvious collision is detected by the CollisionGuard """ # env creation - env = fr3_sim_env( + env = RCSSimEnvCreator()( ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() diff --git a/recordings/give.tj b/recordings/give.tj deleted file mode 100644 index b1d4c184..00000000 --- a/recordings/give.tj +++ /dev/null @@ -1,29 +0,0 @@ -ChnageSpeedFactor;r1;0.3 -ChnageSpeedFactor;r2;0.3 -JointPose;r1;[ 0.005200501057300119 -0.7776761077989306 -0.0025886423488123023 -2.3562608952857937 -0.018184680995392243 1.5544144084722524 0.7862891082622613 ] -JointPose;r2;[-3.3349648587935594e-03 -7.8251447289911946e-01 -6.5597066844244765e-03 -2.3623086468423748e+00 -1.6299339076434930e-03 1.5491097611007218e+00 7.7942399714918364e-01] -JointPose;r2;[ 0.10150165811889729 -0.007628468278851079 0.5860024277410181 -2.2821333998347737 -0.8180989246724721 1.3104456785441492 0.14394481174469945 ] -JointPose;r2;[ 0.21987563274668973 0.8259220264912536 0.40456250172851527 -1.992463128160993 -1.12549397644007 1.2348221522577454 0.6133934311684555 ] -JointPose;r2;[ 0.16365420528557176 0.8060176155778143 0.3318965450860155 -2.120072932392316 -1.215781460891273 1.3078079093522825 0.6428394168209788 ] -GripperPose;r2;None -JointPose;r2;[ 0.03263723304483422 0.7861888436379253 0.26744756628609606 -2.200848132152096 -1.3784803484652357 1.39876967107626 0.6771586005512955 ] -GripperPose;r2;0.1 -JointPose;r2;[ 0.0459554782072226 -0.07262875167150243 0.33825663847881593 -2.506822490657148 -1.2438066942969874 1.358629042041596 0.12285699882288213] -JointPose;r1;[ 0.16335410831115993 -0.007963812769050907 -0.17341217024483851 -2.5849212268938224 1.4937671465712061 1.59120477991401 -1.7385837513997697 ] -JointPose;r2;[-0.5570413598117403 0.12178742366274425 0.042848769121033393 -2.4007001084680937 -2.0136919111729927 1.8706474778876605 0.24861112232918345 ] -JointPose;r1;[ 0.5622675300593329 0.05876481514117871 -0.12643448679904778 -2.4907625030742553 1.96848892790002 1.8630124334109617 -1.830390128989685 ] -GripperPose;r1;None -JointPose;r1;[ 0.6561714204678296 0.1494818804736669 -0.12100520172820554 -2.352088292945169 1.9712564224705111 1.9006234199648582 -1.7918210518999529 ] -GripperPose;r1;0.1 -GripperPose;r2;None -JointPose;r2;[-0.5316253680315972 0.027708863687361546 0.1529940433554669 -2.55312817959101 -1.949997951235751 1.7825631995343467 0.29406272461761845 ] -JointPose;r2;[-0.1382182831550196 0.10642875602552476 0.46537961832832736 -2.455863831816388 -1.330792995601732 1.358157548525049 0.22210592760870762] -JointPose;r1;[ 0.038768820676431365 0.23397922529737566 -0.6506523108734592 -2.368443527642107 1.1468843736186598 1.1745556012969522 -1.9171702488915496 ] -JointPose;r1;[ 7.1643601858826882e-04 9.2586559769729615e-01 -5.2263422457888553e-01 -2.0437971855373345e+00 1.2548578753611292e+00 1.1141209289648575e+00 -2.3022912777344926e+00] -JointPose;r1;[ 0.00498302592038962 0.955518082325489 -0.5219011989448232 -2.02366710520207 1.253856550234887 1.1183371061603722 -2.2922352310660705 ] -GripperPose;r1;None -JointPose;r1;[-0.09764578701723105 0.9875581115212488 -0.5326915049602314 -1.9357790333142697 1.1912705479500743 1.0917946171120798 -2.296580882035137 ] -JointPose;r1;[-0.038161051709861726 0.3604326344605185 -0.7108721329330484 -2.113825709971404 0.9935093881919328 0.9827881568383591 -1.8639555384616995 ] -JointPose;r1;[ 0.5455224455519369 -0.43645601515161375 -0.6690356541230595 -2.480391296209418 1.264091000978408 1.5558361587711358 -1.2956347078087969 ] -JointPose;r1;[ 0.005194100090766051 -0.7776726258509761 -0.0025861961620599785 -2.3562628026332253 -0.018182840847525323 1.5544137714931832 0.7862924845049879 ] -JointPose;r2;[-3.3312545170341239e-03 -7.8251518815490673e-01 -6.5608474911202207e-03 -2.3623068143056036e+00 -1.6310124272287722e-03 1.5491102167827500e+00 7.7942713506359962e-01] \ No newline at end of file diff --git a/recordings/give_back.tj b/recordings/give_back.tj deleted file mode 100644 index 18399c74..00000000 --- a/recordings/give_back.tj +++ /dev/null @@ -1,24 +0,0 @@ -ChnageSpeedFactor;r1;0.6 -ChnageSpeedFactor;r2;0.6 -JointPose;r1;[ 0.005540593164300627 -0.7771050034624761 -0.0031602658476253204 -2.3565521294677807 -0.01789714429148022 1.5544534024687515 0.7859652608172712 ] -JointPose;r2;[-3.6087506427247406e-03 -7.8169468667668507e-01 -6.2338726726721772e-03 -2.3627440358593392e+00 -2.2228341453567127e-03 1.5487404521934136e+00 7.7916095705303667e-01] -JointPose;r1;[-0.10393345485187808 0.43419911021120683 -0.7109929613874955 -2.1161706631439414 0.9558416826971061 0.9633400221264676 -1.8850299066918879 ] -JointPose;r1;[-0.08712937611159308 0.9878206768617879 -0.570535372333761 -1.9271359809814805 1.1926809386582486 1.0820428845281296 -2.263992780759227 ] -JointPose;r1;[ 0.05376711020998031 0.9490484430046773 -0.5621322099784819 -2.0658186816597746 1.3036304297184544 1.1310649487235767 -2.2716985954850006 ] -GripperPose;r1;0.1 -JointPose;r1;[ 0.09727681724374741 0.16884019401271175 -0.6851988796625501 -2.311847659583049 1.0523001398706657 1.1370413756188462 -1.7912600761898025 ] -JointPose;r2;[ 0.03290860855443112 -0.07608390067216875 0.16067721713818942 -2.5602201799333786 -1.3463742755318364 1.5016836762036312 0.14416376544497278] -JointPose;r2;[-0.3984384531206171 0.0623175147059609 -0.12990146026605934 -2.4585208584142384 -2.0312837580709475 1.9045642357051416 0.22544331294730893] -JointPose;r1;[ 0.8876128599961977 0.08694542587442537 -0.4882391195618441 -2.453794035592154 1.8912306228992535 1.7534361060822765 -1.74725960715775 ] -JointPose;r2;[-0.46529394385236417 0.163521751037785 -0.18688901099225602 -2.3088335982016477 -2.1054809637018916 2.0028161927649504 0.21677378718560034] -Sleep;0.5 -GripperPose;r2;0.1 -GripperPose;r1;None -JointPose;r2;[ 0.19284720785389073 0.039678614023772985 0.1915535484322972 -2.5466815290727003 -1.3026469596353931 1.3616999805872063 0.221387371471869 ] -JointPose;r2;[ 0.19942715534924726 0.7862432579498406 0.1281469947225464 -2.1843480528059773 -1.313714516330945 1.4048517694158262 0.7049111482516974 ] -JointPose;r2;[ 0.19761206675956183 0.8135395951245808 0.125581198195316 -2.1758302474567905 -1.3246378744711174 1.4273169907899983 0.6865784082861169 ] -GripperPose;r2;None -JointPose;r2;[ 0.28778802044874746 0.8142818724768215 0.25198536918093883 -2.069375439919398 -1.1360253359264494 1.3353329570683332 0.6847320509436876 ] -JointPose;r2;[ 0.2140778973941568 -0.3431599656847675 0.16111549735155062 -2.5448006763063193 -1.3161994837684685 1.3160165506167443 -0.11825729110241165] -JointPose;r2;[-3.6046893295047762e-03 -7.8169437912776552e-01 -6.2357370621113819e-03 -2.3627472088425705e+00 -2.2215181740813743e-03 1.5487390546110793e+00 7.7916314964041034e-01] -JointPose;r1;[ 0.005544300754546693 -0.777106013697742 -0.003164487460434249 -2.3565503467949216 -0.017896050696933744 1.5544557838476463 0.785965260811265 ] \ No newline at end of file diff --git a/recordings/give_mug.tj b/recordings/give_mug.tj deleted file mode 100644 index eec046da..00000000 --- a/recordings/give_mug.tj +++ /dev/null @@ -1,14 +0,0 @@ -ChnageSpeedFactor;r1;0.2 -ChnageSpeedFactor;r2;0.2 -JointPose;r1;[ 1.0309011091419077e-04 -7.8435040850134630e-01 -9.9655631106610148e-04 -2.3568975495993998e+00 -5.0107630375274488e-05 1.5707281229423282e+00 7.8478608535051186e-01] -JointPose;r1;[-0.13684781365499313 0.4434752655928923 -0.6593130951211976 -2.111907087634484 0.9722664592261988 0.9772754353849308 -1.9032830240248357 ] -JointPose;r1;[-0.13334037964986345 0.9619693464850401 -0.5399355338763744 -1.9320646844570093 1.1583882793419382 1.0901786166436007 -2.2376725451673183 ] -JointPose;r1;[ 0.03714352894213069 0.9219720522524342 -0.5225947053998735 -2.0775672386053015 1.2910358487852918 1.1683754701686722 -2.2638897164209237 ] -GripperPose;r1;0.07984278351068497 -JointPose;r1;[ 0.041596221870175136 0.5229283761435173 -0.5829480747183097 -2.2331173781187292 1.1312229326213066 1.1685998935256474 -2.1152822530397355 ] -JointPose;r1;[ 0.17166655740317122 0.6804395955025289 -0.5172121313676926 -1.739156896033339 1.9309460273105463 1.5408640896883057 -1.5152799692566818 ] -Sleep;2.0 -GripperPose;r1;None -Sleep;2.0 -JointPose;r1;[-0.14701133209370165 0.6808167560005145 -0.6548937819769609 -1.7782070800703322 1.7597523232760524 1.1963935767592802 -1.5617514170654625 ] -JointPose;r1;[ 1.0277274542020433e-04 -7.8434798177077669e-01 -9.9809349196734081e-04 -2.3568957440499099e+00 -5.1583516337425578e-05 1.5707273657673557e+00 7.8478282872554650e-01] \ No newline at end of file diff --git a/recordings/take_mug.tj b/recordings/take_mug.tj deleted file mode 100644 index 6cf9dc2d..00000000 --- a/recordings/take_mug.tj +++ /dev/null @@ -1,14 +0,0 @@ -ChnageSpeedFactor;r1;0.2 -ChnageSpeedFactor;r2;0.2 -JointPose;r1;[-1.1894064476127856e-04 -7.8386350655362147e-01 -1.5070508980715774e-03 -2.3564694160045914e+00 2.2313268428302149e-04 1.5703119606443736e+00 7.8453980654082278e-01] -JointPose;r1;[ 0.15716992692995813 0.9042091822334573 -0.014872630067077987 -1.0285346775252515 -2.283545109006679 2.6725028811967517 -0.021589851781646532] -WaitForInput -GripperPose;r1;0.07984278351068497 -Sleep;2.0 -JointPose;r1;[ 0.7302608711123029 0.27790992430044154 -0.003344315235151654 -2.0615815777099615 -1.57597595446945 1.7291218333914247 -0.02459951595836197 ] -JointPose;r1;[ 0.6218268770598657 0.23417094535800656 -0.08871812584310897 -2.443537206630685 -1.179781885148205 1.4488918465078398 0.31314772524880047] -JointPose;r1;[ 0.594087103431381 0.7951436190396701 -0.09057955707443527 -2.188767530230966 -1.1696096580872013 1.5886539672446114 0.6102872459557469 ] -GripperPose;r1;None -JointPose;r1;[ 0.876464274586186 0.6432870702753212 -0.003061112925875215 -2.0250878349867176 -0.9305118511436237 1.3102256146085673 0.37487937970016333 ] -JointPose;r1;[ 0.7658629951678054 -0.027561649206557085 -0.2553314751273738 -2.039075418448625 -0.9597817147636362 1.0833040786129242 -0.39064862317468163 ] -JointPose;r1;[-1.2386141190446972e-04 -7.8387192979543086e-01 -1.5088306525177786e-03 -2.3564701047680732e+00 2.2330419497083626e-04 1.5703135169700302e+00 7.8454055317433125e-01] \ No newline at end of file diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index f7aaae75..762ec53e 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_library(common) -target_sources(common PRIVATE Pose.cpp NRobotsWithGripper.cpp Robot.cpp IK.cpp) +target_sources(common PRIVATE Pose.cpp Robot.cpp IK.cpp) target_link_libraries(common PUBLIC Eigen3::Eigen mdl) diff --git a/src/common/IK.cpp b/src/common/IK.cpp index 0dd30d3d..04b120df 100644 --- a/src/common/IK.cpp +++ b/src/common/IK.cpp @@ -27,7 +27,7 @@ IK::IK(const std::string& urdf_path, size_t max_duration_ms) : rl() { this->rl.ik->setDuration(std::chrono::milliseconds(max_duration_ms)); } -std::optional IK::ik(const Pose& pose, const Vector7d& q0, +std::optional IK::ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset) { // pose is assumed to be in the robots coordinate frame this->rl.kin->setPosition(q0); diff --git a/src/common/IK.h b/src/common/IK.h index 740f709e..3356f33e 100644 --- a/src/common/IK.h +++ b/src/common/IK.h @@ -27,7 +27,7 @@ class IK { public: IK(const std::string& urdf_path, size_t max_duration_ms = 300); - std::optional ik(const Pose& pose, const Vector7d& q0, + std::optional ik(const Pose& pose, const VectorXd& q0, const Pose& tcp_offset = Pose::Identity()); // TODO: set max time diff --git a/src/common/LinearPoseTrajInterpolator.h b/src/common/LinearPoseTrajInterpolator.h index 65a0b848..cecaa754 100644 --- a/src/common/LinearPoseTrajInterpolator.h +++ b/src/common/LinearPoseTrajInterpolator.h @@ -1,5 +1,5 @@ -#ifndef DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ -#define DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ +#ifndef RCS_LINEAR_POSE_TRAJ_INTERPOLATOR_H +#define RCS_LINEAR_POSE_TRAJ_INTERPOLATOR_H #include namespace rcs { @@ -35,7 +35,7 @@ class LinearPoseTrajInterpolator { inline ~LinearPoseTrajInterpolator(){}; - inline void Reset(const double &time_sec, const Eigen::Vector3d &p_start, + inline void reset(const double &time_sec, const Eigen::Vector3d &p_start, const Eigen::Quaterniond &q_start, const Eigen::Vector3d &p_goal, const Eigen::Quaterniond &q_goal, const int &policy_rate, @@ -75,8 +75,8 @@ class LinearPoseTrajInterpolator { } }; - inline void GetNextStep(const double &time_sec, Eigen::Vector3d &p_t, - Eigen::Quaterniond &q_t) { + inline void next_step(const double &time_sec, Eigen::Vector3d &p_t, + Eigen::Quaterniond &q_t) { if (!start_) { start_time_ = time_sec; last_p_t_ = p_start_; @@ -127,7 +127,7 @@ class LinearJointPositionTrajInterpolator { inline ~LinearJointPositionTrajInterpolator(){}; - inline void Reset(const double &time_sec, + inline void reset(const double &time_sec, const Eigen::Matrix &q_start, const Eigen::Matrix &q_goal, const int &policy_rate, const int &rate, @@ -153,7 +153,7 @@ class LinearJointPositionTrajInterpolator { q_goal_ = q_goal; }; - inline void GetNextStep(const double &time_sec, Vector7d &q_t) { + inline void next_step(const double &time_sec, Vector7d &q_t) { if (!start_) { start_time_ = time_sec; last_q_t_ = q_start_; @@ -172,4 +172,4 @@ class LinearJointPositionTrajInterpolator { }; } // namespace common } // namespace rcs -#endif // DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ +#endif // RCS_LINEAR_POSE_TRAJ_INTERPOLATOR_H diff --git a/src/common/NRobotsWithGripper.cpp b/src/common/NRobotsWithGripper.cpp deleted file mode 100644 index 554bfa31..00000000 --- a/src/common/NRobotsWithGripper.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "NRobotsWithGripper.h" - -namespace rcs { -namespace common { - -NRobotsWithGripper::NRobotsWithGripper( - std::vector> robots_with_gripper) - : robots_with_gripper(robots_with_gripper) {} - -NRobotsWithGripper::~NRobotsWithGripper() {} - -// ROBOT FUNCTIONS -std::vector> NRobotsWithGripper::get_parameters_r( - const std::vector &idxs) { - std::function(size_t)> f = [this, &idxs](size_t i) { - return std::unique_ptr( - this->robots_with_gripper[idxs[i]]->robot->get_parameters()); - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -std::vector> NRobotsWithGripper::get_state_r( - const std::vector &idxs) { - std::function(size_t)> f = [this, &idxs](size_t i) { - return std::unique_ptr( - this->robots_with_gripper[idxs[i]]->robot->get_state()); - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -std::vector NRobotsWithGripper::get_cartesian_position( - const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - return this->robots_with_gripper[idxs[i]]->robot->get_cartesian_position(); - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::set_joint_position(const std::vector &idxs, - const std::vector &q) { - assert(idxs.size() == q.size()); - std::function f = [this, &idxs, &q](size_t i) { - this->robots_with_gripper[idxs[i]]->robot->set_joint_position(q[i]); - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -std::vector NRobotsWithGripper::get_joint_position( - const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - return this->robots_with_gripper[idxs[i]]->robot->get_joint_position(); - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::move_home(const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - return this->robots_with_gripper[idxs[i]]->robot->move_home(); - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::set_cartesian_position(const std::vector &idxs, - const std::vector &pose) { - assert(idxs.size() == pose.size()); - std::function f = [this, &idxs, &pose](size_t i) { - return this->robots_with_gripper[idxs[i]]->robot->set_cartesian_position( - pose[i]); - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -// GRIPPER FUNCTIONS -std::vector>> -NRobotsWithGripper::get_parameters_g(const std::vector &idxs) { - std::function>(size_t)> f = - [this, &idxs](size_t i) -> std::optional> { - if (this->robots_with_gripper[idxs[i]]->gripper.has_value()) { - return std::unique_ptr(this->robots_with_gripper[idxs[i]] - ->gripper.value() - ->get_parameters()); - } - return std::nullopt; - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -std::vector>> -NRobotsWithGripper::get_state_g(const std::vector &idxs) { - std::function>(size_t)> f = - [this, &idxs](size_t i) -> std::optional> { - if (this->robots_with_gripper[idxs[i]]->gripper.has_value()) { - return std::unique_ptr( - this->robots_with_gripper[idxs[i]]->gripper.value()->get_state()); - } - return std::nullopt; - }; - return NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::grasp(const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - if (this->robots_with_gripper[idxs[i]]->gripper.has_value()) { - this->robots_with_gripper[idxs[i]]->gripper.value()->grasp(); - } - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::open(const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - if (this->robots_with_gripper[idxs[i]]->gripper.has_value()) { - this->robots_with_gripper[idxs[i]]->gripper.value()->open(); - } - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -void NRobotsWithGripper::shut(const std::vector &idxs) { - std::function f = [this, &idxs](size_t i) { - if (this->robots_with_gripper[idxs[i]]->gripper.has_value()) { - this->robots_with_gripper[idxs[i]]->gripper.value()->shut(); - } - }; - NRobotsWithGripper::execute_parallel(f, idxs.size()); -} - -} // namespace common -} // namespace rcs \ No newline at end of file diff --git a/src/common/NRobotsWithGripper.h b/src/common/NRobotsWithGripper.h deleted file mode 100644 index 05134072..00000000 --- a/src/common/NRobotsWithGripper.h +++ /dev/null @@ -1,109 +0,0 @@ -#ifndef RCS_NROBOTSWITHGRIPPER_H -#define RCS_NROBOTSWITHGRIPPER_H - -#include -#include -#include -#include -#include - -#include "Pose.h" -#include "Robot.h" -#include "utils.h" - -namespace rcs { -namespace common { - -class NRobotsWithGripper { - public: - std::vector> robots_with_gripper; - - NRobotsWithGripper( - std::vector> robots_with_gripper); - // TODO: create constructor that takes ips and returns this object - - ~NRobotsWithGripper(); - - template - static std::vector execute_parallel(const std::function &func, - size_t n) { - std::vector results; - - if (n == 1) { - // no thread usage if there is only one robot - results.push_back(func(0)); - return results; - } - - // create new thread for each robot - // std::vector threads; - std::vector> threads; - for (size_t i = 0; i < n; i++) { - threads.push_back(std::async(std::launch::async, func, i)); - } - // collect results, no need to wait as get does this for us - for (size_t i = 0; i < n; i++) { - results.push_back(threads[i].get()); - } - return results; - } - - // special case for void functions - static void execute_parallel(const std::function &func, - size_t n) { - if (n == 1) { - // no thread usage if there is only one robot - func(0); - } - - // create new thread for each robot - std::vector threads; - for (size_t i = 0; i < n; i++) { - threads.push_back(std::thread(func, i)); - } - // wait for all threads to finish - for (size_t i = 0; i < n; i++) { - threads[i].join(); - } - } - - // ROBOT FUNCTIONS - std::vector> get_parameters_r( - const std::vector &idxs); - - std::vector> get_state_r( - const std::vector &idxs); - - std::vector get_cartesian_position(const std::vector &idxs); - - void set_joint_position(const std::vector &idxs, - const std::vector &q); - - std::vector get_joint_position(const std::vector &idxs); - - void move_home(const std::vector &idxs); - - void set_cartesian_position(const std::vector &idxs, - const std::vector &pose); - - // GRIPPER FUNCTIONS - std::vector>> get_parameters_g( - const std::vector &idxs); - - std::vector>> get_state_g( - const std::vector &idxs); - - void grasp(const std::vector &idxs); - - void open(const std::vector &idxs); - - void shut(const std::vector &idxs); - - // missing functions: set_normalized_width, is_grasped, reset (also from - // robot) -}; - -} // namespace common -} // namespace rcs - -#endif // RCS_NROBOTSWITHGRIPPER_H \ No newline at end of file diff --git a/src/common/Pose.cpp b/src/common/Pose.cpp index 8df77c04..9175670c 100644 --- a/src/common/Pose.cpp +++ b/src/common/Pose.cpp @@ -5,8 +5,8 @@ namespace common { Eigen::Vector3d IdentityTranslation() { return Eigen::Vector3d::Zero(); } Eigen::Matrix3d IdentityRotMatrix() { return Eigen::Matrix3d::Identity(); } -Eigen::Quaterniond IdentityRotQuart() { return Eigen::Quaterniond::Identity(); } -Eigen::Vector4d IdentityRotQuartVec() { return IdentityRotQuart().coeffs(); } +Eigen::Quaterniond IdentityRotQuat() { return Eigen::Quaterniond::Identity(); } +Eigen::Vector4d IdentityRotQuatVec() { return IdentityRotQuat().coeffs(); } Eigen::Matrix4d FrankaHandTCPOffset() { return (Eigen::Matrix4d() << 0.707, 0.707, 0, 0, -0.707, 0.707, 0, 0, 0, 0, 1, @@ -18,7 +18,7 @@ Eigen::Matrix4d FrankaHandTCPOffset() { Pose::Pose() { this->m_translation = IdentityTranslation(); - this->m_rotation = IdentityRotQuart(); + this->m_rotation = IdentityRotQuat(); } Pose::Pose(const Eigen::Affine3d &pose_matrix) { @@ -73,7 +73,7 @@ Pose::Pose(const Eigen::Vector3d &rotation, Pose::Pose(const Eigen::Vector3d &translation) { this->m_translation = translation; - this->m_rotation = IdentityRotQuart(); + this->m_rotation = IdentityRotQuat(); } Pose::Pose(const Eigen::Quaterniond &quaternion) { @@ -178,14 +178,14 @@ Pose Pose::operator*(const Pose &pose_b) const { } double Pose::total_angle() const { - return this->m_rotation.angularDistance(IdentityRotQuart()); + return this->m_rotation.angularDistance(IdentityRotQuat()); } Pose Pose::limit_rotation_angle(double max_angle) const { auto curr_angle = this->total_angle(); if (curr_angle > max_angle && max_angle >= 0) { auto new_rot = - IdentityRotQuart().slerp(max_angle / curr_angle, this->m_rotation); + IdentityRotQuat().slerp(max_angle / curr_angle, this->m_rotation); return Pose(new_rot, this->m_translation); } return *this; diff --git a/src/common/Pose.h b/src/common/Pose.h index a4978520..28fcb74e 100644 --- a/src/common/Pose.h +++ b/src/common/Pose.h @@ -12,8 +12,8 @@ namespace common { Eigen::Vector3d IdentityTranslation(); Eigen::Matrix3d IdentityRotMatrix(); -Eigen::Quaterniond IdentityRotQuart(); -Eigen::Vector4d IdentityRotQuartVec(); +Eigen::Quaterniond IdentityRotQuat(); +Eigen::Vector4d IdentityRotQuatVec(); Eigen::Matrix4d FrankaHandTCPOffset(); diff --git a/src/common/Robot.h b/src/common/Robot.h index 83fa84e7..fce3b6a5 100644 --- a/src/common/Robot.h +++ b/src/common/Robot.h @@ -12,18 +12,65 @@ namespace rcs { namespace common { -struct RConfig { - virtual ~RConfig(){}; +struct RobotMetaConfig { + VectorXd q_home; + int dof; + Eigen::Matrix joint_limits; }; -struct RState { - virtual ~RState(){}; + +enum RobotType { FR3 = 0, UR5e }; +enum RobotPlatform { SIMULATION = 0, HARDWARE }; + +static const std::unordered_map robots_meta_config = + {{// -------------- FR3 -------------- + {FR3, + RobotMetaConfig{ + // q_home: + (Vector7d() << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, M_PI_4) + .finished(), + // dof: + 7, + // joint_limits: + (Eigen::Matrix(2, 7) << + // low 7‐tuple + -2.3093, + -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895, + // high 7‐tuple + 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895) + .finished()}}, + + // -------------- UR5e -------------- + {UR5e, + RobotMetaConfig{ + // q_home (6‐vector): + (Vector6d() << -0.4488354, -2.02711196, 1.64630026, -1.18999615, + -1.57079762, -2.01963249) + .finished(), + // dof: + 6, + // joint_limits (2×6): + (Eigen::Matrix(2, 6) << + // low 6‐tuple + -2 * M_PI, + -2 * M_PI, -1 * M_PI, -2 * M_PI, -2 * M_PI, -2 * M_PI, + // high 6‐tuple + 2 * M_PI, 2 * M_PI, 1 * M_PI, 2 * M_PI, 2 * M_PI, 2 * M_PI) + .finished()}}}}; + +struct RobotConfig { + RobotType robot_type = RobotType::FR3; + RobotPlatform robot_platform = RobotPlatform::SIMULATION; + virtual ~RobotConfig(){}; +}; +struct RobotState { + virtual ~RobotState(){}; }; -struct GConfig { - virtual ~GConfig(){}; +struct GripperConfig { + virtual ~GripperConfig(){}; }; -struct GState { - virtual ~GState(){}; +struct GripperState { + virtual ~GripperState(){}; }; class Robot { @@ -36,15 +83,15 @@ class Robot { // like // bool set_parameters(const RConfig& cfg); - virtual RConfig* get_parameters() = 0; + virtual RobotConfig* get_parameters() = 0; - virtual RState* get_state() = 0; + virtual RobotState* get_state() = 0; virtual Pose get_cartesian_position() = 0; - virtual void set_joint_position(const Vector7d& q) = 0; + virtual void set_joint_position(const VectorXd& q) = 0; - virtual Vector7d get_joint_position() = 0; + virtual VectorXd get_joint_position() = 0; virtual void move_home() = 0; @@ -71,8 +118,8 @@ class Gripper { // a deduced config type // bool set_parameters(const GConfig& cfg); - virtual GConfig* get_parameters() = 0; - virtual GState* get_state() = 0; + virtual GripperConfig* get_parameters() = 0; + virtual GripperState* get_state() = 0; // set width of the gripper, 0 is closed, 1 is open virtual void set_normalized_width(double width, double force = 0) = 0; @@ -93,18 +140,6 @@ class Gripper { virtual void reset() = 0; }; -class RobotWithGripper { - public: - std::shared_ptr robot; - std::optional> gripper; - - RobotWithGripper(std::shared_ptr robot, - std::optional> gripper) - : robot(robot), gripper(gripper) {} - - ~RobotWithGripper() {} -}; - } // namespace common } // namespace rcs #endif // RCS_ROBOT_H diff --git a/src/common/utils.h b/src/common/utils.h index bff9c763..bb175e2b 100644 --- a/src/common/utils.h +++ b/src/common/utils.h @@ -9,6 +9,7 @@ namespace common { typedef Eigen::Matrix Vector6d; typedef Eigen::Matrix Vector7d; +typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix Vector7i; /*** diff --git a/src/hw/CMakeLists.txt b/src/hw/CMakeLists.txt index a06e75ae..a8669235 100644 --- a/src/hw/CMakeLists.txt +++ b/src/hw/CMakeLists.txt @@ -1,5 +1,5 @@ add_library(hw) -target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp MotionGenerator.cpp) +target_sources(hw PRIVATE FR3.cpp FrankaHand.cpp FR3MotionGenerator.cpp) target_link_libraries( hw PUBLIC franka common diff --git a/src/hw/FR3.cpp b/src/hw/FR3.cpp index de061021..e836ff9a 100644 --- a/src/hw/FR3.cpp +++ b/src/hw/FR3.cpp @@ -14,7 +14,7 @@ #include #include -#include "MotionGenerator.h" +#include "FR3MotionGenerator.h" #include "common/Pose.h" namespace rcs { @@ -98,17 +98,17 @@ common::Pose FR3::get_cartesian_position() { return x; } -void FR3::set_joint_position(const common::Vector7d &q) { +void FR3::set_joint_position(const common::VectorXd &q) { if (this->cfg.async_control) { this->controller_set_joint_position(q); return; } // sync control - MotionGenerator motion_generator(this->cfg.speed_factor, q); + FR3MotionGenerator motion_generator(this->cfg.speed_factor, q); this->robot.control(motion_generator); } -common::Vector7d FR3::get_joint_position() { +common::VectorXd FR3::get_joint_position() { common::Vector7d joints; if (this->running_controller == Controller::none) { this->curr_state = this->robot.readOnce(); @@ -177,7 +177,7 @@ void FR3::controller_set_joint_position(const common::Vector7d &desired_q) { this->interpolator_mutex.lock(); } - this->joint_interpolator.Reset( + this->joint_interpolator.reset( this->controller_time, Eigen::Map(this->curr_state.q.data()), desired_q, policy_rate, traj_rate, traj_interpolation_time_fraction); @@ -212,7 +212,7 @@ void FR3::osc_set_cartesian_position( } common::Pose curr_pose(this->curr_state.O_T_EE); - this->traj_interpolator.Reset( + this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), desired_pose_EE_in_base_frame.quaternion(), policy_rate, traj_rate, @@ -310,17 +310,11 @@ void FR3::osc() { int traj_rate = 500; this->interpolator_mutex.lock(); - // if (this->controller_time == 0) { - // this->traj_interpolator.Reset( - // 0., pose.translation(), pose.quaternion(), - // desired_pos_EE_in_base_frame, fixed_desired_quat_EE_in_base_frame, - // policy_rate, traj_rate, traj_interpolation_time_fraction); - // } this->curr_state = robot_state; this->controller_time += period.toSec(); - this->traj_interpolator.GetNextStep(this->controller_time, - desired_pos_EE_in_base_frame, - desired_quat_EE_in_base_frame); + this->traj_interpolator.next_step(this->controller_time, + desired_pos_EE_in_base_frame, + desired_quat_EE_in_base_frame); this->interpolator_mutex.unlock(); // end torques handler @@ -501,7 +495,7 @@ void FR3::joint_controller() { this->interpolator_mutex.lock(); this->curr_state = robot_state; this->controller_time += period.toSec(); - this->joint_interpolator.GetNextStep(this->controller_time, desired_q); + this->joint_interpolator.next_step(this->controller_time, desired_q); this->interpolator_mutex.unlock(); // end torques handler @@ -585,7 +579,9 @@ void FR3::zero_torque_controller() { void FR3::move_home() { // sync - MotionGenerator motion_generator(this->cfg.speed_factor, q_home); + FR3MotionGenerator motion_generator( + this->cfg.speed_factor, + common::robots_meta_config.at(common::RobotType::FR3).q_home); this->robot.control(motion_generator); } @@ -673,13 +669,13 @@ void FR3::set_cartesian_position(const common::Pose &x) { // nominal end effector frame should be on top of tcp offset as franka already // takes care of the default franka hand offset lets add a franka hand offset - if (this->cfg.ik_solver == IKSolver::franka) { + if (this->cfg.ik_solver == IKSolver::franka_ik) { // if gripper is attached the tcp offset will automatically be applied // by libfranka this->robot.setEE(nominal_end_effector_frame_value.affine_array()); this->set_cartesian_position_internal(x, 1.0, std::nullopt, std::nullopt); - } else if (this->cfg.ik_solver == IKSolver::rcs) { + } else if (this->cfg.ik_solver == IKSolver::rcs_ik) { this->set_cartesian_position_ik(x); } } diff --git a/src/hw/FR3.h b/src/hw/FR3.h index dd8991cb..249235f4 100644 --- a/src/hw/FR3.h +++ b/src/hw/FR3.h @@ -18,9 +18,6 @@ namespace rcs { namespace hw { -const common::Vector7d q_home((common::Vector7d() << 0, -M_PI_4, 0, -3 * M_PI_4, - 0, M_PI_2, M_PI_4) - .finished()); const double DEFAULT_SPEED_FACTOR = 0.2; struct FR3Load { @@ -28,15 +25,15 @@ struct FR3Load { std::optional f_x_cload; std::optional load_inertia; }; -enum IKSolver { franka = 0, rcs }; +enum IKSolver { franka_ik = 0, rcs_ik }; // modes: joint-space control, operational-space control, zero-torque // control enum Controller { none = 0, jsc, osc, ztc }; -struct FR3Config : common::RConfig { +struct FR3Config : common::RobotConfig { // TODO: max force and elbow? // TODO: we can either write specific bindings for each, or we use python // dictionaries with these objects - IKSolver ik_solver = IKSolver::franka; + IKSolver ik_solver = IKSolver::franka_ik; double speed_factor = DEFAULT_SPEED_FACTOR; std::optional load_parameters = std::nullopt; std::optional nominal_end_effector_frame = std::nullopt; @@ -45,7 +42,7 @@ struct FR3Config : common::RConfig { bool async_control = false; }; -struct FR3State : common::RState {}; +struct FR3State : common::RobotState {}; class FR3 : public common::Robot { private: @@ -79,9 +76,9 @@ class FR3 : public common::Robot { common::Pose get_cartesian_position() override; - void set_joint_position(const common::Vector7d &q) override; + void set_joint_position(const common::VectorXd &q) override; - common::Vector7d get_joint_position() override; + common::VectorXd get_joint_position() override; void set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, bool yaw, bool elbow); diff --git a/src/hw/MotionGenerator.cpp b/src/hw/FR3MotionGenerator.cpp similarity index 94% rename from src/hw/MotionGenerator.cpp rename to src/hw/FR3MotionGenerator.cpp index 967ab0f0..810cd2f1 100644 --- a/src/hw/MotionGenerator.cpp +++ b/src/hw/FR3MotionGenerator.cpp @@ -1,4 +1,4 @@ -#include "MotionGenerator.h" +#include "FR3MotionGenerator.h" #include #include @@ -23,8 +23,8 @@ void setDefaultBehavior(franka::Robot& robot) { robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); } -MotionGenerator::MotionGenerator(double speed_factor, - const common::Vector7d q_goal) +FR3MotionGenerator::FR3MotionGenerator(double speed_factor, + const common::Vector7d q_goal) : q_goal_(q_goal) { dq_max_ *= speed_factor; ddq_max_start_ *= speed_factor; @@ -38,7 +38,7 @@ MotionGenerator::MotionGenerator(double speed_factor, q_1_.setZero(); } -bool MotionGenerator::calculateDesiredValues( +bool FR3MotionGenerator::calculateDesiredValues( double t, common::Vector7d* delta_q_d) const { common::Vector7i sign_delta_q; sign_delta_q << delta_q_.cwiseSign().cast(); @@ -78,7 +78,7 @@ bool MotionGenerator::calculateDesiredValues( joint_motion_finished.cend(), [](bool x) { return x; }); } -void MotionGenerator::calculateSynchronizedValues() { +void FR3MotionGenerator::calculateSynchronizedValues() { common::Vector7d dq_max_reach(dq_max_); common::Vector7d t_f = common::Vector7d::Zero(); common::Vector7d delta_t_2 = common::Vector7d::Zero(); @@ -123,7 +123,7 @@ void MotionGenerator::calculateSynchronizedValues() { } } -franka::JointPositions MotionGenerator::operator()( +franka::JointPositions FR3MotionGenerator::operator()( const franka::RobotState& robot_state, franka::Duration period) { time_ += period.toSec(); diff --git a/src/hw/MotionGenerator.h b/src/hw/FR3MotionGenerator.h similarity index 93% rename from src/hw/MotionGenerator.h rename to src/hw/FR3MotionGenerator.h index 981763b8..9777aabe 100644 --- a/src/hw/MotionGenerator.h +++ b/src/hw/FR3MotionGenerator.h @@ -33,15 +33,15 @@ void setDefaultBehavior(franka::Robot& robot); * Identification and Control of Robots (Kogan Page Science Paper edition). * */ -class MotionGenerator { +class FR3MotionGenerator { public: /** - * Creates a new MotionGenerator instance for a target q. + * Creates a new FR3MotionGenerator instance for a target q. * * @param[in] speed_factor General speed factor in range [0, 1]. * @param[in] q_goal Target joint positions. */ - MotionGenerator(double speed_factor, const common::Vector7d q_goal); + FR3MotionGenerator(double speed_factor, const common::Vector7d q_goal); /** * Sends joint position calculations diff --git a/src/hw/FrankaHand.h b/src/hw/FrankaHand.h index 1ddf40a3..95e24bd2 100644 --- a/src/hw/FrankaHand.h +++ b/src/hw/FrankaHand.h @@ -19,7 +19,7 @@ namespace rcs { namespace hw { -struct FHConfig : common::GConfig { +struct FHConfig : common::GripperConfig { double grasping_width = 0.05; double speed = 0.1; double force = 5; @@ -28,7 +28,7 @@ struct FHConfig : common::GConfig { bool async_control = false; }; -struct FHState : common::GState { +struct FHState : common::GripperState { double width; // true is open bool bool_state; diff --git a/src/pybind/CMakeLists.txt b/src/pybind/CMakeLists.txt index 0067d9da..ff76e118 100644 --- a/src/pybind/CMakeLists.txt +++ b/src/pybind/CMakeLists.txt @@ -1,4 +1,4 @@ -pybind11_add_module(_core MODULE rcsss.cpp) +pybind11_add_module(_core MODULE rcs.cpp) target_link_libraries(_core PRIVATE hw sim common) target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) @@ -10,4 +10,4 @@ set_target_properties(franka PROPERTIES INSTALL_RPATH "$ORIGIN/../cmeel.prefix/lib" INTERPROCEDURAL_OPTIMIZATION TRUE ) -install(TARGETS _core franka DESTINATION rcsss COMPONENT python_package) +install(TARGETS _core franka DESTINATION rcs COMPONENT python_package) diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcs.cpp similarity index 76% rename from src/pybind/rcsss.cpp rename to src/pybind/rcs.cpp index b6e2eb8d..dc45d13b 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcs.cpp @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -11,8 +10,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -37,13 +36,13 @@ class PyRobot : public rcs::common::Robot { public: using rcs::common::Robot::Robot; // Inherit constructors - rcs::common::RConfig *get_parameters() override { - PYBIND11_OVERRIDE_PURE(rcs::common::RConfig *, rcs::common::Robot, + rcs::common::RobotConfig *get_parameters() override { + PYBIND11_OVERRIDE_PURE(rcs::common::RobotConfig *, rcs::common::Robot, get_parameters, ); } - rcs::common::RState *get_state() override { - PYBIND11_OVERRIDE_PURE(rcs::common::RState *, rcs::common::Robot, + rcs::common::RobotState *get_state() override { + PYBIND11_OVERRIDE_PURE(rcs::common::RobotState *, rcs::common::Robot, get_state, ); } @@ -52,12 +51,12 @@ class PyRobot : public rcs::common::Robot { get_cartesian_position, ); } - void set_joint_position(const rcs::common::Vector7d &q) override { + void set_joint_position(const rcs::common::VectorXd &q) override { PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, set_joint_position, q); } - rcs::common::Vector7d get_joint_position() override { - PYBIND11_OVERRIDE_PURE(rcs::common::Vector7d, rcs::common::Robot, + rcs::common::VectorXd get_joint_position() override { + PYBIND11_OVERRIDE_PURE(rcs::common::VectorXd, rcs::common::Robot, get_joint_position, ); } @@ -82,13 +81,13 @@ class PyGripper : public rcs::common::Gripper { public: using rcs::common::Gripper::Gripper; // Inherit constructors - rcs::common::GConfig *get_parameters() override { - PYBIND11_OVERRIDE_PURE(rcs::common::GConfig *, rcs::common::Gripper, + rcs::common::GripperConfig *get_parameters() override { + PYBIND11_OVERRIDE_PURE(rcs::common::GripperConfig *, rcs::common::Gripper, get_parameters, ); } - rcs::common::GState *get_state() override { - PYBIND11_OVERRIDE_PURE(rcs::common::GState *, rcs::common::Gripper, + rcs::common::GripperState *get_state() override { + PYBIND11_OVERRIDE_PURE(rcs::common::GripperState *, rcs::common::Gripper, get_state, ); } @@ -143,7 +142,7 @@ PYBIND11_MODULE(_core, m) { common.def("IdentityTranslation", &rcs::common::IdentityTranslation); common.def("IdentityRotMatrix", &rcs::common::IdentityRotMatrix); - common.def("IdentityRotQuartVec", &rcs::common::IdentityRotQuartVec); + common.def("IdentityRotQuatVec", &rcs::common::IdentityRotQuatVec); common.def("FrankaHandTCPOffset", &rcs::common::FrankaHandTCPOffset); py::class_(common, "RPY") @@ -217,10 +216,36 @@ PYBIND11_MODULE(_core, m) { .def("ik", &rcs::common::IK::ik, py::arg("pose"), py::arg("q0"), py::arg("tcp_offset") = rcs::common::Pose::Identity()); - py::class_(common, "RConfig"); - py::class_(common, "RState"); - py::class_(common, "GConfig"); - py::class_(common, "GState"); + py::enum_(common, "RobotType") + .value("FR3", rcs::common::RobotType::FR3) + .value("UR5e", rcs::common::RobotType::UR5e) + .export_values(); + + py::enum_(common, "RobotPlatform") + .value("HARDWARE", rcs::common::RobotPlatform::HARDWARE) + .value("SIMULATION", rcs::common::RobotPlatform::SIMULATION) + .export_values(); + + py::class_(common, "RobotMetaConfig") + .def_readonly("q_home", &rcs::common::RobotMetaConfig::q_home) + .def_readonly("dof", &rcs::common::RobotMetaConfig::dof) + .def_readonly("joint_limits", + &rcs::common::RobotMetaConfig::joint_limits); + + common.def( + "robots_meta_config", + [](rcs::common::RobotType robot_type) -> rcs::common::RobotMetaConfig { + return rcs::common::robots_meta_config.at(robot_type); + }, + py::arg("robot_type")); + + py::class_(common, "RobotConfig") + .def_readwrite("robot_type", &rcs::common::RobotConfig::robot_type) + .def_readwrite("robot_platform", + &rcs::common::RobotConfig::robot_platform); + py::class_(common, "RobotState"); + py::class_(common, "GripperConfig"); + py::class_(common, "GripperState"); // holder type should be smart pointer as we deal with smart pointer // instances of this class @@ -266,47 +291,10 @@ PYBIND11_MODULE(_core, m) { .def("reset", &rcs::common::Gripper::reset, py::call_guard()); - py::class_>(common, - "RobotWithGripper") - .def(py::init, - std::optional>>(), - py::arg("robot"), py::arg("gripper")); - - py::class_(common, "NRobotsWithGripper") - .def(py::init< - std::vector>>(), - py::arg("robots_with_gripper")) - .def("get_parameters_r", - &rcs::common::NRobotsWithGripper::get_parameters_r, py::arg("idxs")) - .def("get_state_r", &rcs::common::NRobotsWithGripper::get_state_r, - py::arg("idxs")) - .def("get_cartesian_position", - &rcs::common::NRobotsWithGripper::get_cartesian_position, - py::arg("idxs")) - .def("set_joint_position", - &rcs::common::NRobotsWithGripper::set_joint_position, - py::arg("idxs"), py::arg("q")) - .def("get_joint_position", - &rcs::common::NRobotsWithGripper::get_joint_position, - py::arg("idxs")) - .def("move_home", &rcs::common::NRobotsWithGripper::move_home, - py::arg("idxs")) - .def("set_cartesian_position", - &rcs::common::NRobotsWithGripper::set_cartesian_position, - py::arg("idxs"), py::arg("pose")) - .def("get_parameters_g", - &rcs::common::NRobotsWithGripper::get_parameters_g, py::arg("idxs")) - .def("get_state_g", &rcs::common::NRobotsWithGripper::get_state_g, - py::arg("idxs")) - .def("grasp", &rcs::common::NRobotsWithGripper::grasp, py::arg("idxs")) - .def("open", &rcs::common::NRobotsWithGripper::open, py::arg("idxs")) - .def("shut", &rcs::common::NRobotsWithGripper::shut, py::arg("idxs")); - // HARDWARE MODULE auto hw = m.def_submodule("hw", "hardware module"); - py::class_(hw, "FR3State") + py::class_(hw, "FR3State") .def(py::init<>()); py::class_(hw, "FR3Load") .def(py::init<>()) @@ -315,11 +303,11 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("load_inertia", &rcs::hw::FR3Load::load_inertia); py::enum_(hw, "IKSolver") - .value("franka", rcs::hw::IKSolver::franka) - .value("rcs", rcs::hw::IKSolver::rcs) + .value("franka_ik", rcs::hw::IKSolver::franka_ik) + .value("rcs_ik", rcs::hw::IKSolver::rcs_ik) .export_values(); - py::class_(hw, "FR3Config") + py::class_(hw, "FR3Config") .def(py::init<>()) .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor) @@ -330,7 +318,7 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); - py::class_(hw, "FHConfig") + py::class_(hw, "FHConfig") .def(py::init<>()) .def_readwrite("grasping_width", &rcs::hw::FHConfig::grasping_width) .def_readwrite("speed", &rcs::hw::FHConfig::speed) @@ -339,7 +327,7 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); - py::class_(hw, "FHState") + py::class_(hw, "FHState") .def(py::init<>()) .def_readonly("width", &rcs::hw::FHState::width) .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped) @@ -415,43 +403,50 @@ PYBIND11_MODULE(_core, m) { // SIM MODULE auto sim = m.def_submodule("sim", "sim module"); - py::class_(sim, "FR3Config") + py::class_( + sim, "SimRobotConfig") .def(py::init<>()) - .def_readwrite("tcp_offset", &rcs::sim::FR3Config::tcp_offset) + .def_readwrite("tcp_offset", &rcs::sim::SimRobotConfig::tcp_offset) .def_readwrite("joint_rotational_tolerance", - &rcs::sim::FR3Config::joint_rotational_tolerance) + &rcs::sim::SimRobotConfig::joint_rotational_tolerance) .def_readwrite("seconds_between_callbacks", - &rcs::sim::FR3Config::seconds_between_callbacks) - .def_readwrite("realtime", &rcs::sim::FR3Config::realtime) + &rcs::sim::SimRobotConfig::seconds_between_callbacks) + .def_readwrite("realtime", &rcs::sim::SimRobotConfig::realtime) .def_readwrite("trajectory_trace", - &rcs::sim::FR3Config::trajectory_trace); - py::class_(sim, "FR3State") + &rcs::sim::SimRobotConfig::trajectory_trace); + py::class_(sim, + "SimRobotState") .def(py::init<>()) - .def_readonly("previous_angles", &rcs::sim::FR3State::previous_angles) - .def_readonly("target_angles", &rcs::sim::FR3State::target_angles) + .def_readonly("previous_angles", + &rcs::sim::SimRobotState::previous_angles) + .def_readonly("target_angles", &rcs::sim::SimRobotState::target_angles) .def_readonly("inverse_tcp_offset", - &rcs::sim::FR3State::inverse_tcp_offset) - .def_readonly("ik_success", &rcs::sim::FR3State::ik_success) - .def_readonly("collision", &rcs::sim::FR3State::collision) - .def_readonly("is_moving", &rcs::sim::FR3State::is_moving) - .def_readonly("is_arrived", &rcs::sim::FR3State::is_arrived); - py::class_(sim, "FHConfig") + &rcs::sim::SimRobotState::inverse_tcp_offset) + .def_readonly("ik_success", &rcs::sim::SimRobotState::ik_success) + .def_readonly("collision", &rcs::sim::SimRobotState::collision) + .def_readonly("is_moving", &rcs::sim::SimRobotState::is_moving) + .def_readonly("is_arrived", &rcs::sim::SimRobotState::is_arrived); + py::class_( + sim, "SimGripperConfig") .def(py::init<>()) - .def_readwrite("epsilon_inner", &rcs::sim::FHConfig::epsilon_inner) - .def_readwrite("epsilon_outer", &rcs::sim::FHConfig::epsilon_outer) + .def_readwrite("epsilon_inner", + &rcs::sim::SimGripperConfig::epsilon_inner) + .def_readwrite("epsilon_outer", + &rcs::sim::SimGripperConfig::epsilon_outer) .def_readwrite("seconds_between_callbacks", - &rcs::sim::FHConfig::seconds_between_callbacks) + &rcs::sim::SimGripperConfig::seconds_between_callbacks) .def_readwrite("ignored_collision_geoms", - &rcs::sim::FHConfig::ignored_collision_geoms); - py::class_(sim, "FHState") + &rcs::sim::SimGripperConfig::ignored_collision_geoms); + py::class_( + sim, "SimGripperState") .def(py::init<>()) .def_readonly("last_commanded_width", - &rcs::sim::FHState::last_commanded_width) + &rcs::sim::SimGripperState::last_commanded_width) .def_readonly("max_unnormalized_width", - &rcs::sim::FHState::max_unnormalized_width) - .def_readonly("is_moving", &rcs::sim::FHState::is_moving) - .def_readonly("last_width", &rcs::sim::FHState::last_width) - .def_readonly("collision", &rcs::sim::FHState::collision); + &rcs::sim::SimGripperState::max_unnormalized_width) + .def_readonly("is_moving", &rcs::sim::SimGripperState::is_moving) + .def_readonly("last_width", &rcs::sim::SimGripperState::last_width) + .def_readonly("collision", &rcs::sim::SimGripperState::collision); py::class_>(sim, "Sim") .def(py::init([](long m, long d) { @@ -465,25 +460,27 @@ PYBIND11_MODULE(_core, m) { .def("reset", &rcs::sim::Sim::reset) .def("_start_gui_server", &rcs::sim::Sim::start_gui_server, py::arg("id")) .def("_stop_gui_server", &rcs::sim::Sim::stop_gui_server); - py::class_>(sim, "FrankaHand") + py::class_>(sim, "SimGripper") .def(py::init, const std::string &, - const rcs::sim::FHConfig &>(), + const rcs::sim::SimGripperConfig &>(), py::arg("sim"), py::arg("id"), py::arg("cfg")) - .def("get_parameters", &rcs::sim::FrankaHand::get_parameters) - .def("get_state", &rcs::sim::FrankaHand::get_state) - .def("set_parameters", &rcs::sim::FrankaHand::set_parameters, + .def("get_parameters", &rcs::sim::SimGripper::get_parameters) + .def("get_state", &rcs::sim::SimGripper::get_state) + .def("set_parameters", &rcs::sim::SimGripper::set_parameters, py::arg("cfg")); - py::class_>( - sim, "FR3") + py::class_>(sim, "SimRobot") .def(py::init, const std::string &, std::shared_ptr, bool>(), py::arg("sim"), py::arg("id"), py::arg("ik"), py::arg("register_convergence_callback") = true) - .def("get_parameters", &rcs::sim::FR3::get_parameters) - .def("set_parameters", &rcs::sim::FR3::set_parameters, py::arg("cfg")) - .def("set_joints_hard", &rcs::sim::FR3::set_joints_hard, py::arg("q")) - .def("get_state", &rcs::sim::FR3::get_state); + .def("get_parameters", &rcs::sim::SimRobot::get_parameters) + .def("set_parameters", &rcs::sim::SimRobot::set_parameters, + py::arg("cfg")) + .def("set_joints_hard", &rcs::sim::SimRobot::set_joints_hard, + py::arg("q")) + .def("get_state", &rcs::sim::SimRobot::get_state); py::enum_(sim, "CameraType") .value("free", rcs::sim::CameraType::free) .value("tracking", rcs::sim::CameraType::tracking) diff --git a/src/sim/CMakeLists.txt b/src/sim/CMakeLists.txt index 16ddf7ca..b0b90079 100644 --- a/src/sim/CMakeLists.txt +++ b/src/sim/CMakeLists.txt @@ -1,6 +1,6 @@ add_library(sim) target_sources(sim - PRIVATE sim.cpp FR3.cpp renderer.cpp camera.cpp FrankaHand.cpp gui_server.cpp gui_client.cpp + PRIVATE sim.cpp SimRobot.cpp renderer.cpp camera.cpp SimGripper.cpp gui_server.cpp gui_client.cpp ) target_link_libraries(sim PUBLIC glfw common MuJoCo::MuJoCo mujoco_ui_lib) diff --git a/src/sim/FrankaHand.cpp b/src/sim/SimGripper.cpp similarity index 82% rename from src/sim/FrankaHand.cpp rename to src/sim/SimGripper.cpp index 1a127f1c..49948c4a 100644 --- a/src/sim/FrankaHand.cpp +++ b/src/sim/SimGripper.cpp @@ -1,5 +1,5 @@ -#include "FrankaHand.h" +#include "SimGripper.h" #include #include @@ -23,10 +23,10 @@ struct { namespace rcs { namespace sim { -FrankaHand::FrankaHand(std::shared_ptr sim, const std::string &id, - const FHConfig &cfg) +SimGripper::SimGripper(std::shared_ptr sim, const std::string &id, + const SimGripperConfig &cfg) : sim{sim}, cfg{cfg}, id{id} { - this->state = FHState(); + this->state = SimGripperState(); this->actuator_id = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, (gripper_names.actuator + "_" + id).c_str()); if (this->actuator_id == -1) { @@ -52,16 +52,16 @@ FrankaHand::FrankaHand(std::shared_ptr sim, const std::string &id, this->add_collision_geoms(gripper_names.collision_geoms_fingers, this->cfgeom, false); - this->sim->register_all_cb(std::bind(&FrankaHand::convergence_callback, this), + this->sim->register_all_cb(std::bind(&SimGripper::convergence_callback, this), this->cfg.seconds_between_callbacks); - this->sim->register_any_cb(std::bind(&FrankaHand::collision_callback, this), + this->sim->register_any_cb(std::bind(&SimGripper::collision_callback, this), this->cfg.seconds_between_callbacks); this->m_reset(); } -FrankaHand::~FrankaHand() {} +SimGripper::~SimGripper() {} -void FrankaHand::add_collision_geoms(const std::vector &cgeoms_str, +void SimGripper::add_collision_geoms(const std::vector &cgeoms_str, std::set &cgeoms_set, bool clear_before) { if (clear_before) { @@ -78,26 +78,26 @@ void FrankaHand::add_collision_geoms(const std::vector &cgeoms_str, } } -bool FrankaHand::set_parameters(const FHConfig &cfg) { +bool SimGripper::set_parameters(const SimGripperConfig &cfg) { this->cfg = cfg; this->add_collision_geoms(cfg.ignored_collision_geoms, this->ignored_collision_geoms, true); return true; } -FHConfig *FrankaHand::get_parameters() { +SimGripperConfig *SimGripper::get_parameters() { // copy config to heap - FHConfig *cfg = new FHConfig(); + SimGripperConfig *cfg = new SimGripperConfig(); *cfg = this->cfg; return cfg; } -FHState *FrankaHand::get_state() { - FHState *state = new FHState(); +SimGripperState *SimGripper::get_state() { + SimGripperState *state = new SimGripperState(); *state = this->state; return state; } -void FrankaHand::set_normalized_width(double width, double force) { +void SimGripper::set_normalized_width(double width, double force) { if (width < 0 || width > 1 || force < 0) { throw std::invalid_argument( "width must be between 0 and 1, force must be positive"); @@ -108,7 +108,7 @@ void FrankaHand::set_normalized_width(double width, double force) { // we ignore force for now // this->sim->d->actuator_force[this->gripper_id] = 0; } -double FrankaHand::get_normalized_width() { +double SimGripper::get_normalized_width() { // TODO: maybe we should use the mujoco sensors? Not sure what the difference // is between reading out from qpos and reading from the sensors. double width = this->sim->d->qpos[this->joint_id_1] / this->MAX_JOINT_WIDTH; @@ -121,7 +121,7 @@ double FrankaHand::get_normalized_width() { return width; } -bool FrankaHand::collision_callback() { +bool SimGripper::collision_callback() { this->state.collision = false; for (size_t i = 0; i < this->sim->d->ncon; ++i) { if (this->cfgeom.contains(this->sim->d->contact[i].geom[0]) && @@ -145,7 +145,7 @@ bool FrankaHand::collision_callback() { return this->state.collision; } -bool FrankaHand::is_grasped() { +bool SimGripper::is_grasped() { double width = this->get_normalized_width(); if (this->state.last_commanded_width - this->cfg.epsilon_inner < width && @@ -156,7 +156,7 @@ bool FrankaHand::is_grasped() { return false; } -bool FrankaHand::convergence_callback() { +bool SimGripper::convergence_callback() { double w = get_normalized_width(); this->state.is_moving = std::abs(this->state.last_width - w) > 0.001 / this->MAX_WIDTH; // 1mm tolerance @@ -164,13 +164,13 @@ bool FrankaHand::convergence_callback() { return not this->state.is_moving; } -void FrankaHand::grasp() { this->shut(); } +void SimGripper::grasp() { this->shut(); } -void FrankaHand::open() { this->set_normalized_width(1); } -void FrankaHand::shut() { this->set_normalized_width(0); } +void SimGripper::open() { this->set_normalized_width(1); } +void SimGripper::shut() { this->set_normalized_width(0); } -void FrankaHand::m_reset() { - this->state = FHState(); +void SimGripper::m_reset() { + this->state = SimGripperState(); this->state.max_unnormalized_width = this->MAX_WIDTH; // reset state hard this->sim->d->qpos[this->joint_id_1] = this->MAX_JOINT_WIDTH; @@ -178,6 +178,6 @@ void FrankaHand::m_reset() { this->sim->d->ctrl[this->actuator_id] = this->MAX_WIDTH; } -void FrankaHand::reset() { this->m_reset(); } +void SimGripper::reset() { this->m_reset(); } } // namespace sim } // namespace rcs diff --git a/src/sim/FrankaHand.h b/src/sim/SimGripper.h similarity index 75% rename from src/sim/FrankaHand.h rename to src/sim/SimGripper.h index 357a3b47..8a06f4ef 100644 --- a/src/sim/FrankaHand.h +++ b/src/sim/SimGripper.h @@ -13,14 +13,14 @@ namespace rcs { namespace sim { -struct FHConfig : common::GConfig { +struct SimGripperConfig : common::GripperConfig { double epsilon_inner = 0.005; double epsilon_outer = 0.005; double seconds_between_callbacks = 0.05; // 20 Hz std::vector ignored_collision_geoms = {}; }; -struct FHState : common::GState { +struct SimGripperState : common::GripperState { double last_commanded_width = 0; double max_unnormalized_width = 0; bool is_moving = false; @@ -28,16 +28,16 @@ struct FHState : common::GState { bool collision = false; }; -class FrankaHand : public common::Gripper { +class SimGripper : public common::Gripper { private: const double MAX_WIDTH = 255; const double MAX_JOINT_WIDTH = 0.04; - FHConfig cfg; + SimGripperConfig cfg; std::shared_ptr sim; int actuator_id; int joint_id_1; int joint_id_2; - FHState state; + SimGripperState state; std::string id; bool convergence_callback(); bool collision_callback(); @@ -49,15 +49,15 @@ class FrankaHand : public common::Gripper { void m_reset(); public: - FrankaHand(std::shared_ptr sim, const std::string &id, - const FHConfig &cfg); - ~FrankaHand() override; + SimGripper(std::shared_ptr sim, const std::string &id, + const SimGripperConfig &cfg); + ~SimGripper() override; - bool set_parameters(const FHConfig &cfg); + bool set_parameters(const SimGripperConfig &cfg); - FHConfig *get_parameters() override; + SimGripperConfig *get_parameters() override; - FHState *get_state() override; + SimGripperState *get_state() override; // normalized width of the gripper, 0 is closed, 1 is open void set_normalized_width(double width, double force = 0) override; diff --git a/src/sim/FR3.cpp b/src/sim/SimRobot.cpp similarity index 75% rename from src/sim/FR3.cpp rename to src/sim/SimRobot.cpp index 25747af9..25720e8d 100644 --- a/src/sim/FR3.cpp +++ b/src/sim/SimRobot.cpp @@ -1,4 +1,4 @@ -#include "FR3.h" +#include "SimRobot.h" #include #include @@ -19,10 +19,6 @@ #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" -const rcs::common::Vector7d q_home((rcs::common::Vector7d() << 0, -M_PI_4, 0, - -3 * M_PI_4, 0, M_PI_2, M_PI_4) - .finished()); - struct { std::array arm_collision_geoms{ "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision", @@ -42,28 +38,32 @@ namespace rcs { namespace sim { // TODO: use C++11 feature to call one constructor from another -FR3::FR3(std::shared_ptr sim, const std::string& id, - std::shared_ptr ik, bool register_convergence_callback) +SimRobot::SimRobot(std::shared_ptr sim, const std::string& id, + std::shared_ptr ik, + bool register_convergence_callback) : sim{sim}, id{id}, cfg{}, state{}, m_ik(ik) { this->init_ids(); if (register_convergence_callback) { - this->sim->register_cb(std::bind(&FR3::is_arrived_callback, this), + this->sim->register_cb(std::bind(&SimRobot::is_arrived_callback, this), this->cfg.seconds_between_callbacks); - this->sim->register_cb(std::bind(&FR3::is_moving_callback, this), + this->sim->register_cb(std::bind(&SimRobot::is_moving_callback, this), this->cfg.seconds_between_callbacks); - this->sim->register_all_cb(std::bind(&FR3::convergence_callback, this), + this->sim->register_all_cb(std::bind(&SimRobot::convergence_callback, this), this->cfg.seconds_between_callbacks); } - this->sim->register_any_cb(std::bind(&FR3::collision_callback, this), + this->sim->register_any_cb(std::bind(&SimRobot::collision_callback, this), this->cfg.seconds_between_callbacks); this->m_reset(); } -FR3::~FR3() {} +SimRobot::~SimRobot() {} -void FR3::move_home() { this->set_joint_position(q_home); } +void SimRobot::move_home() { + this->set_joint_position( + common::robots_meta_config.at(this->cfg.robot_type).q_home); +} -void FR3::init_ids() { +void SimRobot::init_ids() { std::string name; // Collision geoms for (size_t i = 0; i < std::size(model_names.arm_collision_geoms); ++i) { @@ -107,25 +107,25 @@ void FR3::init_ids() { } } -bool FR3::set_parameters(const FR3Config& cfg) { +bool SimRobot::set_parameters(const SimRobotConfig& cfg) { this->cfg = cfg; this->state.inverse_tcp_offset = cfg.tcp_offset.inverse(); return true; } -FR3Config* FR3::get_parameters() { - FR3Config* cfg = new FR3Config(); +SimRobotConfig* SimRobot::get_parameters() { + SimRobotConfig* cfg = new SimRobotConfig(); *cfg = this->cfg; return cfg; } -FR3State* FR3::get_state() { - FR3State* state = new FR3State(); +SimRobotState* SimRobot::get_state() { + SimRobotState* state = new SimRobotState(); *state = this->state; return state; } -common::Pose FR3::get_cartesian_position() { +common::Pose SimRobot::get_cartesian_position() { Eigen::Matrix rotation( this->sim->d->site_xmat + 9 * this->ids.attachment_site); Eigen::Vector3d translation(this->sim->d->site_xpos + @@ -134,7 +134,7 @@ common::Pose FR3::get_cartesian_position() { return this->to_pose_in_robot_coordinates(attachment_site) * cfg.tcp_offset; } -void FR3::set_joint_position(const common::Vector7d& q) { +void SimRobot::set_joint_position(const common::VectorXd& q) { this->state.target_angles = q; this->state.previous_angles = this->get_joint_position(); this->state.is_moving = true; @@ -144,17 +144,19 @@ void FR3::set_joint_position(const common::Vector7d& q) { } } -common::Vector7d FR3::get_joint_position() { - common::Vector7d q; +common::VectorXd SimRobot::get_joint_position() { + common::VectorXd q(std::size(model_names.joints)); for (size_t i = 0; i < std::size(model_names.joints); ++i) { q[i] = this->sim->d->qpos[this->sim->m->jnt_qposadr[this->ids.joints[i]]]; } return q; } -std::optional> FR3::get_ik() { return this->m_ik; } +std::optional> SimRobot::get_ik() { + return this->m_ik; +} -void FR3::set_cartesian_position(const common::Pose& pose) { +void SimRobot::set_cartesian_position(const common::Pose& pose) { // pose is assumed to be in the robots coordinate frame auto joint_vals = this->m_ik->ik(pose, this->get_joint_position(), this->cfg.tcp_offset); @@ -165,8 +167,8 @@ void FR3::set_cartesian_position(const common::Pose& pose) { this->state.ik_success = false; } } -void FR3::is_moving_callback() { - common::Vector7d current_angles = this->get_joint_position(); +void SimRobot::is_moving_callback() { + common::VectorXd current_angles = this->get_joint_position(); // difference of the largest element is smaller than threshold this->state.is_moving = (current_angles - this->state.previous_angles).cwiseAbs().maxCoeff() > @@ -174,14 +176,14 @@ void FR3::is_moving_callback() { this->state.previous_angles = current_angles; } -void FR3::is_arrived_callback() { - common::Vector7d current_angles = this->get_joint_position(); +void SimRobot::is_arrived_callback() { + common::VectorXd current_angles = this->get_joint_position(); this->state.is_arrived = (current_angles - this->state.target_angles).cwiseAbs().maxCoeff() < this->cfg.joint_rotational_tolerance; } -bool FR3::collision_callback() { +bool SimRobot::collision_callback() { this->state.collision = false; for (size_t i = 0; i < this->sim->d->ncon; ++i) { if (this->ids.cgeom.contains(this->sim->d->contact[i].geom[0]) || @@ -193,7 +195,7 @@ bool FR3::collision_callback() { return this->state.collision; } -bool FR3::convergence_callback() { +bool SimRobot::convergence_callback() { /* When ik failed, the robot is not doing anything */ if (not this->state.ik_success) { return true; @@ -202,9 +204,12 @@ bool FR3::convergence_callback() { return this->state.is_arrived and not this->state.is_moving; } -void FR3::m_reset() { this->set_joints_hard(q_home); } +void SimRobot::m_reset() { + this->set_joints_hard( + common::robots_meta_config.at(this->cfg.robot_type).q_home); +} -void FR3::set_joints_hard(const common::Vector7d& q) { +void SimRobot::set_joints_hard(const common::VectorXd& q) { for (size_t i = 0; i < std::size(this->ids.joints); ++i) { size_t jnt_id = this->ids.joints[i]; size_t jnt_qposadr = this->sim->m->jnt_qposadr[jnt_id]; @@ -213,7 +218,7 @@ void FR3::set_joints_hard(const common::Vector7d& q) { } } -common::Pose FR3::get_base_pose_in_world_coordinates() { +common::Pose SimRobot::get_base_pose_in_world_coordinates() { auto id = mj_name2id(this->sim->m, mjOBJ_BODY, (std::string("base_") + this->id).c_str()); Eigen::Map translation(this->sim->d->xpos + 3 * id); @@ -222,6 +227,6 @@ common::Pose FR3::get_base_pose_in_world_coordinates() { return common::Pose(rotation, translation); } -void FR3::reset() { this->m_reset(); } +void SimRobot::reset() { this->m_reset(); } } // namespace sim } // namespace rcs diff --git a/src/sim/FR3.h b/src/sim/SimRobot.h similarity index 63% rename from src/sim/FR3.h rename to src/sim/SimRobot.h index 52964f71..e7daf958 100644 --- a/src/sim/FR3.h +++ b/src/sim/SimRobot.h @@ -11,22 +11,19 @@ namespace rcs { namespace sim { -const common::Vector7d q_home((common::Vector7d() << 0, -M_PI_4, 0, -3 * M_PI_4, - 0, M_PI_2, M_PI_4) - .finished()); - -struct FR3Config : common::RConfig { +struct SimRobotConfig : common::RobotConfig { rcs::common::Pose tcp_offset = rcs::common::Pose::Identity(); double joint_rotational_tolerance = .05 * (std::numbers::pi / 180.0); // 0.05 degree double seconds_between_callbacks = 0.1; // 10 Hz bool realtime = false; bool trajectory_trace = false; + common::RobotType robot_type = common::RobotType::FR3; }; -struct FR3State : common::RState { - common::Vector7d previous_angles; - common::Vector7d target_angles; +struct SimRobotState : common::RobotState { + common::VectorXd previous_angles; + common::VectorXd target_angles; common::Pose inverse_tcp_offset; bool ik_success = true; bool collision = false; @@ -34,28 +31,28 @@ struct FR3State : common::RState { bool is_arrived = false; }; -class FR3 : public common::Robot { +class SimRobot : public common::Robot { public: - FR3(std::shared_ptr sim, const std::string &id, - std::shared_ptr ik, - bool register_convergence_callback = true); - ~FR3() override; - bool set_parameters(const FR3Config &cfg); - FR3Config *get_parameters() override; - FR3State *get_state() override; + SimRobot(std::shared_ptr sim, const std::string &id, + std::shared_ptr ik, + bool register_convergence_callback = true); + ~SimRobot() override; + bool set_parameters(const SimRobotConfig &cfg); + SimRobotConfig *get_parameters() override; + SimRobotState *get_state() override; common::Pose get_cartesian_position() override; - void set_joint_position(const common::Vector7d &q) override; - common::Vector7d get_joint_position() override; + void set_joint_position(const common::VectorXd &q) override; + common::VectorXd get_joint_position() override; void move_home() override; void set_cartesian_position(const common::Pose &pose) override; common::Pose get_base_pose_in_world_coordinates() override; std::optional> get_ik() override; void reset() override; - void set_joints_hard(const common::Vector7d &q); + void set_joints_hard(const common::VectorXd &q); private: - FR3Config cfg; - FR3State state; + SimRobotConfig cfg; + SimRobotState state; std::shared_ptr sim; std::string id; std::shared_ptr m_ik; diff --git a/src/sim/test.cpp b/src/sim/test.cpp index 3c700473..c09e7eb2 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -10,7 +10,7 @@ #include "common/Pose.h" #include "sim.h" -#include "sim/FR3.h" +#include "sim/SimRobot.h" // const std::string mjcf = MODEL_DIR "/mjcf/fr3_modular/scene.xml"; // const std::string urdf = MODEL_DIR "/fr3/urdf/fr3_from_panda.urdf"; @@ -127,9 +127,9 @@ int test_sim() { std::string id = "0"; auto ik = std::make_shared(urdf); - auto fr3 = rcs::sim::FR3(sim, id, ik); + auto fr3 = rcs::sim::SimRobot(sim, id, ik); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); - rcs::sim::FR3Config fr3_config = *fr3.get_parameters(); + rcs::sim::SimRobotConfig fr3_config = *fr3.get_parameters(); fr3_config.tcp_offset = tcp_offset; fr3_config.seconds_between_callbacks = 0.05; // 20hz fr3.set_parameters(fr3_config);