WIP

examples/real_robot_example/gym_real_world/__init__.py

@@ -0,0 +1,8 @@
+from gymnasium.envs.registration import register
+
+register(
+    id="gym_real_world/RealEnv-v0",
+    entry_point="gym_real_world.gym_environment:RealEnv",
+    max_episode_steps=300,
+    nondeterministic=True,
+)

examples/real_robot_example/gym_real_world/dynamixel.py

@@ -0,0 +1,363 @@
+# ruff: noqa
+"""From Alexander Koch low_cost_robot codebase at https://github.com/AlexanderKoch-Koch/low_cost_robot
+Dynamixel class to control the dynamixel servos
+"""
+
+from __future__ import annotations
+
+import enum
+import math
+import os
+from dataclasses import dataclass
+
+import numpy as np
+from dynamixel_sdk import *  # Uses Dynamixel SDK library
+
+
+def pos2pwm(pos: np.ndarray) -> np.ndarray:
+    """
+    :param pos: numpy array of joint positions in range [-pi, pi]
+    :return: numpy array of pwm values in range [0, 4096]
+    """
+    return ((pos / 3.14 + 1.0) * 2048).astype(np.int64)
+
+
+def pwm2pos(pwm: np.ndarray) -> np.ndarray:
+    """
+    :param pwm: numpy array of pwm values in range [0, 4096]
+    :return: numpy array of joint positions in range [-pi, pi]
+    """
+    return (pwm / 2048 - 1) * 3.14
+
+
+def pwm2vel(pwm: np.ndarray) -> np.ndarray:
+    """
+    :param pwm: numpy array of pwm/s joint velocities
+    :return: numpy array of rad/s joint velocities
+    """
+    return pwm * 3.14 / 2048
+
+
+def vel2pwm(vel: np.ndarray) -> np.ndarray:
+    """
+    :param vel: numpy array of rad/s joint velocities
+    :return: numpy array of pwm/s joint velocities
+    """
+    return (vel * 2048 / 3.14).astype(np.int64)
+
+
+class ReadAttribute(enum.Enum):
+    TEMPERATURE = 146
+    VOLTAGE = 145
+    VELOCITY = 128
+    POSITION = 132
+    CURRENT = 126
+    PWM = 124
+    HARDWARE_ERROR_STATUS = 70
+    HOMING_OFFSET = 20
+    BAUDRATE = 8
+
+
+class OperatingMode(enum.Enum):
+    VELOCITY = 1
+    POSITION = 3
+    CURRENT_CONTROLLED_POSITION = 5
+    PWM = 16
+    UNKNOWN = -1
+
+
+class Dynamixel:
+    ADDR_TORQUE_ENABLE = 64
+    ADDR_GOAL_POSITION = 116
+    ADDR_VELOCITY_LIMIT = 44
+    ADDR_GOAL_PWM = 100
+    OPERATING_MODE_ADDR = 11
+    POSITION_I = 82
+    POSITION_P = 84
+    ADDR_ID = 7
+
+    @dataclass
+    class Config:
+        def instantiate(self):
+            return Dynamixel(self)
+
+        baudrate: int = 57600
+        protocol_version: float = 2.0
+        device_name: str = ""  # /dev/tty.usbserial-1120'
+        dynamixel_id: int = 1
+
+    def __init__(self, config: Config):
+        self.config = config
+        self.connect()
+
+    def connect(self):
+        if self.config.device_name == "":
+            for port_name in os.listdir("/dev"):
+                if "ttyUSB" in port_name or "ttyACM" in port_name:
+                    self.config.device_name = "/dev/" + port_name
+                    print(f"using device {self.config.device_name}")
+        self.portHandler = PortHandler(self.config.device_name)
+        # self.portHandler.LA
+        self.packetHandler = PacketHandler(self.config.protocol_version)
+        if not self.portHandler.openPort():
+            raise Exception(f"Failed to open port {self.config.device_name}")
+
+        if not self.portHandler.setBaudRate(self.config.baudrate):
+            raise Exception(f"failed to set baudrate to {self.config.baudrate}")
+
+        # self.operating_mode = OperatingMode.UNKNOWN
+        # self.torque_enabled = False
+        # self._disable_torque()
+
+        self.operating_modes = [None for _ in range(32)]
+        self.torque_enabled = [None for _ in range(32)]
+        return True
+
+    def disconnect(self):
+        self.portHandler.closePort()
+
+    def set_goal_position(self, motor_id, goal_position):
+        # if self.operating_modes[motor_id] is not OperatingMode.POSITION:
+        #     self._disable_torque(motor_id)
+        #     self.set_operating_mode(motor_id, OperatingMode.POSITION)
+
+        # if not self.torque_enabled[motor_id]:
+        #     self._enable_torque(motor_id)
+
+        # self._enable_torque(motor_id)
+        dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
+            self.portHandler, motor_id, self.ADDR_GOAL_POSITION, goal_position
+        )
+        # self._process_response(dxl_comm_result, dxl_error)
+        # print(f'set position of motor {motor_id} to {goal_position}')
+
+    def set_pwm_value(self, motor_id: int, pwm_value, tries=3):
+        if self.operating_modes[motor_id] is not OperatingMode.PWM:
+            self._disable_torque(motor_id)
+            self.set_operating_mode(motor_id, OperatingMode.PWM)
+
+        if not self.torque_enabled[motor_id]:
+            self._enable_torque(motor_id)
+            # print(f'enabling torque')
+        dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
+            self.portHandler, motor_id, self.ADDR_GOAL_PWM, pwm_value
+        )
+        # self._process_response(dxl_comm_result, dxl_error)
+        # print(f'set pwm of motor {motor_id} to {pwm_value}')
+        if dxl_comm_result != COMM_SUCCESS:
+            if tries <= 1:
+                raise ConnectionError(f"dxl_comm_result: {self.packetHandler.getTxRxResult(dxl_comm_result)}")
+            else:
+                print(f"dynamixel pwm setting failure trying again with {tries - 1} tries")
+                self.set_pwm_value(motor_id, pwm_value, tries=tries - 1)
+        elif dxl_error != 0:
+            print(f"dxl error {dxl_error}")
+            raise ConnectionError(f"dynamixel error: {self.packetHandler.getTxRxResult(dxl_error)}")
+
+    def read_temperature(self, motor_id: int):
+        return self._read_value(motor_id, ReadAttribute.TEMPERATURE, 1)
+
+    def read_velocity(self, motor_id: int):
+        pos = self._read_value(motor_id, ReadAttribute.VELOCITY, 4)
+        if pos > 2**31:
+            pos -= 2**32
+        # print(f'read position {pos} for motor {motor_id}')
+        return pos
+
+    def read_position(self, motor_id: int):
+        pos = self._read_value(motor_id, ReadAttribute.POSITION, 4)
+        if pos > 2**31:
+            pos -= 2**32
+        # print(f'read position {pos} for motor {motor_id}')
+        return pos
+
+    def read_position_degrees(self, motor_id: int) -> float:
+        return (self.read_position(motor_id) / 4096) * 360
+
+    def read_position_radians(self, motor_id: int) -> float:
+        return (self.read_position(motor_id) / 4096) * 2 * math.pi
+
+    def read_current(self, motor_id: int):
+        current = self._read_value(motor_id, ReadAttribute.CURRENT, 2)
+        if current > 2**15:
+            current -= 2**16
+        return current
+
+    def read_present_pwm(self, motor_id: int):
+        return self._read_value(motor_id, ReadAttribute.PWM, 2)
+
+    def read_hardware_error_status(self, motor_id: int):
+        return self._read_value(motor_id, ReadAttribute.HARDWARE_ERROR_STATUS, 1)
+
+    def disconnect(self):
+        self.portHandler.closePort()
+
+    def set_id(self, old_id, new_id, use_broadcast_id: bool = False):
+        """
+        sets the id of the dynamixel servo
+        @param old_id: current id of the servo
+        @param new_id: new id
+        @param use_broadcast_id: set ids of all connected dynamixels if True.
+         If False, change only servo with self.config.id
+        @return:
+        """
+        if use_broadcast_id:
+            current_id = 254
+        else:
+            current_id = old_id
+        dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
+            self.portHandler, current_id, self.ADDR_ID, new_id
+        )
+        self._process_response(dxl_comm_result, dxl_error, old_id)
+        self.config.id = id
+
+    def _enable_torque(self, motor_id):
+        dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
+            self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 1
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+        self.torque_enabled[motor_id] = True
+
+    def _disable_torque(self, motor_id):
+        dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
+            self.portHandler, motor_id, self.ADDR_TORQUE_ENABLE, 0
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+        self.torque_enabled[motor_id] = False
+
+    def _process_response(self, dxl_comm_result: int, dxl_error: int, motor_id: int):
+        if dxl_comm_result != COMM_SUCCESS:
+            raise ConnectionError(
+                f"dxl_comm_result for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_comm_result)}"
+            )
+        elif dxl_error != 0:
+            print(f"dxl error {dxl_error}")
+            raise ConnectionError(
+                f"dynamixel error for motor {motor_id}: {self.packetHandler.getTxRxResult(dxl_error)}"
+            )
+
+    def set_operating_mode(self, motor_id: int, operating_mode: OperatingMode):
+        dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
+            self.portHandler, motor_id, self.OPERATING_MODE_ADDR, operating_mode.value
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+        self.operating_modes[motor_id] = operating_mode
+
+    def set_pwm_limit(self, motor_id: int, limit: int):
+        dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(self.portHandler, motor_id, 36, limit)
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+
+    def set_velocity_limit(self, motor_id: int, velocity_limit):
+        dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
+            self.portHandler, motor_id, self.ADDR_VELOCITY_LIMIT, velocity_limit
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+
+    def set_P(self, motor_id: int, P: int):
+        dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
+            self.portHandler, motor_id, self.POSITION_P, P
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+
+    def set_I(self, motor_id: int, I: int):
+        dxl_comm_result, dxl_error = self.packetHandler.write2ByteTxRx(
+            self.portHandler, motor_id, self.POSITION_I, I
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+
+    def read_home_offset(self, motor_id: int):
+        self._disable_torque(motor_id)
+        # dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(self.portHandler, motor_id,
+        #                                                                ReadAttribute.HOMING_OFFSET.value, home_position)
+        home_offset = self._read_value(motor_id, ReadAttribute.HOMING_OFFSET, 4)
+        # self._process_response(dxl_comm_result, dxl_error)
+        self._enable_torque(motor_id)
+        return home_offset
+
+    def set_home_offset(self, motor_id: int, home_position: int):
+        self._disable_torque(motor_id)
+        dxl_comm_result, dxl_error = self.packetHandler.write4ByteTxRx(
+            self.portHandler, motor_id, ReadAttribute.HOMING_OFFSET.value, home_position
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+        self._enable_torque(motor_id)
+
+    def set_baudrate(self, motor_id: int, baudrate):
+        # translate baudrate into dynamixel baudrate setting id
+        if baudrate == 57600:
+            baudrate_id = 1
+        elif baudrate == 1_000_000:
+            baudrate_id = 3
+        elif baudrate == 2_000_000:
+            baudrate_id = 4
+        elif baudrate == 3_000_000:
+            baudrate_id = 5
+        elif baudrate == 4_000_000:
+            baudrate_id = 6
+        else:
+            raise Exception("baudrate not implemented")
+
+        self._disable_torque(motor_id)
+        dxl_comm_result, dxl_error = self.packetHandler.write1ByteTxRx(
+            self.portHandler, motor_id, ReadAttribute.BAUDRATE.value, baudrate_id
+        )
+        self._process_response(dxl_comm_result, dxl_error, motor_id)
+
+    def _read_value(self, motor_id, attribute: ReadAttribute, num_bytes: int, tries=10):
+        try:
+            if num_bytes == 1:
+                value, dxl_comm_result, dxl_error = self.packetHandler.read1ByteTxRx(
+                    self.portHandler, motor_id, attribute.value
+                )
+            elif num_bytes == 2:
+                value, dxl_comm_result, dxl_error = self.packetHandler.read2ByteTxRx(
+                    self.portHandler, motor_id, attribute.value
+                )
+            elif num_bytes == 4:
+                value, dxl_comm_result, dxl_error = self.packetHandler.read4ByteTxRx(
+                    self.portHandler, motor_id, attribute.value
+                )
+        except Exception:
+            if tries == 0:
+                raise Exception
+            else:
+                return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
+        if dxl_comm_result != COMM_SUCCESS:
+            if tries <= 1:
+                # print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result))
+                raise ConnectionError(f"dxl_comm_result {dxl_comm_result} for servo {motor_id} value {value}")
+            else:
+                print(f"dynamixel read failure for servo {motor_id} trying again with {tries - 1} tries")
+                time.sleep(0.02)
+                return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
+        elif dxl_error != 0:  # # print("%s" % self.packetHandler.getRxPacketError(dxl_error))
+            # raise ConnectionError(f'dxl_error {dxl_error} binary ' + "{0:b}".format(37))
+            if tries == 0 and dxl_error != 128:
+                raise Exception(f"Failed to read value from motor {motor_id} error is {dxl_error}")
+            else:
+                return self._read_value(motor_id, attribute, num_bytes, tries=tries - 1)
+        return value
+
+    def set_home_position(self, motor_id: int):
+        print(f"setting home position for motor {motor_id}")
+        self.set_home_offset(motor_id, 0)
+        current_position = self.read_position(motor_id)
+        print(f"position before {current_position}")
+        self.set_home_offset(motor_id, -current_position)
+        # dynamixel.set_home_offset(motor_id, -4096)
+        # dynamixel.set_home_offset(motor_id, -4294964109)
+        current_position = self.read_position(motor_id)
+        # print(f'signed position {current_position - 2** 32}')
+        print(f"position after {current_position}")
+
+
+if __name__ == "__main__":
+    dynamixel = Dynamixel.Config(baudrate=1_000_000, device_name="/dev/tty.usbmodem57380045631").instantiate()
+    motor_id = 1
+    pos = dynamixel.read_position(motor_id)
+    for i in range(10):
+        s = time.monotonic()
+        pos = dynamixel.read_position(motor_id)
+        delta = time.monotonic() - s
+        print(f"read position took {delta}")
+        print(f"position {pos}")

examples/real_robot_example/gym_real_world/gym_environment.py

@@ -0,0 +1,192 @@
+import time
+from unittest.mock import MagicMock
+
+import cv2
+import gymnasium as gym
+import numpy as np
+from gymnasium import spaces
+
+from .dynamixel import pos2pwm, pwm2pos
+from .robot import Robot
+
+FPS = 30
+
+CAMERAS_SHAPES = {
+    "images.high": (480, 640, 3),
+    "images.low": (480, 640, 3),
+}
+
+CAMERAS_PORTS = {
+    "images.high": "/dev/video6",
+    "images.low": "/dev/video0",
+}
+
+LEADER_PORT = "/dev/ttyACM1"
+FOLLOWER_PORT = "/dev/ttyACM0"
+
+MockRobot = MagicMock()
+MockRobot.read_position = MagicMock()
+MockRobot.read_position.return_value = np.array([0.0, 1.0, 2.0, 3.0, 4.0, 5.0])
+
+MockCamera = MagicMock()
+MockCamera.isOpened = MagicMock(return_value=True)
+MockCamera.read = MagicMock(return_value=(True, np.zeros((480, 640, 3), dtype=np.uint8)))
+
+
+def capture_image(cam, cam_width, cam_height):
+    # Capture a single frame
+    _, frame = cam.read()
+    image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+    # # Define your crop coordinates (top left corner and bottom right corner)
+    # x1, y1 = 400, 0  # Example starting coordinates (top left of the crop rectangle)
+    # x2, y2 = 1600, 900  # Example ending coordinates (bottom right of the crop rectangle)
+    # # Crop the image
+    # image = image[y1:y2, x1:x2]
+    # Resize the image
+    image = cv2.resize(image, (cam_width, cam_height), interpolation=cv2.INTER_AREA)
+
+    return image
+
+
+class RealEnv(gym.Env):
+    metadata = {}
+
+    def __init__(
+        self,
+        record: bool = False,
+        num_joints: int = 6,
+        cameras_shapes: dict = CAMERAS_SHAPES,
+        cameras_ports: dict = CAMERAS_PORTS,
+        follower_port: str = FOLLOWER_PORT,
+        leader_port: str = LEADER_PORT,
+        warmup_steps: int = 100,
+        trigger_torque=70,
+        fps: int = FPS,
+        fps_tolerance: float = 0.1,
+        mock: bool = False,
+    ):
+        self.num_joints = num_joints
+        self.cameras_shapes = cameras_shapes
+        self.cameras_ports = cameras_ports
+        self.warmup_steps = warmup_steps
+        assert len(self.cameras_shapes) == len(self.cameras_ports), "Number of cameras and shapes must match."
+
+        self.follower_port = follower_port
+        self.leader_port = leader_port
+        self.record = record
+        self.fps = fps
+        self.fps_tolerance = fps_tolerance
+
+        # Initialize the robot
+        self.follower = Robot(device_name=self.follower_port) if not mock else MockRobot
+        if self.record:
+            self.leader = Robot(device_name=self.leader_port) if not mock else MockRobot
+            self.leader.set_trigger_torque(trigger_torque)
+
+        # Initialize the cameras - sorted by camera names
+        self.cameras = {}
+        for cn, p in sorted(self.cameras_ports.items()):
+            self.cameras[cn] = cv2.VideoCapture(p) if not mock else MockCamera
+            if not self.cameras[cn].isOpened():
+                raise OSError(
+                    f"Cannot open camera port {p} for {cn}."
+                    f" Make sure the camera is connected and the port is correct."
+                    f"Also check you are not spinning several instances of the same environment (eval.batch_size)"
+                )
+
+        # Specify gym action and observation spaces
+        observation_space = {}
+
+        if self.num_joints > 0:
+            observation_space["agent_pos"] = spaces.Box(
+                low=-1000.0,
+                high=1000.0,
+                shape=(num_joints,),
+                dtype=np.float64,
+            )
+        if self.record:
+            observation_space["leader_pos"] = spaces.Box(
+                low=-1000.0,
+                high=1000.0,
+                shape=(num_joints,),
+                dtype=np.float64,
+            )
+
+        if self.cameras_shapes:
+            for cn, hwc_shape in self.cameras_shapes.items():
+                # Assumes images are unsigned int8 in [0,255]
+                observation_space[cn] = spaces.Box(
+                    low=0,
+                    high=255,
+                    # height x width x channels (e.g. 480 x 640 x 3)
+                    shape=hwc_shape,
+                    dtype=np.uint8,
+                )
+
+        self.observation_space = spaces.Dict(observation_space)
+        self.action_space = spaces.Box(low=-1, high=1, shape=(num_joints,), dtype=np.float32)
+
+        self._observation = {}
+        self._terminated = False
+        self.timestamps = []
+
+    def _get_obs(self):
+        qpos = self.follower.read_position()
+        self._observation["agent_pos"] = pwm2pos(qpos)
+        for cn, c in self.cameras.items():
+            self._observation[cn] = capture_image(c, self.cameras_shapes[cn][1], self.cameras_shapes[cn][0])
+
+        if self.record:
+            action = self.leader.read_position()
+            self._observation["leader_pos"] = pwm2pos(action)
+
+    def reset(self, seed: int | None = None):
+        # Reset the robot and sync the leader and follower if we are recording
+        for _ in range(self.warmup_steps):
+            self._get_obs()
+            if self.record:
+                self.follower.set_goal_pos(pos2pwm(self._observation["leader_pos"]))
+        self._terminated = False
+        info = {}
+        self.timestamps = []
+        return self._observation, info
+
+    def step(self, action: np.ndarray = None):
+        if self.timestamps:
+            # wait the right amount of time to stay at the desired fps
+            time.sleep(max(0, 1 / self.fps - (time.time() - self.timestamps[-1])))
+
+        self.timestamps.append(time.time())
+
+        # Get the observation
+        self._get_obs()
+        if self.record:
+            # Teleoperate the leader
+            self.follower.set_goal_pos(pos2pwm(self._observation["leader_pos"]))
+        else:
+            # Apply the action to the follower
+            self.follower.set_goal_pos(pos2pwm(action))
+
+        reward = 0
+        terminated = truncated = self._terminated
+        info = {"timestamp": self.timestamps[-1] - self.timestamps[0], "fps_error": False}
+
+        # Check if we are able to keep up with the desired fps
+        if len(self.timestamps) > 1 and (self.timestamps[-1] - self.timestamps[-2]) > 1 / (
+            self.fps - self.fps_tolerance
+        ):
+            print(
+                f"Error: recording fps {1 / (self.timestamps[-1] - self.timestamps[-2]):.5f} is lower"
+                f" than min admited fps {(self.fps - self.fps_tolerance):.5f}"
+                f" at frame {len(self.timestamps)}"
+            )
+            info["fps_error"] = True
+
+        return self._observation, reward, terminated, truncated, info
+
+    def render(self): ...
+
+    def close(self):
+        self.follower._disable_torque()
+        if self.record:
+            self.leader._disable_torque()

examples/real_robot_example/gym_real_world/robot.py

@@ -0,0 +1,173 @@
+# ruff: noqa
+"""From Alexander Koch low_cost_robot codebase at https://github.com/AlexanderKoch-Koch/low_cost_robot
+Class to control the robot using dynamixel servos.
+"""
+
+from enum import Enum, auto
+from typing import Union
+
+import numpy as np
+from dynamixel_sdk import DXL_HIBYTE, DXL_HIWORD, DXL_LOBYTE, DXL_LOWORD, GroupSyncRead, GroupSyncWrite
+
+from .dynamixel import Dynamixel, OperatingMode, ReadAttribute
+
+
+class MotorControlType(Enum):
+    PWM = auto()
+    POSITION_CONTROL = auto()
+    DISABLED = auto()
+    UNKNOWN = auto()
+
+
+class Robot:
+    def __init__(self, device_name: str, baudrate=1_000_000, servo_ids=[1, 2, 3, 4, 5, 6]) -> None:
+        self.servo_ids = servo_ids
+        self.dynamixel = Dynamixel.Config(baudrate=baudrate, device_name=device_name).instantiate()
+        self._init_motors()
+
+    def _init_motors(self):
+        self.position_reader = GroupSyncRead(
+            self.dynamixel.portHandler, self.dynamixel.packetHandler, ReadAttribute.POSITION.value, 4
+        )
+        for id in self.servo_ids:
+            self.position_reader.addParam(id)
+
+        self.velocity_reader = GroupSyncRead(
+            self.dynamixel.portHandler, self.dynamixel.packetHandler, ReadAttribute.VELOCITY.value, 4
+        )
+        for id in self.servo_ids:
+            self.velocity_reader.addParam(id)
+
+        self.pos_writer = GroupSyncWrite(
+            self.dynamixel.portHandler, self.dynamixel.packetHandler, self.dynamixel.ADDR_GOAL_POSITION, 4
+        )
+        for id in self.servo_ids:
+            self.pos_writer.addParam(id, [2048])
+
+        self.pwm_writer = GroupSyncWrite(
+            self.dynamixel.portHandler, self.dynamixel.packetHandler, self.dynamixel.ADDR_GOAL_PWM, 2
+        )
+        for id in self.servo_ids:
+            self.pwm_writer.addParam(id, [2048])
+        # self._disable_torque()
+        self.motor_control_state = MotorControlType.DISABLED
+
+    def read_position(self, tries=2):
+        """
+        Reads the joint positions of the robot. 2048 is the center position. 0 and 4096 are 180 degrees in each direction.
+        :param tries: maximum number of tries to read the position
+        :return: list of joint positions in range [0, 4096]
+        """
+        result = self.position_reader.txRxPacket()
+        if result != 0:
+            if tries > 0:
+                return self.read_position(tries=tries - 1)
+            else:
+                print("failed to read position!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
+        positions = []
+        for id in self.servo_ids:
+            position = self.position_reader.getData(id, ReadAttribute.POSITION.value, 4)
+            if position > 2**31:
+                position -= 2**32
+            positions.append(position)
+        return np.array(positions)
+
+    def read_velocity(self):
+        """
+        Reads the joint velocities of the robot.
+        :return: list of joint velocities,
+        """
+        self.velocity_reader.txRxPacket()
+        velocties = []
+        for id in self.servo_ids:
+            velocity = self.velocity_reader.getData(id, ReadAttribute.VELOCITY.value, 4)
+            if velocity > 2**31:
+                velocity -= 2**32
+            velocties.append(velocity)
+        return np.array(velocties)
+
+    def set_goal_pos(self, action):
+        """
+        :param action: list or numpy array of target joint positions in range [0, 4096[
+        """
+        if self.motor_control_state is not MotorControlType.POSITION_CONTROL:
+            self._set_position_control()
+        for i, motor_id in enumerate(self.servo_ids):
+            data_write = [
+                DXL_LOBYTE(DXL_LOWORD(action[i])),
+                DXL_HIBYTE(DXL_LOWORD(action[i])),
+                DXL_LOBYTE(DXL_HIWORD(action[i])),
+                DXL_HIBYTE(DXL_HIWORD(action[i])),
+            ]
+            self.pos_writer.changeParam(motor_id, data_write)
+
+        self.pos_writer.txPacket()
+
+    def set_pwm(self, action):
+        """
+        Sets the pwm values for the servos.
+        :param action: list or numpy array of pwm values in range [0, 885]
+        """
+        if self.motor_control_state is not MotorControlType.PWM:
+            self._set_pwm_control()
+        for i, motor_id in enumerate(self.servo_ids):
+            data_write = [
+                DXL_LOBYTE(DXL_LOWORD(action[i])),
+                DXL_HIBYTE(DXL_LOWORD(action[i])),
+            ]
+            self.pwm_writer.changeParam(motor_id, data_write)
+
+        self.pwm_writer.txPacket()
+
+    def set_trigger_torque(self, torque: int):
+        """
+        Sets a constant torque torque for the last servo in the chain. This is useful for the trigger of the leader arm
+        """
+        self.dynamixel._enable_torque(self.servo_ids[-1])
+        self.dynamixel.set_pwm_value(self.servo_ids[-1], torque)
+
+    def limit_pwm(self, limit: Union[int, list, np.ndarray]):
+        """
+        Limits the pwm values for the servos in for position control
+        @param limit: 0 ~ 885
+        @return:
+        """
+        if isinstance(limit, int):
+            limits = [
+                limit,
+            ] * 5
+        else:
+            limits = limit
+        self._disable_torque()
+        for motor_id, limit in zip(self.servo_ids, limits, strict=False):
+            self.dynamixel.set_pwm_limit(motor_id, limit)
+        self._enable_torque()
+
+    def _disable_torque(self):
+        print(f"disabling torque for servos {self.servo_ids}")
+        for motor_id in self.servo_ids:
+            self.dynamixel._disable_torque(motor_id)
+
+    def _enable_torque(self):
+        print(f"enabling torque for servos {self.servo_ids}")
+        for motor_id in self.servo_ids:
+            self.dynamixel._enable_torque(motor_id)
+
+    def _set_pwm_control(self):
+        self._disable_torque()
+        for motor_id in self.servo_ids:
+            self.dynamixel.set_operating_mode(motor_id, OperatingMode.PWM)
+        self._enable_torque()
+        self.motor_control_state = MotorControlType.PWM
+
+    def _set_position_control(self):
+        self._disable_torque()
+        for motor_id in self.servo_ids:
+            # TODO(rcadene): redesign
+            if motor_id == 9:
+                self.dynamixel.set_operating_mode(9, OperatingMode.CURRENT_CONTROLLED_POSITION)
+            else:
+                self.dynamixel.set_operating_mode(motor_id, OperatingMode.POSITION)
+
+        self._enable_torque()
+        self.motor_control_state = MotorControlType.POSITION_CONTROL