diff --git a/CMakeLists.txt b/CMakeLists.txt index 636ecf15..3b98d648 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ find_package(MuJoCo REQUIRED) FetchContent_Declare( libfranka GIT_REPOSITORY https://github.com/frankaemika/libfranka.git - GIT_TAG 0.14.2 + GIT_TAG 0.15.0 GIT_PROGRESS TRUE EXCLUDE_FROM_ALL ) @@ -98,7 +98,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref bfdd70ae189ed3a925bf1c16d29162e847a21d56) + set(ref 055a6fdab480dde8443030236012f76d9be90918) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/pyproject.toml b/pyproject.toml index f2fde75e..afde7fa0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -22,10 +22,11 @@ dependencies = ["websockets>=11.0", "pillow~=10.3", "python-dotenv==1.0.1", "opencv-python~=4.10.0.84", + "h5py~=3.13.0", # NOTE: when changing the mujoco version, also change it in requirements_dev.txt "mujoco==3.2.6", # NOTE: Same for pinocchio (pin) - "pin==2.7.0" + "pin==2.7.0", ] readme = "README.md" maintainers = [ diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 5bf3b325..62d0f853 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -4,7 +4,7 @@ import numpy as np import rcsss from rcsss import sim -from rcsss._core.hw import FR3Config, IKController +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 @@ -82,7 +82,7 @@ def main(): # add camera to have a rendering gui cameras = { "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), + "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), } cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20) camera_set = SimCameraSet(simulation, cam_cfg) # noqa: F841 @@ -95,7 +95,7 @@ def main(): robot = rcsss.hw.FR3(ROBOT_IP, ik) robot_cfg = FR3Config() robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()) - robot_cfg.controller = IKController.robotics_library + robot_cfg.ik_solver = IKSolver.rcs robot.set_parameters(robot_cfg) gripper_cfg_hw = rcsss.hw.FHConfig() diff --git a/python/examples/grasp_demo.py b/python/examples/grasp_demo.py index 7c3ff1ee..89653a46 100644 --- a/python/examples/grasp_demo.py +++ b/python/examples/grasp_demo.py @@ -92,7 +92,7 @@ def main(): print(env.unwrapped.robot.get_cartesian_position().translation()) # type: ignore # assert False controller = PickUpDemo(env) - controller.pickup("yellow_box_geom") + controller.pickup("box_geom") if __name__ == "__main__": diff --git a/python/examples/grasp_demo_lab.py b/python/examples/grasp_demo_lab.py index d3bb6c62..d4817132 100644 --- a/python/examples/grasp_demo_lab.py +++ b/python/examples/grasp_demo_lab.py @@ -89,7 +89,7 @@ def main(): ) env.reset() controller = PickUpDemo(env) - controller.pickup("yellow_box_geom") + controller.pickup("box_geom") if __name__ == "__main__": diff --git a/python/rcsss/_core/hw/__init__.pyi b/python/rcsss/_core/hw/__init__.pyi index b3423d24..e80e1eda 100644 --- a/python/rcsss/_core/hw/__init__.pyi +++ b/python/rcsss/_core/hw/__init__.pyi @@ -19,13 +19,14 @@ __all__ = [ "FR3Load", "FR3State", "FrankaHand", - "IKController", + "IKSolver", "exceptions", - "internal", - "robotics_library", + "franka", + "rcs", ] class FHConfig(rcsss._core.common.GConfig): + async_control: bool epsilon_inner: float epsilon_outer: float force: float @@ -36,8 +37,12 @@ class FHConfig(rcsss._core.common.GConfig): class FHState(rcsss._core.common.GState): def __init__(self) -> None: ... @property + def bool_state(self) -> bool: ... + @property def is_grasped(self) -> bool: ... @property + def is_moving(self) -> bool: ... + @property def last_commanded_width(self) -> float: ... @property def max_unnormalized_width(self) -> float: ... @@ -49,20 +54,35 @@ class FHState(rcsss._core.common.GState): class FR3(rcsss._core.common.Robot): def __init__(self, ip: str, ik: rcsss._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]] + ) -> None: ... 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 set_cartesian_position_ik( self, pose: rcsss._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_default_robot_behavior(self) -> None: ... - def set_guiding_mode(self, enabled: bool) -> None: ... + def set_guiding_mode( + self, + x: bool = True, + y: bool = True, + z: bool = True, + roll: bool = True, + pitch: bool = True, + yaw: bool = True, + elbow: bool = True, + ) -> None: ... def set_parameters(self, cfg: FR3Config) -> bool: ... + def stop_control_thread(self) -> None: ... + def zero_torque_guiding(self) -> None: ... class FR3Config(rcsss._core.common.RConfig): - controller: IKController - guiding_mode_enabled: bool + async_control: bool + ik_solver: IKSolver load_parameters: FR3Load | None nominal_end_effector_frame: rcsss._core.common.Pose | None speed_factor: float @@ -87,20 +107,20 @@ class FrankaHand(rcsss._core.common.Gripper): def is_grasped(self) -> bool: ... def set_parameters(self, cfg: FHConfig) -> bool: ... -class IKController: +class IKSolver: """ Members: - internal + franka - robotics_library + rcs """ __members__: typing.ClassVar[ - dict[str, IKController] - ] # value = {'internal': , 'robotics_library': } - internal: typing.ClassVar[IKController] # value = - robotics_library: typing.ClassVar[IKController] # value = + dict[str, IKSolver] + ] # value = {'franka': , 'rcs': } + franka: typing.ClassVar[IKSolver] # value = + rcs: typing.ClassVar[IKSolver] # value = def __eq__(self, other: typing.Any) -> bool: ... def __getstate__(self) -> int: ... def __hash__(self) -> int: ... @@ -116,5 +136,5 @@ class IKController: @property def value(self) -> int: ... -internal: IKController # value = -robotics_library: IKController # value = +franka: IKSolver # value = +rcs: IKSolver # value = diff --git a/python/rcsss/camera/hw.py b/python/rcsss/camera/hw.py index 9dd0538c..4c45717e 100644 --- a/python/rcsss/camera/hw.py +++ b/python/rcsss/camera/hw.py @@ -14,6 +14,7 @@ BaseCameraSetConfig, Frame, FrameSet, + SimpleFrameRate, ) @@ -40,6 +41,7 @@ def __init__(self): self._next_ring_index = 0 self._buffer_len = 0 self.writer: dict[str, cv2.VideoWriter] = {} + self.rate = SimpleFrameRate() def buffer_size(self) -> int: return len(self._buffer) - self._buffer.count(None) @@ -91,33 +93,35 @@ def start(self, warm_up: bool = True): self._thread = threading.Thread(target=self.polling_thread, args=(warm_up,)) self._thread.start() - def record_video(self, path: Path, episode: int): + def record_video(self, path: Path, str_id: str): + if self.recording_ongoing(): + return + self.clear_buffer() for camera in self.camera_names: self.writer[camera] = cv2.VideoWriter( - str(path / f"episode_{episode}_{camera}.mp4"), + str(path / f"episode_{str_id}_{camera}.mp4"), # migh require to install ffmpeg cv2.VideoWriter_fourcc(*"mp4v"), # type: ignore self.config.frame_rate, (self.config.resolution_width, self.config.resolution_height), ) + def recording_ongoing(self) -> bool: + with self._buffer_lock: + return len(self.writer) > 0 + def stop_video(self): if len(self.writer) > 0: - for camera_key, writer in self.writer.items(): - for i in range(self._next_ring_index): - frameset = self._buffer[i] - assert frameset is not None - # rgb to bgr as expected by opencv - writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1]) - for camera in self.camera_names: - self.writer[camera].release() - self.writer = {} + with self._buffer_lock: + for camera in self.camera_names: + self.writer[camera].release() + self.writer = {} def warm_up(self): for _ in range(self.config.warm_up_disposal_frames): for camera_name in self.camera_names: self._poll_frame(camera_name) - sleep(1 / self.config.frame_rate) + self.rate(self.config.frame_rate) def polling_thread(self, warm_up: bool = True): if warm_up: @@ -126,16 +130,14 @@ def polling_thread(self, warm_up: bool = True): frame_set = self.poll_frame_set() with self._buffer_lock: self._buffer[self._next_ring_index] = frame_set + # copy the buffer to the record path + for camera_key, writer in self.writer.items(): + frameset = self._buffer[self._next_ring_index] + assert frameset is not None + writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1]) self._next_ring_index = (self._next_ring_index + 1) % self.config.max_buffer_frames self._buffer_len = max(self._buffer_len + 1, self.config.max_buffer_frames) - if self._next_ring_index == 0: - # copy the buffer to the record path - for camera_key, writer in self.writer.items(): - for i in range(self.config.max_buffer_frames): - frameset = self._buffer[i] - assert frameset is not None - writer.write(frameset.frames[camera_key].camera.color.data[:, :, ::-1]) - sleep(1 / self.config.frame_rate) + self.rate(self.config.frame_rate) def poll_frame_set(self) -> FrameSet: """Gather frames over all available cameras.""" diff --git a/python/rcsss/camera/interface.py b/python/rcsss/camera/interface.py index f341ee69..e7ab9257 100644 --- a/python/rcsss/camera/interface.py +++ b/python/rcsss/camera/interface.py @@ -1,9 +1,40 @@ +import logging from dataclasses import dataclass from datetime import datetime +from time import sleep, time from typing import Any, Protocol from pydantic import BaseModel, Field +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + + +class SimpleFrameRate: + def __init__(self): + self.t = None + self._last_print = None + + def reset(self): + self.t = None + + def __call__(self, frame_rate: int | float): + if self.t is None: + self.t = time() + self._last_print = self.t + sleep(1 / frame_rate if isinstance(frame_rate, int) else frame_rate) + return + sleep_time = ( + 1 / frame_rate - (time() - self.t) if isinstance(frame_rate, int) else frame_rate - (time() - self.t) + ) + if sleep_time > 0: + sleep(sleep_time) + if self._last_print is None or time() - self._last_print > 10: + self._last_print = time() + logger.info(f"FPS: {1 / (time() - self.t)}") + + self.t = time() + class BaseCameraConfig(BaseModel): identifier: str diff --git a/python/rcsss/cli/main.py b/python/rcsss/cli/main.py index fa467625..edb5eb8a 100644 --- a/python/rcsss/cli/main.py +++ b/python/rcsss/cli/main.py @@ -1,6 +1,5 @@ import logging from contextlib import ExitStack -from pathlib import Path from time import sleep from typing import Annotated, Optional @@ -8,9 +7,7 @@ import rcsss import rcsss.control.fr3_desk import typer -from PIL import Image from rcsss.camera.realsense import RealSenseCameraSet -from rcsss.config import create_sample_config_yaml, read_config_yaml from rcsss.control.record import PoseList from rcsss.control.utils import load_creds_fr3_desk from rcsss.envs.factories import get_urdf_path @@ -21,16 +18,6 @@ main_app = typer.Typer(help="CLI tool for the Robot Control Stack (RCS).") -@main_app.command() -def sample_config( - path: Annotated[ - str, typer.Option("-p", help="Path to where the default config file should be saved") - ] = "config.yaml", -): - """Creates a sample yaml config file""" - create_sample_config_yaml(path) - - # REALSENSE CLI realsense_app = typer.Typer() main_app.add_typer( @@ -53,52 +40,6 @@ def serials(): logger.info(" %s: %s", device.product_line, device.serial) -@realsense_app.command() -def test( - path: Annotated[str, typer.Argument(help="Path to the config file")], -): - """Tests all configured and connected realsense by saving the current picture.""" - cfg = read_config_yaml(path) - assert cfg.hw.camera_type == "realsense", "Only realsense cameras are supported for this test." - assert cfg.hw.camera_config is not None, "Camera config is not set." - assert cfg.hw.camera_config.realsense_config is not None, "Realsense config is not set." - cs = RealSenseCameraSet(cfg.hw.camera_config.realsense_config) - cs.warm_up() - testdir = Path(cfg.hw.camera_config.realsense_config.record_path) - frame_set = cs.poll_frame_set() - for device_str in cfg.hw.camera_config.realsense_config.name_to_identifier: - assert device_str in frame_set.frames - frame = frame_set.frames[device_str] - if frame.camera.color is not None: - im = Image.fromarray(frame.camera.color.data, mode="RGB") - im.save(testdir / f"test_img_{device_str}.png") - if frame.camera.depth is not None: - im = Image.fromarray(frame.camera.depth.data) - im.save(testdir / f"test_depth_{device_str}.png") - if frame.camera.ir is not None: - im = Image.fromarray(frame.camera.ir.data) - im.save(testdir / f"test_ir_{device_str}.png") - if frame.imu is not None: - logger.info("IMU data: %s %s", frame.imu.accel, frame.imu.gyro) - - -@realsense_app.command() -def test_record( - path: Annotated[str, typer.Argument(help="Path to the config file")], - n_frames: Annotated[int, typer.Argument(help="Name of the camera that should be tested")] = 100, -): - """Tests all configured and connected realsense by saving the current picture.""" - cfg = read_config_yaml(path) - assert cfg.hw.camera_type == "realsense", "Only realsense cameras are supported for this test." - assert cfg.hw.camera_config is not None, "Camera config is not set." - assert cfg.hw.camera_config.realsense_config is not None, "Realsense config is not set." - cs = RealSenseCameraSet(cfg.hw.camera_config.realsense_config) - cs.start(warm_up=True) - while cs.buffer_size() < n_frames: - sleep(1 / cfg.hw.camera_config.realsense_config.frame_rate) - cs.stop() - - # FR3 CLI fr3_app = typer.Typer() main_app.add_typer( diff --git a/python/rcsss/control/fr3_desk.py b/python/rcsss/control/fr3_desk.py index f35797d9..f2826ddd 100644 --- a/python/rcsss/control/fr3_desk.py +++ b/python/rcsss/control/fr3_desk.py @@ -3,6 +3,7 @@ import hashlib import json as json_module import logging +import os import ssl import threading import time @@ -30,9 +31,8 @@ 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() - config.speed_factor = 0.7 - config.controller = rcsss.hw.IKController.internal - config.guiding_mode_enabled = True + config.speed_factor = 0.2 + config.ik_solver = rcsss.hw.IKSolver.franka f.set_parameters(config) config_hand = rcsss.hw.FHConfig() g = rcsss.hw.FrankaHand(ip, config_hand) @@ -161,6 +161,11 @@ def __init__(self, hostname: str, username: str, password: str) -> None: "down": False, "up": False, } + # Create an SSLContext that doesn't verify certificates + self.ssl_context = ssl.create_default_context() + self.ssl_context.check_hostname = False # Disable hostname verification + self.ssl_context.verify_mode = ssl.CERT_NONE # Disable certificate verification + self.login() def __enter__(self) -> "Desk": @@ -293,6 +298,14 @@ def take_control(self, force: bool = False) -> bool: """ active = self._get_active_token() + # try to read token from cache file + token_path = os.path.expanduser("~/.cache/rcs_fr3_token") + if active.id != "" and self._token.id == "" and os.path.exists(token_path): + with open(token_path, "r") as f: + content = f.read() + content_splitted = content.split("/n") + self._token = Token(*content_splitted) + # we already have control if active.id != "" and self._token.id == active.id: _logger.info("Retaken control.") @@ -322,18 +335,21 @@ def take_control(self, force: bool = False) -> bool: f"wss://{self._hostname}/desk/api/navigation/events", server_hostname="robot.franka.de", additional_headers={"authorization": self._session.cookies.get("authorization")}, # type: ignore[arg-type] + ssl_context=self.ssl_context, ) as websocket: while True: event: dict = json_module.loads(websocket.recv(timeout)) - if event["circle"]: + if event.get("circle", False): break self._token = Token(str(response["id"]), self._username, response["token"]) + with open(token_path, "w") as f: + f.write("/n".join([self._token.id, self._token.owned_by, self._token.token])) _logger.info("Taken control.") return True def release_control(self) -> None: """ - Explicitly relinquish control of the Desk. This will allow + Explicitly relinquish cofilentrol of the Desk. This will allow other users to take control or transfer control to the next user if there is an active queue of control requests. """ diff --git a/python/rcsss/control/vive.py b/python/rcsss/control/vive.py deleted file mode 100644 index dbd3cc8e..00000000 --- a/python/rcsss/control/vive.py +++ /dev/null @@ -1,240 +0,0 @@ -import logging -import threading -import typing -from enum import IntFlag, auto -from socket import AF_INET, SOCK_DGRAM, socket -from struct import pack, unpack - -import numpy as np -from rcsss._core.common import RPY, Pose -from rcsss.envs.base import ( - ControlMode, - GripperDictType, - LimitedTQuartRelDictType, - RelativeActionSpace, - RelativeTo, - RobotInstance, -) -from rcsss.envs.factories import fr3_hw_env, fr3_sim_env -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_mujoco_cameraset_cfg, -) - -# import matplotlib.pyplot as plt - - -logger = logging.getLogger(__name__) - -EGO_LOCK = False -VIVE_HOST = "192.168.100.1" -VIVE_PORT = 54321 -INCLUDE_ROTATION = True -ROBOT_IP = "192.168.101.1" -ROBOT_INSTANCE = RobotInstance.HARDWARE - - -class Button(IntFlag): - L_TRIGGER = auto() - L_SQUEEZE = auto() - LT_CLICK = auto() - R_TRIGGER = auto() - R_SQUEEZE = auto() - RT_CLICK = auto() - - -class UDPViveActionServer(threading.Thread): - # seven doubles and one integer in network byte order - FMT = "!" + 7 * "d" + "i" - - # base transform from OpenXR coordinate system - transform_from_openxr = Pose(RPY(roll=0.5 * np.pi, yaw=np.pi)) - - def __init__( - self, host: str, port: int, env: RelativeActionSpace, trg_btn=Button.R_TRIGGER, grp_btn=Button.R_SQUEEZE - ): - super().__init__() - self._host: str = host - self._port: int = port - - self._resource_lock = threading.Lock() - self._env_lock = threading.Lock() - self._env = env - self._trg_btn = trg_btn - self._grp_btn = grp_btn - self._grp_pos = 1 - self._buttons = 0 - self._exit_requested = False - self._last_controller_pose = Pose() - self._offset_pose = Pose() - self._ego_lock = EGO_LOCK - self._ego_transform = Pose() - self._env.set_origin_to_current() - - def next_action(self) -> Pose: - transform = Pose( - translation=self._last_controller_pose.translation() - self._offset_pose.translation(), - quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), - ) - return ( - self._ego_transform - * UDPViveActionServer.transform_from_openxr - * transform - * UDPViveActionServer.transform_from_openxr.inverse() - * self._ego_transform.inverse() - ) - - def get_last_controller_pose(self) -> Pose: - return self._last_controller_pose - - def run(self): - warning_raised = False - - with socket(AF_INET, SOCK_DGRAM) as sock, socket(AF_INET, SOCK_DGRAM) as send_sock: - sock.settimeout(2) - sock.setblocking(False) - sock.bind((self._host, self._port)) - send_sock.connect(("127.0.0.1", self._port + 1)) - while not self._exit_requested: - try: - unpacked = unpack(UDPViveActionServer.FMT, sock.recv(7 * 8 + 4)) - if warning_raised: - logger.info("[UDP Server] connection reestablished") - warning_raised = False - except TimeoutError: - if not warning_raised: - logger.warning("[UDP server] socket timeout (0.1s), waiting for packets") - warning_raised = True - continue - with self._resource_lock: - last_controller_pose_raw = np.ctypeslib.as_array(unpacked[:7]) - last_controller_pose = Pose( - translation=last_controller_pose_raw[4:], - quaternion=last_controller_pose_raw[:4] if INCLUDE_ROTATION else np.array([0, 0, 0, 1]), - ) - if Button(int(unpacked[7])) & self._trg_btn and not Button(int(self._buttons)) & self._trg_btn: - # trigger just pressed (first data sample with button pressed - - # set forward direction based on current controller pose - if self._ego_lock: - x_axis = Pose(translation=np.array([1, 0, 0])) - x_axis_rot = ( - UDPViveActionServer.transform_from_openxr - * Pose(quaternion=last_controller_pose.rotation_q()) - * UDPViveActionServer.transform_from_openxr.inverse() - * x_axis - ) - - # Compute angle around z axis: https://stackoverflow.com/questions/21483999/using-atan2-to-find-angle-between-two-vectors - rot_z = np.arctan2(x_axis_rot.translation()[1], x_axis_rot.translation()[0]) - np.arctan2( - x_axis.translation()[1], x_axis.translation()[0] - ) - rot_z -= np.pi / 2 - - print(f"Angle: {rot_z*180/np.pi}") - self._ego_transform = Pose(RPY(yaw=-rot_z)) - else: - self._ego_transform = Pose() - - self._offset_pose = last_controller_pose - self._last_controller_pose = last_controller_pose - - elif not Button(int(unpacked[7])) & self._trg_btn and Button(int(self._buttons)) & self._trg_btn: - # released - with self._env_lock: - self._last_controller_pose = Pose() - self._offset_pose = Pose() - self._ego_transform = Pose() - self._env.set_origin_to_current() - - elif Button(int(unpacked[7])) & self._trg_btn: - # button is pressed - self._last_controller_pose = last_controller_pose - - # plot current offset with liveplot.py - transform = Pose( - translation=self._last_controller_pose.translation() - self._offset_pose.translation(), - quaternion=(self._last_controller_pose * self._offset_pose.inverse()).rotation_q(), - ) - offset = ( - self._ego_transform - * UDPViveActionServer.transform_from_openxr - * transform - * UDPViveActionServer.transform_from_openxr.inverse() - * self._ego_transform.inverse() - ) - send_sock.sendall(pack(UDPViveActionServer.FMT, *offset.rotation_q(), *offset.translation(), 0)) - - if Button(int(unpacked[7])) & self._grp_btn and not Button(int(self._buttons)) & self._grp_btn: - # just pressed - self._grp_pos = 0 - elif not Button(int(unpacked[7])) & self._grp_btn and Button(int(self._buttons)) & self._grp_btn: - # just released - self._grp_pos = 1 - - self._buttons = unpacked[7] - - def stop(self): - self._exit_requested = True - self.join() - - def __enter__(self): - self.start() - return self - - def __exit__(self, *_): - self.stop() - - def environment_step_loop(self): - while True: - with self._resource_lock: - displacement = self.next_action() - action = dict( - LimitedTQuartRelDictType(tquart=np.concatenate([displacement.translation(), displacement.rotation_q()])) - ) - action.update(GripperDictType(gripper=self._grp_pos)) - with self._env_lock: - self._env.step(action) - - -def main(): - # if ROBOT_INSTANCE == RobotInstance.HARDWARE: - # user, pw = load_creds_fr3_desk() - # resource_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False) - # else: - # resource_manger = DummyResourceManager() - - # with resource_manger: - - if ROBOT_INSTANCE == RobotInstance.HARDWARE: - env_rel = fr3_hw_env( - ip=ROBOT_IP, - control_mode=ControlMode.CARTESIAN_TQuart, - robot_cfg=default_fr3_hw_robot_cfg(), - collision_guard="lab", - gripper_cfg=default_fr3_hw_gripper_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - else: - env_rel = fr3_sim_env( - control_mode=ControlMode.CARTESIAN_TQuart, - # control_mode=ControlMode.JOINTS, - robot_cfg=default_fr3_sim_robot_cfg(), - gripper_cfg=default_fr3_sim_gripper_cfg(), - camera_set_cfg=default_mujoco_cameraset_cfg(), - max_relative_movement=0.5, - relative_to=RelativeTo.CONFIGURED_ORIGIN, - ) - - env_rel.reset() - - with UDPViveActionServer(VIVE_HOST, VIVE_PORT, typing.cast(RelativeActionSpace, env_rel)) as action_server: - action_server.environment_step_loop() - - -if __name__ == "__main__": - main() diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 1724998b..c38f7f6b 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -8,6 +8,7 @@ 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 ( ActObsInfoWrapper, @@ -18,7 +19,7 @@ get_space_keys, ) -_logger = logging.getLogger() +_logger = logging.getLogger(__name__) class TRPYDictType(RCSpaceType): @@ -159,7 +160,7 @@ class FR3Env(gym.Env): y """ - def __init__(self, robot: common.Robot, control_mode: ControlMode): + def __init__(self, robot: Robot, control_mode: ControlMode): self.robot = robot self._control_mode_overrides = [control_mode] self.action_space: gym.spaces.Dict @@ -222,7 +223,6 @@ def step(self, action: CartOrJointContType) -> tuple[ArmObsType, float, bool, bo self.prev_action is None or not np.allclose(action_dict[self.joints_key], self.prev_action[self.joints_key], atol=1e-03, rtol=0) ): - # cast is needed because typed dicts cannot be checked at runtime self.robot.set_joint_position(action_dict[self.joints_key]) elif self.get_base_control_mode() == ControlMode.CARTESIAN_TRPY and ( self.prev_action is None @@ -251,8 +251,12 @@ def reset( msg = "options not implemented yet" raise NotImplementedError(msg) self.robot.reset() + self.robot.move_home() return self.get_obs(), {} + def close(self): + super().close() + class RelativeTo(Enum): LAST_STEP = auto() @@ -329,6 +333,7 @@ def __init__( self.tquart_key = get_space_keys(LimitedTQuartRelDictType)[0] self.initial_obs: dict[str, Any] | None = None self._origin: common.Pose | Vec7Type | None = None + self._last_action: common.Pose | Vec7Type | None = None def set_origin(self, origin: common.Pose | Vec7Type): if self.unwrapped.get_control_mode() == ControlMode.JOINTS: @@ -352,11 +357,13 @@ def reset(self, **kwargs) -> tuple[dict, dict[str, Any]]: obs, info = super().reset(**kwargs) self.initial_obs = obs self.set_origin_to_current() + self._last_action = None return obs, info def action(self, action: dict[str, Any]) -> dict[str, Any]: if self.relative_to == RelativeTo.LAST_STEP: # TODO: should we use the last observation instead? + # -> could be done after the step to the state that is returned by the observation self.set_origin_to_current() action = copy.deepcopy(action) if self.unwrapped.get_control_mode() == ControlMode.JOINTS and self.joints_key in action: @@ -364,7 +371,14 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: assert isinstance(self.max_mov, float) joint_space = cast(gym.spaces.Box, get_space(JointsDictType).spaces[self.joints_key]) # TODO: should we also clip euqally for all joints? - limited_joints = np.clip(action[self.joints_key], -self.max_mov, self.max_mov) + 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) + self._last_action = limited_joints + else: + joints_diff = action[self.joints_key] - self._last_action + 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)) ) @@ -374,14 +388,30 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: assert isinstance(self.max_mov, tuple) pose_space = cast(gym.spaces.Box, get_space(TRPYDictType).spaces[self.trpy_key]) - clipped_pose_offset = ( - common.Pose( - translation=action[self.trpy_key][:3], - rpy_vector=action[self.trpy_key][3:], + if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None: + clipped_pose_offset = ( + common.Pose( + translation=action[self.trpy_key][:3], + rpy_vector=action[self.trpy_key][3:], + ) + .limit_translation_length(self.max_mov[0]) + .limit_rotation_angle(self.max_mov[1]) ) - .limit_translation_length(self.max_mov[0]) - .limit_rotation_angle(self.max_mov[1]) - ) + self._last_action = clipped_pose_offset + else: + assert isinstance(self._last_action, common.Pose) + pose_diff = ( + common.Pose( + translation=action[self.trpy_key][:3], + rpy_vector=action[self.trpy_key][3:], + ) + * self._last_action.inverse() + ) + clipped_pose_diff = pose_diff.limit_translation_length(self.max_mov[0]).limit_rotation_angle( + self.max_mov[1] + ) + clipped_pose_offset = clipped_pose_diff * self._last_action + self._last_action = clipped_pose_offset unclipped_pose = common.Pose( translation=self._origin.translation() + clipped_pose_offset.translation(), @@ -402,14 +432,30 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: assert isinstance(self.max_mov, tuple) pose_space = cast(gym.spaces.Box, get_space(TQuartDictType).spaces[self.tquart_key]) - clipped_pose_offset = ( - common.Pose( - translation=action[self.tquart_key][:3], - quaternion=action[self.tquart_key][3:], + 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:], + ) + .limit_translation_length(self.max_mov[0]) + .limit_rotation_angle(self.max_mov[1]) ) - .limit_translation_length(self.max_mov[0]) - .limit_rotation_angle(self.max_mov[1]) - ) + self._last_action = clipped_pose_offset + else: + assert isinstance(self._last_action, common.Pose) + pose_diff = ( + common.Pose( + translation=action[self.tquart_key][:3], + quaternion=action[self.tquart_key][3:], + ) + * self._last_action.inverse() + ) + clipped_pose_diff = pose_diff.limit_translation_length(self.max_mov[0]).limit_rotation_angle( + self.max_mov[1] + ) + clipped_pose_offset = clipped_pose_diff * self._last_action + self._last_action = clipped_pose_offset unclipped_pose = common.Pose( translation=self._origin.translation() + clipped_pose_offset.translation(), @@ -569,11 +615,10 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]: gripper_action = np.round(action[self.gripper_key]) if self.binary else action[self.gripper_key] gripper_action = np.clip(gripper_action, 0.0, 1.0) - if self._last_gripper_cmd is None or self._last_gripper_cmd != gripper_action: - if self.binary: - self._gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self._gripper.open() - else: - self._gripper.set_normalized_width(gripper_action) + if self.binary: + self._gripper.grasp() if gripper_action == self.BINARY_GRIPPER_CLOSED else self._gripper.open() + else: + self._gripper.set_normalized_width(gripper_action) self._last_gripper_cmd = gripper_action del action[self.gripper_key] return action diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 95c413fa..90a63b41 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -248,8 +248,8 @@ def __call__( # type: ignore resolution = (256, 256) cameras = { - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), - "bird_eye": SimCameraConfig(identifier="bird-eye-cam", type=int(CameraType.fixed)), + "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)), @@ -278,7 +278,7 @@ def __call__( # type: ignore if resolution is None: resolution = (256, 256) - cameras = {"wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed))} + cameras = {"wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed))} camera_cfg = SimCameraSetConfig( cameras=cameras, @@ -326,8 +326,8 @@ def __call__( # type: ignore resolution = (256, 256) cameras = { - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), - "side": SimCameraConfig(identifier="eye-in-hand_1", type=int(CameraType.fixed)), + "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), + "side": SimCameraConfig(identifier="wrist_1", type=int(CameraType.fixed)), } camera_cfg = SimCameraSetConfig( diff --git a/python/rcsss/envs/hw.py b/python/rcsss/envs/hw.py index 5ce0f755..d55f00db 100644 --- a/python/rcsss/envs/hw.py +++ b/python/rcsss/envs/hw.py @@ -29,3 +29,7 @@ def reset( self, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: return super().reset(seed=seed, options=options) + + def close(self): + self.hw_robot.stop_control_thread() + super().close() diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index 3ac7f498..3112ae7e 100644 --- a/python/rcsss/envs/sim.py +++ b/python/rcsss/envs/sim.py @@ -192,7 +192,7 @@ def reset( pos_x = iso_cube[0] + np.random.random() * 0.2 - 0.1 pos_y = iso_cube[1] + np.random.random() * 0.2 - 0.1 - self.sim.data.joint("yellow-box-joint").qpos[:3] = [pos_x, pos_y, pos_z] + self.sim.data.joint("box_joint").qpos[:3] = [pos_x, pos_y, pos_z] return obs, info @@ -212,14 +212,12 @@ def step(self, action: dict[str, Any]): obs, reward, done, truncated, info = super().step(action) success = ( - self.sim.data.joint("yellow-box-joint").qpos[2] > 0.3 - and obs["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED + self.sim.data.joint("box_joint").qpos[2] > 0.3 and obs["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED ) diff_ee_cube = np.linalg.norm( - self.sim.data.joint("yellow-box-joint").qpos[:3] - - self.unwrapped.robot.get_cartesian_position().translation() + self.sim.data.joint("box_joint").qpos[:3] - self.unwrapped.robot.get_cartesian_position().translation() ) - diff_cube_home = np.linalg.norm(self.sim.data.joint("yellow-box-joint").qpos[:3] - self.EE_HOME) + diff_cube_home = np.linalg.norm(self.sim.data.joint("box_joint").qpos[:3] - self.EE_HOME) reward = -diff_cube_home - diff_ee_cube return obs, reward, success, truncated, info diff --git a/python/rcsss/envs/utils.py b/python/rcsss/envs/utils.py index 9b9590e2..19bba80f 100644 --- a/python/rcsss/envs/utils.py +++ b/python/rcsss/envs/utils.py @@ -6,7 +6,7 @@ import numpy as np import rcsss from rcsss import sim -from rcsss._core.hw import FR3Config, IKController +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 @@ -23,19 +23,21 @@ def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.FR3Co return cfg -def default_fr3_hw_robot_cfg(): +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.speed_factor = 0.1 - robot_cfg.controller = IKController.robotics_library + robot_cfg.ik_solver = IKSolver.rcs + robot_cfg.async_control = async_control return robot_cfg -def default_fr3_hw_gripper_cfg(): +def default_fr3_hw_gripper_cfg(async_control: bool = False): gripper_cfg = rcsss.hw.FHConfig() gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 gripper_cfg.force = 30 + gripper_cfg.async_control = async_control return gripper_cfg @@ -61,9 +63,9 @@ def default_fr3_sim_gripper_cfg(): def default_mujoco_cameraset_cfg(): cameras = { - "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)), + "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), - # "bird_eye": SimCameraConfig(identifier="bird-eye-cam", type=int(CameraType.fixed)), + # "bird_eye": SimCameraConfig(identifier="bird_eye_cam", type=int(CameraType.fixed)), } # 256x256 needed for VLAs return SimCameraSetConfig( diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index 83baa811..9806d9ec 100644 --- a/python/tests/test_sim_envs.py +++ b/python/tests/test_sim_envs.py @@ -108,10 +108,10 @@ def test_non_zero_action_trpy(self, cfg): 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, cam_cfg): + def test_relative_zero_action_trpy(self, cfg, gripper_cfg): # env creation env = fr3_sim_env( - ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5 + ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() # action to be performed @@ -120,10 +120,10 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg): obs, _, _, _, info = env.step(zero_action) self.assert_no_pose_change(info, obs_initial, obs) - def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): + def test_relative_non_zero_action(self, cfg, gripper_cfg): # env creation env = fr3_sim_env( - ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5 + ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() # action to be performed @@ -135,13 +135,13 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg): obs, _, _, _, info = env.step(non_zero_action) self.assert_no_pose_change(info, obs_initial, expected_obs) - def test_collision_trpy(self, cfg, gripper_cfg, cam_cfg): + def test_collision_trpy(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by sim """ # env creation env = fr3_sim_env( - ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None + ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None ) obs, _ = env.reset() # an obvious below ground collision action @@ -152,7 +152,7 @@ def test_collision_trpy(self, cfg, gripper_cfg, cam_cfg): obs, _, _, _, info = env.step(collision_action) self.assert_collision(info) - def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): + def test_collision_guard_trpy(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by the CollisionGuard """ @@ -162,7 +162,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg): cfg, gripper_cfg=gripper_cfg, collision_guard=True, - camera_set_cfg=cam_cfg, + camera_set_cfg=None, max_relative_movement=None, ) obs, _ = env.reset() @@ -236,13 +236,13 @@ def test_zero_action_tquart(self, cfg): 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, cam_cfg): + def test_relative_zero_action_tquart(self, cfg, gripper_cfg): # env creation env_rel = fr3_sim_env( ControlMode.CARTESIAN_TQuart, cfg, gripper_cfg=gripper_cfg, - camera_set_cfg=cam_cfg, + camera_set_cfg=None, max_relative_movement=0.5, ) obs_initial, _ = env_rel.reset() @@ -251,7 +251,7 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg): 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, cam_cfg): + def test_collision_tquart(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by sim """ @@ -260,7 +260,7 @@ def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg): ControlMode.CARTESIAN_TQuart, cfg, gripper_cfg=gripper_cfg, - camera_set_cfg=cam_cfg, + camera_set_cfg=None, max_relative_movement=None, ) obs, _ = env.reset() @@ -272,7 +272,7 @@ def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg): _, _, _, _, info = env.step(collision_action) self.assert_collision(info) - def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): + def test_collision_guard_tquart(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by the CollisionGuard """ @@ -282,7 +282,7 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg): cfg, gripper_cfg=gripper_cfg, collision_guard=True, - camera_set_cfg=cam_cfg, + camera_set_cfg=None, max_relative_movement=None, ) obs, _ = env.reset() @@ -346,13 +346,13 @@ def test_non_zero_action_joints(self, cfg): assert info["is_sim_converged"] assert np.allclose(obs["joints"], non_zero_action["joints"], atol=0.01, rtol=0) - def test_collision_joints(self, cfg, gripper_cfg, cam_cfg): + def test_collision_joints(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by the CollisionGuard """ # env creation env = fr3_sim_env( - ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None + ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None ) env.reset() # the below action is a test_case where there is an obvious collision regardless of the gripper action @@ -361,7 +361,7 @@ def test_collision_joints(self, cfg, gripper_cfg, cam_cfg): _, _, _, _, info = env.step(collision_act) self.assert_collision(info) - def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): + def test_collision_guard_joints(self, cfg, gripper_cfg): """ Check that an obvious collision is detected by sim """ @@ -371,7 +371,7 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): cfg, gripper_cfg=gripper_cfg, collision_guard=True, - camera_set_cfg=cam_cfg, + camera_set_cfg=None, max_relative_movement=None, ) env.reset() @@ -387,13 +387,13 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg): # assure that the robot did not move assert np.allclose(p1, p2) - def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg): + 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( - ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5 + ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5 ) obs_initial, _ = env.reset() act = JointsDictType(joints=np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32)) diff --git a/src/common/LinearPoseTrajInterpolator.h b/src/common/LinearPoseTrajInterpolator.h new file mode 100644 index 00000000..65a0b848 --- /dev/null +++ b/src/common/LinearPoseTrajInterpolator.h @@ -0,0 +1,175 @@ +#ifndef DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ +#define DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ +#include + +namespace rcs { +namespace common { + +class LinearPoseTrajInterpolator { + private: + Eigen::Vector3d p_start_; + Eigen::Vector3d p_goal_; + Eigen::Vector3d last_p_t_; + Eigen::Vector3d prev_p_goal_; + + Eigen::Quaterniond q_start_; + Eigen::Quaterniond q_goal_; + Eigen::Quaterniond last_q_t_; + Eigen::Quaterniond prev_q_goal_; + + double dt_; + double last_time_; + double max_time_; + double start_time_; + bool start_; + bool first_goal_; + + public: + inline LinearPoseTrajInterpolator() + : dt_(0.), + last_time_(0.), + max_time_(1.), + start_time_(0.), + start_(false), + first_goal_(true){}; + + inline ~LinearPoseTrajInterpolator(){}; + + 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, + const int &rate, + const double &traj_interpolator_time_fraction) { + dt_ = 1. / static_cast(rate); + last_time_ = time_sec; + max_time_ = + 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + start_time_ = time_sec; + + start_ = false; + + if (first_goal_) { + p_start_ = p_start; + q_start_ = q_start; + + prev_p_goal_ = p_start; + prev_q_goal_ = q_start; + first_goal_ = false; + } else { + // If the goal is already set, use prev goal as the starting point of + // interpolation. + prev_p_goal_ = p_goal_; + prev_q_goal_ = q_goal_; + + p_start_ = prev_p_goal_; + q_start_ = prev_q_goal_; + } + + p_goal_ = p_goal; + q_goal_ = q_goal; + + // Flip the sign if the dot product of quaternions is negative + if (q_goal_.coeffs().dot(q_start_.coeffs()) < 0.0) { + q_start_.coeffs() << -q_start_.coeffs(); + } + }; + + inline void GetNextStep(const double &time_sec, Eigen::Vector3d &p_t, + Eigen::Quaterniond &q_t) { + if (!start_) { + start_time_ = time_sec; + last_p_t_ = p_start_; + last_q_t_ = q_start_; + start_ = true; + } + + if (last_time_ + dt_ <= time_sec) { + double t = + std::min(std::max((time_sec - start_time_) / max_time_, 0.), 1.); + last_p_t_ = p_start_ + t * (p_goal_ - p_start_); + last_q_t_ = q_start_.slerp(t, q_goal_); + last_time_ = time_sec; + } + p_t = last_p_t_; + q_t = last_q_t_; + }; +}; +class LinearJointPositionTrajInterpolator { + private: + using Vector7d = Eigen::Matrix; + using Vector7i = Eigen::Matrix; + + Vector7d q_start_; + Vector7d q_goal_; + + Vector7d last_q_t_; + Vector7d prev_q_goal_; + + double dt_; + double last_time_; + double max_time_; + double start_time_; + bool start_; + bool first_goal_; + + double interpolation_fraction_; // fraction of actual interpolation within an + // interval + + public: + inline LinearJointPositionTrajInterpolator() + : dt_(0.), + last_time_(0.), + max_time_(1.), + start_time_(0.), + start_(false), + first_goal_(true){}; + + inline ~LinearJointPositionTrajInterpolator(){}; + + inline void Reset(const double &time_sec, + const Eigen::Matrix &q_start, + const Eigen::Matrix &q_goal, + const int &policy_rate, const int &rate, + const double &traj_interpolator_time_fraction) { + dt_ = 1. / static_cast(rate); + last_time_ = time_sec; + + max_time_ = + 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + start_time_ = time_sec; + + start_ = false; + + if (first_goal_) { + q_start_ = q_start; + prev_q_goal_ = q_start; + first_goal_ = false; + // std::cout << "First goal of the interpolation" << std::endl; + } else { + prev_q_goal_ = q_goal_; + q_start_ = prev_q_goal_; + } + q_goal_ = q_goal; + }; + + inline void GetNextStep(const double &time_sec, Vector7d &q_t) { + if (!start_) { + start_time_ = time_sec; + last_q_t_ = q_start_; + start_ = true; + } + // std::cout << q_start_.transpose() << " | " << q_goal_.transpose() << + // std::endl; + if (last_time_ + dt_ <= time_sec) { + double t = + std::min(std::max((time_sec - start_time_) / max_time_, 0.), 1.); + last_q_t_ = q_start_ + t * (q_goal_ - q_start_); + last_time_ = time_sec; + } + q_t = last_q_t_; + }; +}; +} // namespace common +} // namespace rcs +#endif // DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_ diff --git a/src/hw/FR3.cpp b/src/hw/FR3.cpp index d746c7a7..de061021 100644 --- a/src/hw/FR3.cpp +++ b/src/hw/FR3.cpp @@ -1,5 +1,9 @@ #include "FR3.h" +#include +#include +#include +#include #include #include @@ -20,7 +24,7 @@ FR3::FR3(const std::string &ip, std::optional> ik, : robot(ip), m_ik(ik) { // set collision behavior and impedance this->set_default_robot_behavior(); - this->set_guiding_mode(true); + this->set_guiding_mode(true, true, true, true, true, true, true); if (cfg.has_value()) { this->cfg = cfg.value(); @@ -82,37 +86,514 @@ void FR3::set_default_robot_behavior() { } common::Pose FR3::get_cartesian_position() { - franka::RobotState state = this->robot.readOnce(); - common::Pose x(state.O_T_EE); + common::Pose x; + if (this->running_controller == Controller::none) { + this->curr_state = this->robot.readOnce(); + x = common::Pose(this->curr_state.O_T_EE); + } else { + this->interpolator_mutex.lock(); + x = common::Pose(this->curr_state.O_T_EE); + this->interpolator_mutex.unlock(); + } return x; } void FR3::set_joint_position(const common::Vector7d &q) { - // TODO: max force? + if (this->cfg.async_control) { + this->controller_set_joint_position(q); + return; + } + // sync control MotionGenerator motion_generator(this->cfg.speed_factor, q); this->robot.control(motion_generator); } common::Vector7d FR3::get_joint_position() { - franka::RobotState state = this->robot.readOnce(); - common::Vector7d joints(state.q.data()); + common::Vector7d joints; + if (this->running_controller == Controller::none) { + this->curr_state = this->robot.readOnce(); + joints = common::Vector7d(this->curr_state.q.data()); + } else { + this->interpolator_mutex.lock(); + joints = common::Vector7d(this->curr_state.q.data()); + this->interpolator_mutex.unlock(); + } return joints; } -void FR3::set_guiding_mode(bool enabled) { - std::array activated; - activated.fill(enabled); - this->robot.setGuidingMode(activated, enabled); - this->cfg.guiding_mode_enabled = enabled; +void FR3::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, + bool yaw, bool elbow) { + std::array activated = {x, y, z, roll, pitch, yaw}; + this->robot.setGuidingMode(activated, elbow); +} + +void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv, + double epsilon = 0.00025) { + Eigen::JacobiSVD svd( + M, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::JacobiSVD::SingularValuesType singular_vals = + svd.singularValues(); + + Eigen::MatrixXd S_inv = M; + S_inv.setZero(); + for (int i = 0; i < singular_vals.size(); i++) { + if (singular_vals(i) < epsilon) { + S_inv(i, i) = 0.; + } else { + S_inv(i, i) = 1. / singular_vals(i); + } + } + M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose()); +} + +void TorqueSafetyGuardFn(std::array &tau_d_array, double min_torque, + double max_torque) { + for (size_t i = 0; i < tau_d_array.size(); i++) { + if (tau_d_array[i] < min_torque) { + tau_d_array[i] = min_torque; + } else if (tau_d_array[i] > max_torque) { + tau_d_array[i] = max_torque; + } + } +} + +void FR3::controller_set_joint_position(const common::Vector7d &desired_q) { + // from deoxys/config/osc-position-controller.yml + double traj_interpolation_time_fraction = 1.0; // in s + // form deoxys/config/charmander.yml + int policy_rate = 20; + int traj_rate = 500; + + if (this->running_controller == Controller::none) { + this->controller_time = 0.0; + this->get_joint_position(); + } else if (this->running_controller != Controller::jsc) { + // runtime error + throw std::runtime_error( + "Controller type must but joint space but is " + + std::to_string(this->running_controller) + + ". To change controller type stop the current controller first."); + } else { + this->interpolator_mutex.lock(); + } + + this->joint_interpolator.Reset( + this->controller_time, + Eigen::Map(this->curr_state.q.data()), desired_q, + policy_rate, traj_rate, traj_interpolation_time_fraction); + + // if not thread is running, then start + if (this->running_controller == Controller::none) { + this->running_controller = Controller::jsc; + this->control_thread = std::thread(&FR3::joint_controller, this); + } else { + this->interpolator_mutex.unlock(); + } +} + +void FR3::osc_set_cartesian_position( + const common::Pose &desired_pose_EE_in_base_frame) { + // from deoxys/config/osc-position-controller.yml + double traj_interpolation_time_fraction = 1.0; + // form deoxys/config/charmander.yml + int policy_rate = 20; + int traj_rate = 500; + + if (this->running_controller == Controller::none) { + this->controller_time = 0.0; + this->get_cartesian_position(); + } else if (this->running_controller != Controller::osc) { + throw std::runtime_error( + "Controller type must but osc but is " + + std::to_string(this->running_controller) + + ". To change controller type stop the current controller first."); + } else { + this->interpolator_mutex.lock(); + } + + common::Pose curr_pose(this->curr_state.O_T_EE); + 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, + traj_interpolation_time_fraction); + + // if not thread is running, then start + if (this->running_controller == Controller::none) { + this->running_controller = Controller::osc; + this->control_thread = std::thread(&FR3::osc, this); + } else { + this->interpolator_mutex.unlock(); + } +} + +// method to stop thread +void FR3::stop_control_thread() { + if (this->control_thread.has_value() && + this->running_controller != Controller::none) { + this->running_controller = Controller::none; + this->control_thread->join(); + this->control_thread.reset(); + } } -void FR3::move_home() { this->set_joint_position(q_home); } +void FR3::osc() { + franka::Model model = this->robot.loadModel(); + + this->controller_time = 0.0; + + // conservative collision and impedance behavior + this->set_default_robot_behavior(); + + // high collision threshold values for high impedance + this->robot.setCollisionBehavior( + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); + + // from bench mark + // ([150.0, 150.0, 60.0], 250.0), // kp_translation, kp_rotation + // ([60.0, 150.0, 150.0], 250.0), // kd_translation, kd_rotation + + // from config file + // Kp: + // translation: [150.0, 150.0, 150.0] + // rotation: 250.0 + + Eigen::Matrix Kp_p, Kp_r, Kd_p, Kd_r; + Eigen::Matrix static_q_task_; + Eigen::Matrix residual_mass_vec_; + Eigen::Array joint_max_; + Eigen::Array joint_min_; + Eigen::Array avoidance_weights_; + + // values from deoxys/config/osc-position-controller.yml + Kp_p.diagonal() << 150, 150, 150; + Kp_r.diagonal() << 250, 250, 250; + + Kd_p << Kp_p.cwiseSqrt() * 2.0; + Kd_r << Kp_r.cwiseSqrt() * 2.0; + + static_q_task_ << 0.09017809387254755, -0.9824203501652151, + 0.030509718397568178, -2.694229634937343, 0.057700675144720104, + 1.860298714876101, 0.8713759453244422; + + // The manual residual mass matrix to add on the internal mass matrix + residual_mass_vec_ << 0.0, 0.0, 0.0, 0.0, 0.1, 0.5, 0.5; + + joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973; + joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973; + avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.; + + this->robot.control([&](const franka::RobotState &robot_state, + franka::Duration period) -> franka::Torques { + std::chrono::high_resolution_clock::time_point t1 = + std::chrono::high_resolution_clock::now(); + + // torques handler + if (this->running_controller == Controller::none) { + franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + return franka::MotionFinished(zero_torques); + } + // TO BE replaced + // if (!this->control_thread_running && dq.maxCoeff() < 0.0001) { + // return franka::MotionFinished(franka::Torques(tau_d_array)); + // } + + Eigen::Vector3d desired_pos_EE_in_base_frame; + Eigen::Quaterniond desired_quat_EE_in_base_frame; + + common::Pose pose(robot_state.O_T_EE); + // form deoxys/config/charmander.yml + int policy_rate = 20; + 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->interpolator_mutex.unlock(); + + // end torques handler + + Eigen::Matrix tau_d; + + std::array mass_array = model.mass(robot_state); + Eigen::Map> M(mass_array.data()); + + M = M + Eigen::Matrix(residual_mass_vec_.asDiagonal()); + + // coriolis and gravity + std::array coriolis_array = model.coriolis(robot_state); + Eigen::Map> coriolis( + coriolis_array.data()); + + std::array gravity_array = model.gravity(robot_state); + Eigen::Map> gravity(gravity_array.data()); + + std::array jacobian_array = + model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + Eigen::Map> jacobian( + jacobian_array.data()); + + Eigen::MatrixXd jacobian_pos(3, 7); + Eigen::MatrixXd jacobian_ori(3, 7); + jacobian_pos << jacobian.block(0, 0, 3, 7); + jacobian_ori << jacobian.block(3, 0, 3, 7); + + // End effector pose in base frame + Eigen::Affine3d T_EE_in_base_frame( + Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); + Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation()); + Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear()); + + // Nullspace goal + Eigen::Map> q(robot_state.q.data()); + + // Joint velocity + Eigen::Map> dq(robot_state.dq.data()); + + if (desired_quat_EE_in_base_frame.coeffs().dot( + quat_EE_in_base_frame.coeffs()) < 0.0) { + quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs(); + } + + Eigen::Vector3d pos_error; + + pos_error << desired_pos_EE_in_base_frame - pos_EE_in_base_frame; + Eigen::Quaterniond quat_error(desired_quat_EE_in_base_frame.inverse() * + quat_EE_in_base_frame); + Eigen::Vector3d ori_error; + ori_error << quat_error.x(), quat_error.y(), quat_error.z(); + ori_error << -T_EE_in_base_frame.linear() * ori_error; + + // Compute matrices + Eigen::Matrix M_inv(M.inverse()); + Eigen::MatrixXd Lambda_inv(6, 6); + Lambda_inv << jacobian * M_inv * jacobian.transpose(); + Eigen::MatrixXd Lambda(6, 6); + PInverse(Lambda_inv, Lambda); + + Eigen::Matrix J_inv; + J_inv << M_inv * jacobian.transpose() * Lambda; + Eigen::Matrix Nullspace; + Nullspace << Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose() * J_inv.transpose(); + + // Decoupled mass matrices + Eigen::MatrixXd Lambda_pos_inv(3, 3); + Lambda_pos_inv << jacobian_pos * M_inv * jacobian_pos.transpose(); + Eigen::MatrixXd Lambda_ori_inv(3, 3); + Lambda_ori_inv << jacobian_ori * M_inv * jacobian_ori.transpose(); + + Eigen::MatrixXd Lambda_pos(3, 3); + Eigen::MatrixXd Lambda_ori(3, 3); + PInverse(Lambda_pos_inv, Lambda_pos); + PInverse(Lambda_ori_inv, Lambda_ori); + + pos_error = + pos_error.unaryExpr([](double x) { return (abs(x) < 1e-4) ? 0. : x; }); + ori_error = + ori_error.unaryExpr([](double x) { return (abs(x) < 5e-3) ? 0. : x; }); + + tau_d << jacobian_pos.transpose() * + (Lambda_pos * + (Kp_p * pos_error - Kd_p * (jacobian_pos * dq))) + + jacobian_ori.transpose() * + (Lambda_ori * + (Kp_r * ori_error - Kd_r * (jacobian_ori * dq))); + + // nullspace control + tau_d << tau_d + Nullspace * (static_q_task_ - q); + + // Add joint avoidance potential + Eigen::Matrix avoidance_force; + avoidance_force.setZero(); + Eigen::Matrix dist2joint_max; + Eigen::Matrix dist2joint_min; + + dist2joint_max = joint_max_.matrix() - q; + dist2joint_min = q - joint_min_.matrix(); + + for (int i = 0; i < 7; i++) { + if (dist2joint_max[i] < 0.25 && dist2joint_max[i] > 0.1) + avoidance_force[i] += -avoidance_weights_[i] * dist2joint_max[i]; + if (dist2joint_min[i] < 0.25 && dist2joint_min[i] > 0.1) + avoidance_force[i] += avoidance_weights_[i] * dist2joint_min[i]; + } + tau_d << tau_d + Nullspace * avoidance_force; + for (int i = 0; i < 7; i++) { + if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; + if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; + } + + std::array tau_d_array{}; + Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; + + // end of controller + std::chrono::high_resolution_clock::time_point t2 = + std::chrono::high_resolution_clock::now(); + auto time = std::chrono::duration_cast(t2 - t1); + + std::array tau_d_rate_limited = franka::limitRate( + franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); + + // deoxys/config/control_config.yml + double min_torque = -5; + double max_torque = 5; + TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); + + return tau_d_rate_limited; + }); +} + +void FR3::joint_controller() { + franka::Model model = this->robot.loadModel(); + this->controller_time = 0.0; + + // conservative collision and impedance behavior + this->set_default_robot_behavior(); + + // high collision threshold values for high impedance + // this->robot.setCollisionBehavior( + // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); + + // deoxys/config/joint-impedance-controller.yml + common::Vector7d Kp; + Kp << 100., 100., 100., 100., 75., 150., 50.; + + common::Vector7d Kd; + Kd << 20., 20., 20., 20., 7.5, 15.0, 5.0; + + Eigen::Array joint_max_; + Eigen::Array joint_min_; + + joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973; + joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973; + + this->robot.control([&](const franka::RobotState &robot_state, + franka::Duration period) -> franka::Torques { + std::chrono::high_resolution_clock::time_point t1 = + std::chrono::high_resolution_clock::now(); + + // torques handler + if (this->running_controller == Controller::none) { + // TODO: test if this also works when the robot is moving + franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + return franka::MotionFinished(zero_torques); + } + + common::Vector7d desired_q; + common::Pose pose(robot_state.O_T_EE); + + this->interpolator_mutex.lock(); + this->curr_state = robot_state; + this->controller_time += period.toSec(); + this->joint_interpolator.GetNextStep(this->controller_time, desired_q); + this->interpolator_mutex.unlock(); + // end torques handler + + Eigen::Matrix tau_d; + + // Current joint velocity + Eigen::Map> dq(robot_state.dq.data()); + + // Current joint position + Eigen::Map> q(robot_state.q.data()); + + Eigen::MatrixXd joint_pos_error(7, 1); + + joint_pos_error << desired_q - q; + + tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq); + + Eigen::Matrix dist2joint_max; + Eigen::Matrix dist2joint_min; + + dist2joint_max = joint_max_.matrix() - q; + dist2joint_min = q - joint_min_.matrix(); + + for (int i = 0; i < 7; i++) { + if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; + if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; + } + + std::array tau_d_array{}; + Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; + + // end of controller + std::chrono::high_resolution_clock::time_point t2 = + std::chrono::high_resolution_clock::now(); + auto time = std::chrono::duration_cast(t2 - t1); + + std::array tau_d_rate_limited = franka::limitRate( + franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); + + // deoxys/config/control_config.yml + double min_torque = -5; + double max_torque = 5; + TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); + + return tau_d_rate_limited; + }); +} + +void FR3::zero_torque_guiding() { + if (this->running_controller != Controller::none) { + throw std::runtime_error( + "A controller is currently running. Please stop it first."); + } + this->controller_time = 0.0; + this->running_controller = Controller::ztc; + this->control_thread = std::thread(&FR3::zero_torque_controller, this); +} + +void FR3::zero_torque_controller() { + // high collision threshold values for high impedance + robot.setCollisionBehavior( + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); + + this->controller_time = 0.0; + this->robot.control([&](const franka::RobotState &robot_state, + franka::Duration period) -> franka::Torques { + this->interpolator_mutex.lock(); + this->curr_state = robot_state; + this->controller_time += period.toSec(); + this->interpolator_mutex.unlock(); + if (this->running_controller == Controller::none) { + // stop + return franka::MotionFinished(franka::Torques({0, 0, 0, 0, 0, 0, 0})); + } + return franka::Torques({0, 0, 0, 0, 0, 0, 0}); + }); +} + +void FR3::move_home() { + // sync + MotionGenerator motion_generator(this->cfg.speed_factor, q_home); + this->robot.control(motion_generator); +} void FR3::automatic_error_recovery() { this->robot.automaticErrorRecovery(); } void FR3::reset() { + this->stop_control_thread(); this->automatic_error_recovery(); - this->move_home(); } void FR3::wait_milliseconds(int milliseconds) { @@ -177,6 +658,11 @@ std::optional> FR3::get_ik() { return this->m_ik; } void FR3::set_cartesian_position(const common::Pose &x) { // pose is assumed to be in the robots coordinate frame + if (this->cfg.async_control) { + this->osc_set_cartesian_position(x); + return; + } + // TODO: this should handled with tcp offset config common::Pose nominal_end_effector_frame_value; if (this->cfg.nominal_end_effector_frame.has_value()) { nominal_end_effector_frame_value = @@ -184,14 +670,16 @@ void FR3::set_cartesian_position(const common::Pose &x) { } else { nominal_end_effector_frame_value = common::Pose::Identity(); } + // 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.controller == IKController::internal) { + if (this->cfg.ik_solver == IKSolver::franka) { // 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.controller == IKController::robotics_library) { + } else if (this->cfg.ik_solver == IKSolver::rcs) { this->set_cartesian_position_ik(x); } } diff --git a/src/hw/FR3.h b/src/hw/FR3.h index bba59083..dd8991cb 100644 --- a/src/hw/FR3.h +++ b/src/hw/FR3.h @@ -2,6 +2,7 @@ #define RCS_FR3_H #include +#include #include #include #include @@ -9,8 +10,10 @@ #include #include +#include #include #include +#include namespace rcs { namespace hw { @@ -25,18 +28,21 @@ struct FR3Load { std::optional f_x_cload; std::optional load_inertia; }; -enum IKController { internal = 0, robotics_library }; +enum IKSolver { franka = 0, rcs }; +// modes: joint-space control, operational-space control, zero-torque +// control +enum Controller { none = 0, jsc, osc, ztc }; struct FR3Config : common::RConfig { // TODO: max force and elbow? // TODO: we can either write specific bindings for each, or we use python // dictionaries with these objects - IKController controller = IKController::internal; - bool guiding_mode_enabled = true; + IKSolver ik_solver = IKSolver::franka; double speed_factor = DEFAULT_SPEED_FACTOR; std::optional load_parameters = std::nullopt; std::optional nominal_end_effector_frame = std::nullopt; std::optional world_to_robot = std::nullopt; common::Pose tcp_offset = common::Pose::Identity(); + bool async_control = false; }; struct FR3State : common::RState {}; @@ -46,6 +52,16 @@ class FR3 : public common::Robot { franka::Robot robot; FR3Config cfg; std::optional> m_ik; + std::optional control_thread = std::nullopt; + common::LinearPoseTrajInterpolator traj_interpolator; + double controller_time = 0.0; + common::LinearJointPositionTrajInterpolator joint_interpolator; + franka::RobotState curr_state; + std::mutex interpolator_mutex; + Controller running_controller = Controller::none; + void osc(); + void joint_controller(); + void zero_torque_controller(); public: FR3(const std::string &ip, @@ -67,7 +83,15 @@ class FR3 : public common::Robot { common::Vector7d get_joint_position() override; - void set_guiding_mode(bool enabled); + void set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, bool yaw, + bool elbow); + + void controller_set_joint_position(const common::Vector7d &desired_q); + void osc_set_cartesian_position( + const common::Pose &desired_pose_EE_in_base_frame); + void zero_torque_guiding(); + + void stop_control_thread(); void move_home() override; diff --git a/src/hw/FrankaHand.cpp b/src/hw/FrankaHand.cpp index 7ec33e85..0ef42013 100644 --- a/src/hw/FrankaHand.cpp +++ b/src/hw/FrankaHand.cpp @@ -1,9 +1,11 @@ #include "FrankaHand.h" +#include #include #include #include +#include #include #include @@ -19,7 +21,7 @@ FrankaHand::FrankaHand(const std::string &ip, const FHConfig &cfg) FrankaHand::~FrankaHand() {} bool FrankaHand::set_parameters(const FHConfig &cfg) { - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); if (gripper_state.max_width < cfg.grasping_width) { return false; } @@ -35,7 +37,7 @@ FHConfig *FrankaHand::get_parameters() { } FHState *FrankaHand::get_state() { - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); if (std::abs(gripper_state.max_width - this->cfg.grasping_width) > 0.01) { this->max_width = gripper_state.max_width - 0.001; } @@ -45,6 +47,8 @@ FHState *FrankaHand::get_state() { state->temperature = gripper_state.temperature; state->max_unnormalized_width = this->max_width; state->last_commanded_width = this->last_commanded_width; + state->bool_state = this->last_commanded_width > 0; + state->is_moving = this->is_moving; return state; } @@ -53,54 +57,129 @@ void FrankaHand::set_normalized_width(double width, double force) { throw std::invalid_argument( "width must be between 0 and 1, force must be positive"); } - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); width = width * gripper_state.max_width; this->last_commanded_width = width; if (force < 0.01) { - gripper.move(width, this->cfg.speed); + this->gripper.move(width, this->cfg.speed); } else { - gripper.grasp(width, this->cfg.speed, force, this->cfg.epsilon_inner, - this->cfg.epsilon_outer); + this->gripper.grasp(width, this->cfg.speed, force, this->cfg.epsilon_inner, + this->cfg.epsilon_outer); } } double FrankaHand::get_normalized_width() { - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); return gripper_state.width / gripper_state.max_width; } +void FrankaHand::m_stop() { + try { + this->gripper.stop(); + } catch (const franka::CommandException &e) { + std::cerr << "franka hand command exception ignored stop" << std::endl; + } + this->m_wait(); + this->is_moving = false; +} + +void FrankaHand::m_wait() { + if (this->control_thread.has_value()) { + this->control_thread->join(); + this->control_thread.reset(); + } +} + void FrankaHand::m_reset() { - this->gripper.stop(); + this->m_stop(); // open gripper - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); this->max_width = gripper_state.max_width - 0.001; this->last_commanded_width = this->max_width; - gripper.move(this->max_width, this->cfg.speed); + this->is_moving = true; + this->gripper.move(this->max_width, this->cfg.speed); + this->is_moving = false; } void FrankaHand::reset() { this->m_reset(); } bool FrankaHand::is_grasped() { - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = this->gripper.readOnce(); return gripper_state.is_grasped; } bool FrankaHand::homing() { // Do a homing in order to estimate the maximum // grasping width with the current fingers. - return gripper.homing(); + this->is_moving = true; + bool success = this->gripper.homing(); + this->is_moving = false; + return success; } void FrankaHand::grasp() { - this->last_commanded_width = this->max_width; - gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1); + if (this->is_moving || this->last_commanded_width == 0) { + return; + } + this->last_commanded_width = 0; + if (!this->cfg.async_control) { + this->is_moving = true; + this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1); + this->is_moving = false; + return; + } + this->m_stop(); + this->control_thread = std::thread([&]() { + this->is_moving = true; + try { + this->gripper.grasp(0, this->cfg.speed, this->cfg.force, 1, 1); + } catch (const franka::CommandException &e) { + std::cerr << "franka hand command exception ignored grasp" << std::endl; + } + this->is_moving = false; + }); } void FrankaHand::open() { + if (this->is_moving || this->last_commanded_width == this->max_width) { + return; + } this->last_commanded_width = this->max_width; - gripper.move(this->max_width, this->cfg.speed); + if (!this->cfg.async_control) { + this->is_moving = true; + this->gripper.move(this->max_width, this->cfg.speed); + this->is_moving = false; + return; + } + this->m_stop(); + this->control_thread = std::thread([&]() { + this->is_moving = true; + try { + // perhaps we should use graps here + this->gripper.move(this->max_width, this->cfg.speed); + // this->gripper.grasp(this->max_width, this->cfg.speed, this->cfg.force, + // 1, 1); + } catch (const franka::CommandException &e) { + std::cerr << "franka hand command exception ignored open" << std::endl; + } + this->is_moving = false; + }); } void FrankaHand::shut() { + if (this->is_moving || this->last_commanded_width == 0) { + return; + } this->last_commanded_width = 0; - gripper.move(0, this->cfg.speed); + if (!this->cfg.async_control) { + this->is_moving = true; + this->gripper.move(0, this->cfg.speed); + this->is_moving = false; + return; + } + this->m_stop(); + this->control_thread = std::thread([&]() { + this->is_moving = true; + this->gripper.move(0, this->cfg.speed); + this->is_moving = false; + }); } } // namespace hw } // namespace rcs \ No newline at end of file diff --git a/src/hw/FrankaHand.h b/src/hw/FrankaHand.h index 90bfc448..1ddf40a3 100644 --- a/src/hw/FrankaHand.h +++ b/src/hw/FrankaHand.h @@ -7,6 +7,7 @@ #include #include #include +#include // TODO: we need a common interface for the gripper, maybe we do use the hal // we need to create a robot class that has both the robot and the gripper @@ -24,14 +25,18 @@ struct FHConfig : common::GConfig { double force = 5; double epsilon_inner = 0.005; double epsilon_outer = 0.005; + bool async_control = false; }; struct FHState : common::GState { double width; + // true is open + bool bool_state; bool is_grasped; uint16_t temperature; double last_commanded_width; double max_unnormalized_width; + bool is_moving; }; class FrankaHand : public common::Gripper { @@ -40,7 +45,12 @@ class FrankaHand : public common::Gripper { FHConfig cfg; double max_width; double last_commanded_width; + // TODO: might be better if is_moving is a lock + bool is_moving = false; + std::optional control_thread = std::nullopt; void m_reset(); + void m_stop(); + void m_wait(); public: FrankaHand(const std::string &ip, const FHConfig &cfg); diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index 1ba93d27..b6e2eb8d 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -314,22 +314,21 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("f_x_cload", &rcs::hw::FR3Load::f_x_cload) .def_readwrite("load_inertia", &rcs::hw::FR3Load::load_inertia); - py::enum_(hw, "IKController") - .value("internal", rcs::hw::IKController::internal) - .value("robotics_library", rcs::hw::IKController::robotics_library) + py::enum_(hw, "IKSolver") + .value("franka", rcs::hw::IKSolver::franka) + .value("rcs", rcs::hw::IKSolver::rcs) .export_values(); py::class_(hw, "FR3Config") .def(py::init<>()) - .def_readwrite("controller", &rcs::hw::FR3Config::controller) + .def_readwrite("ik_solver", &rcs::hw::FR3Config::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FR3Config::speed_factor) - .def_readwrite("guiding_mode_enabled", - &rcs::hw::FR3Config::guiding_mode_enabled) .def_readwrite("load_parameters", &rcs::hw::FR3Config::load_parameters) .def_readwrite("nominal_end_effector_frame", &rcs::hw::FR3Config::nominal_end_effector_frame) .def_readwrite("world_to_robot", &rcs::hw::FR3Config::world_to_robot) - .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset); + .def_readwrite("tcp_offset", &rcs::hw::FR3Config::tcp_offset) + .def_readwrite("async_control", &rcs::hw::FR3Config::async_control); py::class_(hw, "FHConfig") .def(py::init<>()) @@ -337,12 +336,15 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("speed", &rcs::hw::FHConfig::speed) .def_readwrite("force", &rcs::hw::FHConfig::force) .def_readwrite("epsilon_inner", &rcs::hw::FHConfig::epsilon_inner) - .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer); + .def_readwrite("epsilon_outer", &rcs::hw::FHConfig::epsilon_outer) + .def_readwrite("async_control", &rcs::hw::FHConfig::async_control); py::class_(hw, "FHState") .def(py::init<>()) .def_readonly("width", &rcs::hw::FHState::width) .def_readonly("is_grasped", &rcs::hw::FHState::is_grasped) + .def_readonly("is_moving", &rcs::hw::FHState::is_moving) + .def_readonly("bool_state", &rcs::hw::FHState::bool_state) .def_readonly("last_commanded_width", &rcs::hw::FHState::last_commanded_width) .def_readonly("max_unnormalized_width", @@ -360,7 +362,16 @@ PYBIND11_MODULE(_core, m) { .def("set_default_robot_behavior", &rcs::hw::FR3::set_default_robot_behavior) .def("set_guiding_mode", &rcs::hw::FR3::set_guiding_mode, - py::arg("enabled")) + py::arg("x") = true, py::arg("y") = true, py::arg("z") = true, + py::arg("roll") = true, py::arg("pitch") = true, + py::arg("yaw") = true, py::arg("elbow") = true) + .def("zero_torque_guiding", &rcs::hw::FR3::zero_torque_guiding) + .def("osc_set_cartesian_position", + &rcs::hw::FR3::osc_set_cartesian_position, + py::arg("desired_pos_EE_in_base_frame")) + .def("controller_set_joint_position", + &rcs::hw::FR3::controller_set_joint_position, py::arg("desired_q")) + .def("stop_control_thread", &rcs::hw::FR3::stop_control_thread) .def("automatic_error_recovery", &rcs::hw::FR3::automatic_error_recovery) .def("double_tap_robot_to_continue", &rcs::hw::FR3::double_tap_robot_to_continue)