From cdfb5714794bbb29f2b123e857c059785a1d0861 Mon Sep 17 00:00:00 2001 From: apirrone Date: Mon, 17 Mar 2025 11:39:29 +0100 Subject: [PATCH] cleaning up --- .../mini_bdx_runtime/feet_contacts.py | 9 + mini_bdx_runtime/mini_bdx_runtime/feetech.py | 1039 ----------------- .../mini_bdx_runtime/feetech_pwm_control.py | 120 -- .../feetech_pwm_control_rustypot.py | 111 -- mini_bdx_runtime/mini_bdx_runtime/hwi.py | 188 --- .../hwi_feetech_pwm_control.py | 207 ---- .../mini_bdx_runtime/hwi_feetech_pypot.py | 180 --- .../mini_bdx_runtime/hwi_feetech_sdk.py | 139 --- mini_bdx_runtime/mini_bdx_runtime/io_330.py | 221 ---- .../mini_bdx_runtime/rustypot_control_hwi.py | 214 ---- scripts/check_voltage.py | 31 - scripts/ear_servo_test.py | 35 - scripts/head_puppet.py | 3 - scripts/imu_client_raw.py | 88 -- scripts/imu_server_raw.py | 62 - scripts/led_test.py | 39 - scripts/plot_imu_data.py | 14 - scripts/servo_test.py | 62 - scripts/sound_test.py | 75 -- scripts/test_feet.py | 15 - scripts/test_xbox_controller.py | 7 - scripts/turn_off.py | 1 - scripts/turn_on.py | 1 - scripts/v2_rl_walk_mujoco.py | 3 - 24 files changed, 9 insertions(+), 2855 deletions(-) delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/feetech.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control_rustypot.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/hwi.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pwm_control.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pypot.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_sdk.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/io_330.py delete mode 100644 mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py delete mode 100644 scripts/check_voltage.py delete mode 100644 scripts/ear_servo_test.py delete mode 100644 scripts/imu_client_raw.py delete mode 100644 scripts/imu_server_raw.py delete mode 100644 scripts/led_test.py delete mode 100644 scripts/plot_imu_data.py delete mode 100644 scripts/servo_test.py delete mode 100644 scripts/sound_test.py delete mode 100644 scripts/test_feet.py delete mode 100644 scripts/test_xbox_controller.py diff --git a/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py b/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py index 2b23398..a44748f 100644 --- a/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py +++ b/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py @@ -21,3 +21,12 @@ class FeetContacts: if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW: right = True return np.array([left, right]) + + +if __name__ == "__main__": + import time + + feet_contacts = FeetContacts() + while True: + print(feet_contacts.get()) + time.sleep(0.05) diff --git a/mini_bdx_runtime/mini_bdx_runtime/feetech.py b/mini_bdx_runtime/mini_bdx_runtime/feetech.py deleted file mode 100644 index bbafd67..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/feetech.py +++ /dev/null @@ -1,1039 +0,0 @@ -import enum -import logging -import math -import time -import traceback -from copy import deepcopy - -import numpy as np -import tqdm - -# from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError - -# import platform -# import time -from datetime import datetime, timezone - -def capture_timestamp_utc(): - return datetime.now(timezone.utc) - -# def busy_wait(seconds): -# if platform.system() == "Darwin": -# # On Mac, `time.sleep` is not accurate and we need to use this while loop trick, -# # but it consumes CPU cycles. -# # TODO(rcadene): find an alternative: from python 11, time.sleep is precise -# end_time = time.perf_counter() + seconds -# while time.perf_counter() < end_time: -# pass -# else: -# # On Linux time.sleep is accurate -# if seconds > 0: -# time.sleep(seconds) - - -# def safe_disconnect(func): -# # TODO(aliberts): Allow to pass custom exceptions -# # (e.g. ThreadServiceExit, KeyboardInterrupt, SystemExit, UnpluggedError, DynamixelCommError) -# def wrapper(robot, *args, **kwargs): -# try: -# return func(robot, *args, **kwargs) -# except Exception as e: -# if robot.is_connected: -# robot.disconnect() -# raise e - -# return wrapper - - -class RobotDeviceNotConnectedError(Exception): - """Exception raised when the robot device is not connected.""" - - def __init__( - self, message="This robot device is not connected. Try calling `robot_device.connect()` first." - ): - self.message = message - super().__init__(self.message) - - -class RobotDeviceAlreadyConnectedError(Exception): - """Exception raised when the robot device is already connected.""" - - def __init__( - self, - message="This robot device is already connected. Try not calling `robot_device.connect()` twice.", - ): - self.message = message - super().__init__(self.message) - - -PROTOCOL_VERSION = 0 -BAUDRATE = 1_000_000 -TIMEOUT_MS = 1000 - -MAX_ID_RANGE = 252 - -# The following bounds define the lower and upper joints range (after calibration). -# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees -# which corresponds to a half rotation on the left and half rotation on the right. -# Some joints might require higher range, so we allow up to [-270, 270] degrees until -# an error is raised. -LOWER_BOUND_DEGREE = -270 -UPPER_BOUND_DEGREE = 270 -# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper), -# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully -# closed, and 100% is fully open. To account for slight calibration issue, we allow up to -# [-10, 110] until an error is raised. -LOWER_BOUND_LINEAR = -10 -UPPER_BOUND_LINEAR = 110 - -HALF_TURN_DEGREE = 180 - - -# See this link for STS3215 Memory Table: -# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true -# data_name: (address, size_byte) -SCS_SERIES_CONTROL_TABLE = { - "Model": (3, 2), - "ID": (5, 1), - "Baud_Rate": (6, 1), - "Return_Delay": (7, 1), - "Response_Status_Level": (8, 1), - "Min_Angle_Limit": (9, 2), - "Max_Angle_Limit": (11, 2), - "Max_Temperature_Limit": (13, 1), - "Max_Voltage_Limit": (14, 1), - "Min_Voltage_Limit": (15, 1), - "Max_Torque_Limit": (16, 2), - "Phase": (18, 1), - "Unloading_Condition": (19, 1), - "LED_Alarm_Condition": (20, 1), - "P_Coefficient": (21, 1), - "D_Coefficient": (22, 1), - "I_Coefficient": (23, 1), - "Minimum_Startup_Force": (24, 2), - "CW_Dead_Zone": (26, 1), - "CCW_Dead_Zone": (27, 1), - "Protection_Current": (28, 2), - "Angular_Resolution": (30, 1), - "Offset": (31, 2), - "Mode": (33, 1), - "Protective_Torque": (34, 1), - "Protection_Time": (35, 1), - "Overload_Torque": (36, 1), - "Speed_closed_loop_P_proportional_coefficient": (37, 1), - "Over_Current_Protection_Time": (38, 1), - "Velocity_closed_loop_I_integral_coefficient": (39, 1), - "Torque_Enable": (40, 1), - "Acceleration": (41, 1), - "Goal_Position": (42, 2), - "Goal_Time": (44, 2), - "Goal_Speed": (46, 2), - "Torque_Limit": (48, 2), - "Lock": (55, 1), - "Present_Position": (56, 2), - "Present_Speed": (58, 2), - "Present_Load": (60, 2), - "Present_Voltage": (62, 1), - "Present_Temperature": (63, 1), - "Status": (65, 1), - "Moving": (66, 1), - "Present_Current": (69, 2), - # Not in the Memory Table - "Maximum_Acceleration": (85, 2), -} - -SCS_SERIES_BAUDRATE_TABLE = { - 0: 1_000_000, - 1: 500_000, - 2: 250_000, - 3: 128_000, - 4: 115_200, - 5: 57_600, - 6: 38_400, - 7: 19_200, -} - -CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] -CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"] - - -MODEL_CONTROL_TABLE = { - "scs_series": SCS_SERIES_CONTROL_TABLE, - "sts3215": SCS_SERIES_CONTROL_TABLE, -} - -MODEL_RESOLUTION = { - "scs_series": 4096, - "sts3215": 4096, -} - -MODEL_BAUDRATE_TABLE = { - "scs_series": SCS_SERIES_BAUDRATE_TABLE, - "sts3215": SCS_SERIES_BAUDRATE_TABLE, -} - -# High number of retries is needed for feetech compared to dynamixel motors. -NUM_READ_RETRY = 20 -NUM_WRITE_RETRY = 20 - - -def convert_degrees_to_steps( - degrees: float | np.ndarray, models: str | list[str] -) -> np.ndarray: - """This function converts the degree range to the step range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -180 degree position to +180. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - resolutions = [MODEL_RESOLUTION[model] for model in models] - steps = degrees / 180 * np.array(resolutions) / 2 - steps = steps.astype(int) - return steps - - -def convert_steps_to_degrees( - steps: int | np.ndarray, models: str | list[str] -) -> np.ndarray: - """This function converts the step range to the degree range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -180 degree position to +180. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - resolutions = [MODEL_RESOLUTION[model] for model in models] - degrees = steps * 180 / np.array(resolutions) * 2 - return degrees - - -def convert_steps_to_radians( - steps: int | np.ndarray, models: str | list[str] -) -> np.ndarray: - """This function converts the step range to the radian range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -pi radian position to +pi. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - deg = convert_steps_to_degrees(steps, models) - return np.deg2rad(deg) - - -def convert_radians_to_steps( - radians: float | np.ndarray, models: str | list[str] -) -> np.ndarray: - """This function converts the radian range to the step range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -pi radian position to +pi. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - deg = np.rad2deg(radians) - return convert_degrees_to_steps(deg, models) - - -def configure(motor_bus): - - # Mode=0 for Position Control - motor_bus.write("Mode", 0) - # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - motor_bus.write("P_Coefficient", 32) - # Set I_Coefficient and D_Coefficient to default value 0 and 32 - motor_bus.write("I_Coefficient", 0) - motor_bus.write("D_Coefficient", 32) - # Close the write lock so that Maximum_Acceleration gets written to EPROM address, - # which is mandatory for Maximum_Acceleration to take effect after rebooting. - motor_bus.write("Lock", 0) - # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of - # the motors. Note: this configuration is not in the official STS3215 Memory Table - motor_bus.write("Maximum_Acceleration", 254) - motor_bus.write("Acceleration", 254) - - -def convert_to_bytes(value, bytes, mock=False): - if mock: - return value - - import scservo_sdk as scs - - # Note: No need to convert back into unsigned int, since this byte preprocessing - # already handles it for us. - if bytes == 1: - data = [ - scs.SCS_LOBYTE(scs.SCS_LOWORD(value)), - ] - elif bytes == 2: - data = [ - scs.SCS_LOBYTE(scs.SCS_LOWORD(value)), - scs.SCS_HIBYTE(scs.SCS_LOWORD(value)), - ] - elif bytes == 4: - data = [ - scs.SCS_LOBYTE(scs.SCS_LOWORD(value)), - scs.SCS_HIBYTE(scs.SCS_LOWORD(value)), - scs.SCS_LOBYTE(scs.SCS_HIWORD(value)), - scs.SCS_HIBYTE(scs.SCS_HIWORD(value)), - ] - else: - raise NotImplementedError( - f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but " - f"{bytes} is provided instead." - ) - return data - - -def get_group_sync_key(data_name, motor_names): - group_key = f"{data_name}_" + "_".join(motor_names) - return group_key - - -def get_result_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - rslt_name = f"{fn_name}_{group_key}" - return rslt_name - - -def get_queue_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - queue_name = f"{fn_name}_{group_key}" - return queue_name - - -def get_log_name(var_name, fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - log_name = f"{var_name}_{fn_name}_{group_key}" - return log_name - - -def assert_same_address(model_ctrl_table, motor_models, data_name): - all_addr = [] - all_bytes = [] - for model in motor_models: - addr, bytes = model_ctrl_table[model][data_name] - all_addr.append(addr) - all_bytes.append(bytes) - - if len(set(all_addr)) != 1: - raise NotImplementedError( - f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer." - ) - - if len(set(all_bytes)) != 1: - raise NotImplementedError( - f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer." - ) - - -class TorqueMode(enum.Enum): - ENABLED = 1 - DISABLED = 0 - - -class DriveMode(enum.Enum): - NON_INVERTED = 0 - INVERTED = 1 - - -class CalibrationMode(enum.Enum): - # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] - DEGREE = 0 - # Joints with linear motions (like gripper of Aloha) are experessed in nominal range of [0, 100] - LINEAR = 1 - - -class JointOutOfRangeError(Exception): - def __init__(self, message="Joint is out of range"): - self.message = message - super().__init__(self.message) - - -class FeetechMotorsBus: - """ - The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on - the python feetech sdk to communicate with the motors. For more info, see the [feetech SDK Documentation](https://emanual.robotis.com/docs/en/software/feetech/feetech_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20). - - A FeetechMotorsBus instance requires a port (e.g. `FeetechMotorsBus(port="/dev/tty.usbmodem575E0031751"`)). - To find the port, you can run our utility script: - ```bash - python lerobot/scripts/find_motors_bus_port.py - >>> Finding all available ports for the MotorsBus. - >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] - >>> Remove the usb cable from your FeetechMotorsBus and press Enter when done. - >>> The port of this FeetechMotorsBus is /dev/tty.usbmodem575E0031751. - >>> Reconnect the usb cable. - ``` - - Example of usage for 1 motor connected to the bus: - ```python - motor_name = "gripper" - motor_index = 6 - motor_model = "sts3215" - - motors_bus = FeetechMotorsBus( - port="/dev/tty.usbmodem575E0031751", - motors={motor_name: (motor_index, motor_model)}, - ) - motors_bus.connect() - - position = motors_bus.read("Present_Position") - - # move from a few motor steps as an example - few_steps = 30 - motors_bus.write("Goal_Position", position + few_steps) - - # when done, consider disconnecting - motors_bus.disconnect() - ``` - """ - - def __init__( - self, - port: str, - motors: dict[str, tuple[int, str]], - extra_model_control_table: dict[str, list[tuple]] | None = None, - extra_model_resolution: dict[str, int] | None = None, - mock=False, - ): - self.port = port - self.motors = motors - self.mock = mock - - self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) - if extra_model_control_table: - self.model_ctrl_table.update(extra_model_control_table) - - self.model_resolution = deepcopy(MODEL_RESOLUTION) - if extra_model_resolution: - self.model_resolution.update(extra_model_resolution) - - self.port_handler = None - self.packet_handler = None - self.calibration = None - self.is_connected = False - self.group_readers = {} - self.group_writers = {} - self.logs = {} - - self.track_positions = {} - - def connect(self): - if self.is_connected: - raise RobotDeviceAlreadyConnectedError( - f"FeetechMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." - ) - - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - self.port_handler = scs.PortHandler(self.port) - self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION) - - try: - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - except Exception: - traceback.print_exc() - print( - "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n" - ) - raise - - # Allow to read and write - self.is_connected = True - - self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS) - - def reconnect(self): - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - self.port_handler = scs.PortHandler(self.port) - self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION) - - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - - self.is_connected = True - - def are_motors_configured(self): - # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, - # a ConnectionError will be raised anyway. - try: - return (self.motor_indices == self.read("ID")).all() - except ConnectionError as e: - print(e) - return False - - def find_motor_indices(self, possible_ids=None, num_retry=2): - if possible_ids is None: - possible_ids = range(MAX_ID_RANGE) - - indices = [] - for idx in tqdm.tqdm(possible_ids): - try: - present_idx = self.read_with_motor_ids( - self.motor_models, [idx], "ID", num_retry=num_retry - )[0] - except ConnectionError: - continue - - if idx != present_idx: - # sanity check - raise OSError( - "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged." - ) - indices.append(idx) - - return indices - - def set_bus_baudrate(self, baudrate): - present_bus_baudrate = self.port_handler.getBaudRate() - if present_bus_baudrate != baudrate: - print( - f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}." - ) - self.port_handler.setBaudRate(baudrate) - - if self.port_handler.getBaudRate() != baudrate: - raise OSError("Failed to write bus baud rate.") - - @property - def motor_names(self) -> list[str]: - return list(self.motors.keys()) - - @property - def motor_models(self) -> list[str]: - return [model for _, model in self.motors.values()] - - @property - def motor_indices(self) -> list[int]: - return [idx for idx, _ in self.motors.values()] - - def set_calibration(self, calibration: dict[str, list]): - self.calibration = calibration - - def apply_calibration_autocorrect( - self, values: np.ndarray | list, motor_names: list[str] | None - ): - """This function apply the calibration, automatically detects out of range errors for motors values and attempt to correct. - - For more info, see docstring of `apply_calibration` and `autocorrect_calibration`. - """ - try: - values = self.apply_calibration(values, motor_names) - except JointOutOfRangeError as e: - print(e) - self.autocorrect_calibration(values, motor_names) - values = self.apply_calibration(values, motor_names) - return values - - def apply_calibration( - self, values: np.ndarray | list, motor_names: list[str] | None - ): - """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with - a "zero position" at 0 degree. - - Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor - rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range. - - Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation - when given a goal position that is + or - their resolution. For instance, feetech xl330-m077 have a resolution of 4096, and - at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830, - or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor. - To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work - in the centered nominal degree range ]-180, 180[. - """ - if motor_names is None: - motor_names = self.motor_names - - # Convert from unsigned int32 original range [0, 2**32] to signed float32 range - values = values.astype(np.float32) - - for i, name in enumerate(motor_names): - calib_idx = self.calibration["motor_names"].index(name) - calib_mode = self.calibration["calib_mode"][calib_idx] - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - drive_mode = self.calibration["drive_mode"][calib_idx] - homing_offset = self.calibration["homing_offset"][calib_idx] - _, model = self.motors[name] - resolution = self.model_resolution[model] - - # Update direction of rotation of the motor to match between leader and follower. - # In fact, the motor of the leader for a given joint can be assembled in an - # opposite direction in term of rotation than the motor of the follower on the same joint. - if drive_mode: - values[i] *= -1 - - # Convert from range [-2**31, 2**31[ to - # nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[) - values[i] += homing_offset - - # Convert from range ]-resolution, resolution[ to - # universal float32 centered degree range ]-180, 180[ - values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE - - if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE): - raise JointOutOfRangeError( - f"Wrong motor position range detected for {name}. " - f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), " - f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, " - f"but present value is {values[i]} degree. " - "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. " - "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`" - ) - - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - start_pos = self.calibration["start_pos"][calib_idx] - end_pos = self.calibration["end_pos"][calib_idx] - - # Rescale the present position to a nominal range [0, 100] %, - # useful for joints with linear motions like Aloha gripper - values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100 - - if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR): - raise JointOutOfRangeError( - f"Wrong motor position range detected for {name}. " - f"Expected to be in nominal range of [0, 100] % (a full linear translation), " - f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, " - f"but present value is {values[i]} %. " - "This might be due to a cable connection issue creating an artificial jump in motor values. " - "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`" - ) - - return values - - def autocorrect_calibration( - self, values: np.ndarray | list, motor_names: list[str] | None - ): - """This function automatically detects issues with values of motors after calibration, and correct for these issues. - - Some motors might have values outside of expected maximum bounds after calibration. - For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given - a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position. - - Known issues: - #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors. - #2: Motor internal homing offset is shifted of a full turn, caused by using default calibration (e.g Aloha). - #3: motor internal homing offset is shifted of less or more than a full turn, caused by using default calibration - or by human error during manual calibration. - - Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn. - Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`, - that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue. - - Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. - """ - if motor_names is None: - motor_names = self.motor_names - - # Convert from unsigned int32 original range [0, 2**32] to signed float32 range - values = values.astype(np.float32) - - for i, name in enumerate(motor_names): - calib_idx = self.calibration["motor_names"].index(name) - calib_mode = self.calibration["calib_mode"][calib_idx] - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - drive_mode = self.calibration["drive_mode"][calib_idx] - homing_offset = self.calibration["homing_offset"][calib_idx] - _, model = self.motors[name] - resolution = self.model_resolution[model] - - if drive_mode: - values[i] *= -1 - - # Convert from initial range to range [-180, 180] degrees - calib_val = ( - (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE - ) - in_range = (calib_val > LOWER_BOUND_DEGREE) and ( - calib_val < UPPER_BOUND_DEGREE - ) - - # Solve this inequality to find the factor to shift the range into [-180, 180] degrees - # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE - # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE - # (- HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= (HALF_TURN_DEGREE / 180 * (resolution // 2) - values[i] - homing_offset) / resolution - low_factor = ( - -HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - - values[i] - - homing_offset - ) / resolution - upp_factor = ( - HALF_TURN_DEGREE / HALF_TURN_DEGREE * (resolution // 2) - - values[i] - - homing_offset - ) / resolution - - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - start_pos = self.calibration["start_pos"][calib_idx] - end_pos = self.calibration["end_pos"][calib_idx] - - # Convert from initial range to range [0, 100] in % - calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100 - in_range = (calib_val > LOWER_BOUND_LINEAR) and ( - calib_val < UPPER_BOUND_LINEAR - ) - - # Solve this inequality to find the factor to shift the range into [0, 100] % - # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100 - # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 - # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100 - # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution - low_factor = (start_pos - values[i]) / resolution - upp_factor = (end_pos - values[i]) / resolution - - if not in_range: - # Get first integer between the two bounds - if low_factor < upp_factor: - factor = math.ceil(low_factor) - - if factor > upp_factor: - raise ValueError( - f"No integer found between bounds [{low_factor=}, {upp_factor=}]" - ) - else: - factor = math.ceil(upp_factor) - - if factor > low_factor: - raise ValueError( - f"No integer found between bounds [{low_factor=}, {upp_factor=}]" - ) - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" - in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - out_of_range_str = ( - f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" - ) - in_range_str = ( - f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" - ) - - logging.warning( - f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, " - f"from '{out_of_range_str}' to '{in_range_str}'." - ) - - # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. - self.calibration["homing_offset"][calib_idx] += resolution * factor - - def revert_calibration( - self, values: np.ndarray | list, motor_names: list[str] | None - ): - """Inverse of `apply_calibration`.""" - if motor_names is None: - motor_names = self.motor_names - - for i, name in enumerate(motor_names): - calib_idx = self.calibration["motor_names"].index(name) - calib_mode = self.calibration["calib_mode"][calib_idx] - - if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: - drive_mode = self.calibration["drive_mode"][calib_idx] - homing_offset = self.calibration["homing_offset"][calib_idx] - _, model = self.motors[name] - resolution = self.model_resolution[model] - - # Convert from nominal 0-centered degree range [-180, 180] to - # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096) - values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2) - - # Substract the homing offsets to come back to actual motor range of values - # which can be arbitrary. - values[i] -= homing_offset - - # Remove drive mode, which is the rotation direction of the motor, to come back to - # actual motor rotation direction which can be arbitrary. - if drive_mode: - values[i] *= -1 - - elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: - start_pos = self.calibration["start_pos"][calib_idx] - end_pos = self.calibration["end_pos"][calib_idx] - - # Convert from nominal lnear range of [0, 100] % to - # actual motor range of values which can be arbitrary. - values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos - - values = np.round(values).astype(np.int32) - return values - - def avoid_rotation_reset(self, values, motor_names, data_name): - if data_name not in self.track_positions: - self.track_positions[data_name] = { - "prev": [None] * len(self.motor_names), - # Assume False at initialization - "below_zero": [False] * len(self.motor_names), - "above_max": [False] * len(self.motor_names), - } - - track = self.track_positions[data_name] - - if motor_names is None: - motor_names = self.motor_names - - for i, name in enumerate(motor_names): - idx = self.motor_names.index(name) - - if track["prev"][idx] is None: - track["prev"][idx] = values[i] - continue - - # Detect a full rotation occured - if abs(track["prev"][idx] - values[i]) > 2048: - # Position went below 0 and got reset to 4095 - if track["prev"][idx] < values[i]: - # So we set negative value by adding a full rotation - values[i] -= 4096 - - # Position went above 4095 and got reset to 0 - elif track["prev"][idx] > values[i]: - # So we add a full rotation - values[i] += 4096 - - track["prev"][idx] = values[i] - - return values - - def read_with_motor_ids( - self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY - ): - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - return_list = True - if not isinstance(motor_ids, list): - return_list = False - motor_ids = [motor_ids] - - assert_same_address(self.model_ctrl_table, self.motor_models, data_name) - addr, bytes = self.model_ctrl_table[motor_models[0]][data_name] - group = scs.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes) - for idx in motor_ids: - group.addParam(idx) - - for _ in range(num_retry): - comm = group.txRxPacket() - if comm == scs.COMM_SUCCESS: - break - - if comm != scs.COMM_SUCCESS: - raise ConnectionError( - f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " - f"{self.packet_handler.getTxRxResult(comm)}" - ) - - values = [] - for idx in motor_ids: - value = group.getData(idx, addr, bytes) - values.append(value) - - if return_list: - return values - else: - return values[0] - - def read(self, data_name, motor_names: str | list[str] | None = None): - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - if not self.is_connected: - raise RobotDeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - - motor_ids = [] - models = [] - for name in motor_names: - motor_idx, model = self.motors[name] - motor_ids.append(motor_idx) - models.append(model) - - assert_same_address(self.model_ctrl_table, models, data_name) - addr, bytes = self.model_ctrl_table[model][data_name] - group_key = get_group_sync_key(data_name, motor_names) - - if data_name not in self.group_readers: - # create new group reader - self.group_readers[group_key] = scs.GroupSyncRead( - self.port_handler, self.packet_handler, addr, bytes - ) - for idx in motor_ids: - self.group_readers[group_key].addParam(idx) - - for _ in range(NUM_READ_RETRY): - comm = self.group_readers[group_key].txRxPacket() - if comm == scs.COMM_SUCCESS: - break - - if comm != scs.COMM_SUCCESS: - raise ConnectionError( - f"Read failed due to communication error on port {self.port} for group_key {group_key}: " - f"{self.packet_handler.getTxRxResult(comm)}" - ) - - values = [] - for idx in motor_ids: - value = self.group_readers[group_key].getData(idx, addr, bytes) - values.append(value) - - values = np.array(values) - - # Convert to signed int to use range [-2048, 2048] for our motor positions. - if data_name in CONVERT_UINT32_TO_INT32_REQUIRED: - values = values.astype(np.int32) - - if data_name in CALIBRATION_REQUIRED: - values = self.avoid_rotation_reset(values, motor_names, data_name) - - if data_name in CALIBRATION_REQUIRED and self.calibration is not None: - values = self.apply_calibration_autocorrect(values, motor_names) - - # log the number of seconds it took to read the data from the motors - delta_ts_name = get_log_name( - "delta_timestamp_s", "read", data_name, motor_names - ) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # log the utc time at which the data was received - ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() - - return values - - def write_with_motor_ids( - self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY - ): - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - if not isinstance(motor_ids, list): - motor_ids = [motor_ids] - if not isinstance(values, list): - values = [values] - - assert_same_address(self.model_ctrl_table, motor_models, data_name) - addr, bytes = self.model_ctrl_table[motor_models[0]][data_name] - group = scs.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes) - for idx, value in zip(motor_ids, values, strict=True): - data = convert_to_bytes(value, bytes, self.mock) - group.addParam(idx, data) - - for _ in range(num_retry): - comm = group.txPacket() - if comm == scs.COMM_SUCCESS: - break - - if comm != scs.COMM_SUCCESS: - raise ConnectionError( - f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " - f"{self.packet_handler.getTxRxResult(comm)}" - ) - - def write( - self, - data_name, - values: int | float | np.ndarray, - motor_names: str | list[str] | None = None, - ): - if not self.is_connected: - raise RobotDeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if self.mock: - import tests.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - - if isinstance(values, (int, float, np.integer)): - values = [int(values)] * len(motor_names) - - values = np.array(values) - - motor_ids = [] - models = [] - for name in motor_names: - motor_idx, model = self.motors[name] - motor_ids.append(motor_idx) - models.append(model) - - if data_name in CALIBRATION_REQUIRED and self.calibration is not None: - values = self.revert_calibration(values, motor_names) - - values = values.tolist() - - assert_same_address(self.model_ctrl_table, models, data_name) - addr, bytes = self.model_ctrl_table[model][data_name] - group_key = get_group_sync_key(data_name, motor_names) - - init_group = data_name not in self.group_readers - if init_group: - self.group_writers[group_key] = scs.GroupSyncWrite( - self.port_handler, self.packet_handler, addr, bytes - ) - - for idx, value in zip(motor_ids, values, strict=True): - data = convert_to_bytes(value, bytes, self.mock) - if init_group: - self.group_writers[group_key].addParam(idx, data) - else: - self.group_writers[group_key].changeParam(idx, data) - - comm = self.group_writers[group_key].txPacket() - if comm != scs.COMM_SUCCESS: - raise ConnectionError( - f"Write failed due to communication error on port {self.port} for group_key {group_key}: " - f"{self.packet_handler.getTxRxResult(comm)}" - ) - - # log the number of seconds it took to write the data to the motors - delta_ts_name = get_log_name( - "delta_timestamp_s", "write", data_name, motor_names - ) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # TODO(rcadene): should we log the time before sending the write command? - # log the utc time when the write has been completed - ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() - - def disconnect(self): - if not self.is_connected: - raise RobotDeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." - ) - - if self.port_handler is not None: - self.port_handler.closePort() - self.port_handler = None - - self.packet_handler = None - self.group_readers = {} - self.group_writers = {} - self.is_connected = False - - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() diff --git a/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control.py b/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control.py deleted file mode 100644 index c4a3efa..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control.py +++ /dev/null @@ -1,120 +0,0 @@ -from pypot.feetech import FeetechSTS3215IO -import time -import numpy as np -from threading import Thread - - -class FeetechPWMControl: - def __init__(self, ids, init_pos_rad, usb_port="/dev/ttyACM0"): - self.io = FeetechSTS3215IO( - usb_port, - baudrate=1000000, - use_sync_read=True, - ) - self.ids = ids - - self.io.set_mode({id: 2 for id in self.ids}) - self.kps = np.ones(len(self.ids)) * 32 # default kp - - self.init_pos_rad = init_pos_rad - self.init_pos_deg = np.rad2deg(self.init_pos_rad) - - self.control_freq = 200 # Hz - self.goal_positions = [0] * len(self.ids) - self.goal_positions = self.init_pos_deg - self.present_positions = [0] * len(self.ids) - self.last_positions = [0] * len(self.ids) - self.present_speeds = [0] * len(self.ids) - self.speed_decimation = 2 - self.speed_decimation_index = 0 - self.last_t = time.time() - - Thread(target=self.update, daemon=True).start() - - def set_kps(self, kps): - self.kps = np.array(kps) - - def disable_torque(self): - self.io.set_mode({id: 0 for id in self.ids}) - self.io.disable_torque(self.ids) - - # def enable_torque(self): - # self.io.enable_torque(self.ids) - # self.io.set_mode({id: 2 for id in self.ids}) - - def freeze(self): - present_position = list(self.io.get_present_position(self.ids)) - print(present_position) - self.io.set_goal_position( - {id: present_position[i] for i, id in enumerate(self.ids)} - ) - self.io.set_mode({id: 0 for id in self.ids}) - self.io.enable_torque(self.ids) - - def update(self): - while True: - s = time.time() - try: - self.present_positions = self.io.get_present_position(self.ids) - # self.present_speeds = self.io.get_present_speed(self.ids) - except Exception as e: - print(e) - continue - - if len(self.present_positions) != len(self.ids): - print("ERROR : present_positions and ids do not have the same length") - time.sleep(1 / self.control_freq) - continue - errors = np.array(self.goal_positions) - np.array(self.present_positions) - - pwms = self.kps * errors - pwms = np.int16(pwms) - pwms = np.clip(pwms, -1000, 1000) - - pwm_magnitudes = abs(pwms) - - direction_bits = [1 if pwm >= 0 else 0 for pwm in pwms] - - goal_times = [ - (direction_bits[i] << 10) | pwm_magnitudes[i] - for i in range(len(pwm_magnitudes)) - ] - try: - self.io.set_goal_time({id: goal_times[i] for i, id in enumerate(self.ids)}) - except Exception as e: - print(e) - continue - - if self.speed_decimation_index % self.speed_decimation == 0: - self.present_speeds = ( - np.array(self.present_positions) - np.array(self.last_positions) - ) / (time.time() - self.last_t) - self.last_positions = np.array(self.present_positions).copy() - self.last_t = time.time() - - took = time.time() - s - # print("Took : ", np.around(took, 3), ". Budget : ", np.around(1/self.control_freq, 3), "diff : ", ((1/self.control_freq - took))) - # if (1 / self.control_freq - took) < 0: - # print( - # "Low level control budget exceded by ", - # np.around(took - 1 / self.control_freq, 3), - # ) - time.sleep(max(0, (1 / self.control_freq - took))) - self.speed_decimation_index += 1 - - -if __name__ == "__main__": - ids = [1, 2] - pwm_control = FeetechPWMControl(ids) - pwm_control.enable_torque() - pwm_control.set_kps([32, 32]) - - pwm_control.goal_positions = [-90, -90] - try: - while True: - pwm_control.goal_positions = [90, 90] - time.sleep(1) - pwm_control.goal_positions = [-90, -90] - time.sleep(1) - except KeyboardInterrupt: - pwm_control.disable_torque() diff --git a/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control_rustypot.py b/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control_rustypot.py deleted file mode 100644 index a9debdb..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/feetech_pwm_control_rustypot.py +++ /dev/null @@ -1,111 +0,0 @@ -import rustypot -import time -import numpy as np -from threading import Thread - - -class FeetechPWMControl: - def __init__(self, ids, init_pos_rad, usb_port="/dev/ttyACM0"): - self.io = rustypot.feetech(usb_port, 1000000) - - self.ids = ids - - self.io.set_mode(self.ids, 2) - self.kps = np.ones(len(self.ids)) * 32 # default kp - - self.init_pos_rad = init_pos_rad - self.init_pos_deg = np.rad2deg(self.init_pos_rad) - - self.control_freq = 100 # Hz - self.goal_positions = [0] * len(self.ids) - self.goal_positions = self.init_pos_deg - self.present_positions = [0] * len(self.ids) - self.last_positions = [0] * len(self.ids) - self.present_speeds = [0] * len(self.ids) - self.speed_decimation = 2 - self.speed_decimation_index = 0 - self.last_t = time.time() - - Thread(target=self.update, daemon=True).start() - - def set_kps(self, kps): - self.kps = np.array(kps) - - def disable_torque(self): - self.io.set_mode(self.ids, 0) - self.io.disable_torque(self.ids) - - def freeze(self): - present_position = list(self.io.read_present_position(self.ids)) - self.io.write_goal_position(self.ids, present_position) - self.io.set_mode(self.ids, 0) - - def update(self): - while True: - s = time.time() - try: - self.present_positions = np.rad2deg(self.io.read_present_position(self.ids)) - - # self.present_speeds = self.io.get_present_speed(self.ids) - except Exception as e: - print(e) - continue - - if len(self.present_positions) != len(self.ids): - print("ERROR : present_positions and ids do not have the same length") - time.sleep(1 / self.control_freq) - continue - errors = np.array(self.goal_positions) - np.array(self.present_positions) - - pwms = self.kps * errors - pwms = np.int16(pwms) - pwms = np.clip(pwms, -1000, 1000) - - pwm_magnitudes = abs(pwms) - - direction_bits = [1 if pwm >= 0 else 0 for pwm in pwms] - - goal_times = [ - (direction_bits[i] << 10) | pwm_magnitudes[i] - for i in range(len(pwm_magnitudes)) - ] - try: - self.io.set_goal_time(self.ids, goal_times) - # self.io.set_goal_time({id: goal_times[i] for i, id in enumerate(self.ids)}) - except Exception as e: - print(e) - continue - - if self.speed_decimation_index % self.speed_decimation == 0: - self.present_speeds = ( - np.array(self.present_positions) - np.array(self.last_positions) - ) / (time.time() - self.last_t) - self.last_positions = np.array(self.present_positions).copy() - self.last_t = time.time() - - took = time.time() - s - # print("Took : ", np.around(took, 3), ". Budget : ", np.around(1/self.control_freq, 3), "diff : ", ((1/self.control_freq - took))) - if (1 / self.control_freq - took) < 0: - print( - "Low level control budget exceded by ", - np.around(took - 1 / self.control_freq, 3), - ) - time.sleep(max(0, (1 / self.control_freq - took))) - self.speed_decimation_index += 1 - - -if __name__ == "__main__": - ids = [1, 2] - pwm_control = FeetechPWMControl(ids) - pwm_control.enable_torque() - pwm_control.set_kps([32, 32]) - - pwm_control.goal_positions = [-90, -90] - try: - while True: - pwm_control.goal_positions = [90, 90] - time.sleep(1) - pwm_control.goal_positions = [-90, -90] - time.sleep(1) - except KeyboardInterrupt: - pwm_control.disable_torque() diff --git a/mini_bdx_runtime/mini_bdx_runtime/hwi.py b/mini_bdx_runtime/mini_bdx_runtime/hwi.py deleted file mode 100644 index d49a7ee..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/hwi.py +++ /dev/null @@ -1,188 +0,0 @@ -import time -from typing import List - -import numpy as np - -from mini_bdx_runtime.io_330 import Dxl330IO - - -class HWI: - def __init__(self, usb_port="/dev/ttyUSB1", baudrate=3000000): - self.dxl_io = Dxl330IO(usb_port, baudrate=baudrate, use_sync_read=True) - self.joints = { - "right_hip_yaw": 10, - "right_hip_roll": 11, - "right_hip_pitch": 12, - "right_knee": 13, - "right_ankle": 14, - "left_hip_yaw": 20, - "left_hip_roll": 21, - "left_hip_pitch": 22, - "left_knee": 23, - "left_ankle": 24, - "neck_pitch": 30, - "head_pitch": 31, - "head_yaw": 32, - } - - self.init_pos = { - "right_hip_yaw": 0.001171696610228082, - "right_hip_roll": 0.006726989242258406, - "right_hip_pitch": 1.0129772861831692, - "right_knee": -1.4829304760981399, - "right_ankle": 0.6444901047812701, - "left_hip_yaw": -0.002853397830292128, - "left_hip_roll": 0.01626303761810685, - "left_hip_pitch": 1.0105624704499077, - "left_knee": -1.4865015965817336, - "left_ankle": 0.6504953719748071, - "neck_pitch": -0.17453292519943295, - "head_pitch": -0.17453292519943295, - "head_yaw": 0, - "left_antenna": 0, - "right_antenna": 0, - } - - # self.joints_offsets = { - # "right_hip_yaw": -0.02, - # "right_hip_roll": 0, - # "right_hip_pitch": 0, - # "right_knee": -0.05, - # "right_ankle": -0.1, - # "left_hip_yaw": -0.05, - # "left_hip_roll": 0.05, - # "left_hip_pitch": 0.02, - # "left_knee": -0.08, - # "left_ankle": -0.1, - # "neck_pitch": 0, - # "head_pitch": 0.3, - # "head_yaw": 0, - # # "left_antenna": 0, - # # "right_antenna": 0, - # } - - self.joints_offsets = { - "right_hip_yaw": 0.0, - "right_hip_roll": 0, - "right_hip_pitch": 0, - "right_knee": 0.0, - "right_ankle": 0.0, - "left_hip_yaw": 0.00, - "left_hip_roll": 0.00, - "left_hip_pitch": 0.00, - "left_knee": 0.00, - "left_ankle": 0.0, - "neck_pitch": 0, - "head_pitch": 0.0, - "head_yaw": 0, - # "left_antenna": 0, - # "right_antenna": 0, - } - - self.init_pos = { - joint: position - offset - for joint, position, offset in zip( - self.init_pos.keys(), - self.init_pos.values(), - self.joints_offsets.values(), - ) - } - - # current based position : 0x5 - self.dxl_io.set_operating_mode({id: 0x3 for id in self.joints.values()}) - - def set_pid(self, pid, joint_name): - self.dxl_io.set_pid_gain({self.joints[joint_name]: pid}) - - def set_pid_all(self, pid): - self.dxl_io.set_pid_gain({id: pid for id in self.joints.values()}) - - def set_low_torque(self): - self.dxl_io.set_pid_gain({id: [100, 0, 0] for id in self.joints.values()}) - - def set_high_torque(self): - # https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/#position-pid-gain80-82-84-feedforward-1st2nd-gains88-90 - # 128 P factor - # 16 D factor - self.dxl_io.set_pid_gain( - {id: [10 * 128, 0, int(0.5 * 16)] for id in self.joints.values()} - ) - for name in ["neck_pitch", "head_pitch", "head_yaw"]: - self.dxl_io.set_pid_gain({self.joints[name]: [150, 0, 0]}) - - def turn_on(self): - # self.set_low_torque() - self.dxl_io.enable_torque(self.joints.values()) - time.sleep(1) - # self.goto_zero() - self.set_position_all(self.init_pos) - time.sleep(1) - # self.set_high_torque() - - def turn_off(self): - self.dxl_io.disable_torque(self.joints.values()) - - def goto_zero(self): - goal_dict = {joint: 0 for joint in self.joints.keys()} - - self.set_position_all(goal_dict) - - def set_position_all(self, joints_positions): - """ - joints_positions is a dictionary with joint names as keys and joint positions as values - Warning: expects radians - """ - ids_positions = { - self.joints[joint]: np.rad2deg(-position) - for joint, position in joints_positions.items() - } - - # print(ids_positions) - self.dxl_io.set_goal_position(ids_positions) - - def set_position(self, joint_name, position): - self.dxl_io.set_goal_position({self.joints[joint_name]: np.rad2deg(-position)}) - - def get_present_current(self, joint_name): - return self.dxl_io.get_present_current([self.joints[joint_name]])[0] - - def get_voltage_all(self): - return self.dxl_io.get_present_input_voltage(self.joints.values()) - - def get_present_input_voltage(self, joint_name): - return self.dxl_io.get_present_input_voltage([self.joints[joint_name]])[0] - - def get_current_all(self): - return self.dxl_io.get_present_current(self.joints.values()) - - def get_goal_current(self, joint_name): - return self.dxl_io.get_goal_current([self.joints[joint_name]])[0] - - def get_current_limit(self, joint_name): - return self.dxl_io.get_current_limit([self.joints[joint_name]])[0] - - def get_present_positions(self): - present_position = list( - np.around( - np.deg2rad((self.dxl_io.get_present_position(self.joints.values()))), 3 - ) - ) - factor = np.ones(len(present_position)) * -1 - return present_position * factor - - def get_present_velocities(self, rad_s=True) -> List[float]: - """ - Returns the present velocities in rad/s or rev/min - """ - # rev/min - present_velocities = np.array( - self.dxl_io.get_present_velocity(self.joints.values()) - ) - if rad_s: - present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s - - factor = np.ones(len(present_velocities)) * -1 - return list(present_velocities * factor) - - def get_operating_modes(self): - return self.dxl_io.get_operating_mode(self.joints.values()) diff --git a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pwm_control.py b/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pwm_control.py deleted file mode 100644 index bcf809d..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pwm_control.py +++ /dev/null @@ -1,207 +0,0 @@ -import time -from typing import List - -import numpy as np - -# from pypot.feetech import FeetechSTS3215IO -# from mini_bdx_runtime.feetech_pwm_control import FeetechPWMControl -from mini_bdx_runtime.feetech_pwm_control_rustypot import FeetechPWMControl - - -class HWI: - def __init__(self, usb_port="/dev/ttyACM0"): - - # Order matters here - self.joints = { - "left_hip_yaw": 20, - "left_hip_roll": 21, - "left_hip_pitch": 22, - "left_knee": 23, - "left_ankle": 24, - "neck_pitch": 30, - "head_pitch": 31, - "head_yaw": 32, - "head_roll": 33, - # "left_antenna": None, - # "right_antenna": None, - "right_hip_yaw": 10, - "right_hip_roll": 11, - "right_hip_pitch": 12, - "right_knee": 13, - "right_ankle": 14, - } - - self.zero_pos = { - "left_hip_yaw": 0, - "left_hip_roll": 0, - "left_hip_pitch": 0, - "left_knee": 0, - "left_ankle": 0, - "neck_pitch": 0, - "head_pitch": 0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna":0, - # "right_antenna":0, - "right_hip_yaw": 0, - "right_hip_roll": 0, - "right_hip_pitch": 0, - "right_knee": 0, - "right_ankle": 0, - } - - self.init_pos = { - "left_hip_yaw": 0.002, - "left_hip_roll": 0.053, - "left_hip_pitch": -0.63, - "left_knee": 1.368, - "left_ankle": -0.784, - "neck_pitch": 0.0, - "head_pitch": 0.0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw": -0.003, - "right_hip_roll": -0.065, - "right_hip_pitch": 0.635, - "right_knee": 1.379, - "right_ankle": -0.796, - } - - # self.init_pos = self.zero_pos # TODO REMOVE - - # self.joints_offsets = { - # "left_hip_yaw": 0.07, - # "left_hip_roll": -0.1, - # "left_hip_pitch": 0.0, - # "left_knee": 0.05, - # "left_ankle": -0.1, - # "neck_pitch": 0.1, - # "head_pitch": 0.1, - # "head_yaw": 0, - # "head_roll": 0.1, - # # "left_antenna": 0, - # # "right_antenna": 0, - # "right_hip_yaw": -0.15, - # "right_hip_roll": 0.15, - # "right_hip_pitch": 0.05, - # "right_knee": -0.05, - # "right_ankle": -0.08, - # } - self.joints_offsets = { - "left_hip_yaw" : 0.084, - "left_hip_roll" : -0.112, - "left_hip_pitch" : -0.024, - "left_knee" : 0.005, - "left_ankle" : -0.09, - "neck_pitch" : 0.017, - "head_pitch" : 0.114, - "head_yaw" : -0.083, - "head_roll" : 0.072, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw" : -0.13499999999999998, - "right_hip_roll" : 0.205, - "right_hip_pitch" : 0.064, - "right_knee" : -0.027999999999999997, - "right_ankle" : -0.09799999999999999, - - } - - init_pos_with_offsets = { - joint: pos + self.joints_offsets[joint] - for joint, pos in self.init_pos.items() - } - - self.control = FeetechPWMControl( - ids=list(self.joints.values()), - init_pos_rad=list(init_pos_with_offsets.values()), - usb_port=usb_port, - ) - - self.kps = np.ones(len(self.joints)) * 32 # default kp - self.low_torque_kps = np.ones(len(self.joints)) * 8 # default kp - - def set_kps(self, kps): - self.kps = kps - self.control.set_kps(self.kps) - - def turn_on(self): - self.control.set_kps(self.low_torque_kps) - print("turn on : low KPS set") - time.sleep(1) - - self.set_position_all(self.init_pos) - print("turn on : init pos set") - - time.sleep(1) - - self.control.set_kps(self.kps) - print("turn on : high kps") - - def turn_off(self): - self.control.disable_torque() - - def freeze(self): - self.control.freeze() - - def set_position_all(self, joints_positions): - """ - joints_positions is a dictionary with joint names as keys and joint positions as values - Warning: expects radians - """ - ids_positions = { - self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint]) - for joint, position in joints_positions.items() - } - - self.control.goal_positions = list(ids_positions.values()) - - def get_present_positions(self, ignore=[]): - """ - Returns the present positions in radians - """ - - # present_positions = np.deg2rad( - # self.control.io.get_present_position(self.joints.values()) - # ) - - present_positions = np.deg2rad(self.control.present_positions) - present_positions = [ - pos - self.joints_offsets[joint] - for joint, pos in zip(self.joints.keys(), present_positions) - if joint not in ignore - ] - return np.array(np.around(present_positions, 3)) - - def get_present_velocities(self, rad_s=True, ignore=[]): - """ - Returns the present velocities in rad/s (default) or rev/min - """ - # deg/s - present_velocities = np.array(self.control.present_speeds) - present_velocities = [ - vel for joint, vel in zip(self.joints.keys(), present_velocities) if joint not in ignore - ] - # present_velocities = np.array( - # self.control.io.get_present_speed(self.joints.values()) - # ) - if rad_s: - present_velocities = np.deg2rad(present_velocities) # rad/s - return np.array(np.around(present_velocities, 3)) - - def get_present_voltages(self): - return np.array(self.control.io.get_present_voltage(self.joints.values())) * 0.1 - - # def get_present_velocities(self, rad_s=True): - # """ - # Returns the present velocities in rad/s (default) or rev/min - # """ - # # rev/min - # present_velocities = np.array( - # self.control.io.get_present_speed(self.joints.values()) - # ) - # if rad_s: - # present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s - # return np.array(np.around(present_velocities, 3)) diff --git a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pypot.py b/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pypot.py deleted file mode 100644 index 12b9938..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_pypot.py +++ /dev/null @@ -1,180 +0,0 @@ -import time -from typing import List - -import numpy as np - -from pypot.feetech import FeetechSTS3215IO - - -class HWI: - def __init__(self, usb_port="/dev/ttyACM0", baudrate=3000000): - self.dxl_io = FeetechSTS3215IO( - usb_port, - baudrate=1000000, - use_sync_read=True, - ) - self.joints = { - "left_hip_yaw": 20, - "left_hip_roll": 21, - "left_hip_pitch": 22, - "left_knee": 23, - "left_ankle": 24, - "neck_pitch": 30, - "head_pitch": 31, - "head_yaw": 32, - "head_roll": 33, - # "left_antenna": None, - # "right_antenna": None, - "right_hip_yaw": 10, - "right_hip_roll": 11, - "right_hip_pitch": 12, - "right_knee": 13, - "right_ankle": 14, - } - - self.zero_pos = { - "left_hip_yaw": 0, - "left_hip_roll": 0, - "left_hip_pitch": 0, - "left_knee": 0, - "left_ankle": 0, - "neck_pitch": 0, - "head_pitch": 0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna":0, - # "right_antenna":0, - "right_hip_yaw": 0, - "right_hip_roll": 0, - "right_hip_pitch": 0, - "right_knee": 0, - "right_ankle": 0, - } - - self.init_pos = { - "left_hip_yaw": 0.002, - "left_hip_roll": 0.053, - "left_hip_pitch": -0.63, - "left_knee": 1.368, - "left_ankle": -0.784, - "neck_pitch": 0.002, - "head_pitch": 0.0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw": -0.003, - "right_hip_roll": -0.065, - "right_hip_pitch": 0.635, - "right_knee": 1.379, - "right_ankle": -0.796, - } - - # self.init_pos = self.zero_pos # TODO REMOVE - - self.joints_offsets = { - "left_hip_yaw": 0.07, - "left_hip_roll": -0.1, - "left_hip_pitch": 0.0, - "left_knee": 0.05, - "left_ankle": -0.1, - "neck_pitch": 0.1, - "head_pitch": 0.1, - "head_yaw": 0, - "head_roll": 0.1, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw": -0.15, - "right_hip_roll": 0.07, - "right_hip_pitch": 0.05, - "right_knee": -0.05, - "right_ankle": -0.08, - } - - def set_pid(self, pid, joint_name): - # TODO - pass - - def set_pid_all(self, pid): - # TODO WEIRD THINGS HERE - # If I set 32 manually, it works, but if it comes from a variable, it doesn't. - # sometimes i get : ValueError: byte must be in range(0, 256) - # sometimes 32 becomes 76 somehow - # P = np.uint8(pid[0]) - # I = np.uint8(pid[1]) - # D = np.uint8(pid[2]) - - self.dxl_io.set_P_coefficient({id: 32 for id in self.joints.values()}) - self.dxl_io.set_I_coefficient({id: 0 for id in self.joints.values()}) - self.dxl_io.set_D_coefficient({id: 32 for id in self.joints.values()}) - - def get_pid_all(self): - Ps = self.dxl_io.get_P_coefficient(self.joints.values()) - Is = self.dxl_io.get_I_coefficient(self.joints.values()) - Ds = self.dxl_io.get_D_coefficient(self.joints.values()) - print("Ps", Ps) - print("Is", Is) - print("Ds", Ds) - - return Ps, Is, Ds - - def turn_on(self): - self.dxl_io.enable_torque(self.joints.values()) - self.dxl_io.set_mode({id: 0 for id in self.joints.values()}) - # self.dxl_io.set_mode - self.dxl_io.set_acceleration({id: 16 for id in self.joints.values()}) - - time.sleep(1) - - self.set_position_all(self.init_pos) - - time.sleep(1) - - for name, id in self.joints.items(): - if "neck" in name or "head" in name: - self.dxl_io.set_acceleration({id: 32}) - else: - self.dxl_io.set_acceleration({id: 254}) - - def turn_off(self): - self.dxl_io.disable_torque(self.joints.values()) - - def set_position_all(self, joints_positions): - """ - joints_positions is a dictionary with joint names as keys and joint positions as values - Warning: expects radians - """ - ids_positions = { - self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint]) - for joint, position in joints_positions.items() - } - - self.dxl_io.set_goal_position(ids_positions) - - def set_position(self, joint_name, position): - self.dxl_io.set_goal_position({self.joints[joint_name]: np.rad2deg(-position)}) - - def get_present_positions(self): - """ - Returns the present positions in radians - """ - present_positions = np.deg2rad( - self.dxl_io.get_present_position(self.joints.values()) - ) - present_positions = [ - pos - self.joints_offsets[joint] - for joint, pos in zip(self.joints.keys(), present_positions) - ] - return np.array(np.around(present_positions, 3)) - - def get_present_velocities(self, rad_s=True): - """ - Returns the present velocities in rad/s (default) or rev/min - """ - # rev/min - present_velocities = np.array( - self.dxl_io.get_present_speed(self.joints.values()) - ) - if rad_s: - present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s - return np.array(np.around(present_velocities, 3)) diff --git a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_sdk.py b/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_sdk.py deleted file mode 100644 index d55d72f..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/hwi_feetech_sdk.py +++ /dev/null @@ -1,139 +0,0 @@ -from mini_bdx_runtime.feetech import ( - FeetechMotorsBus, - convert_radians_to_steps, - convert_steps_to_radians, - configure, -) -import time -import numpy as np - -# Everything is in radians here - - -class HWI: - def __init__(self, usb_port="/dev/ttyACM0"): - self.motors = { - "right_hip_yaw": (10, "sts3215"), - "right_hip_roll": (11, "sts3215"), - "right_hip_pitch": (12, "sts3215"), - "right_knee": (13, "sts3215"), - "right_ankle": (14, "sts3215"), - "left_hip_yaw": (20, "sts3215"), - "left_hip_roll": (21, "sts3215"), - "left_hip_pitch": (22, "sts3215"), - "left_knee": (23, "sts3215"), - "left_ankle": (24, "sts3215"), - # "neck_pitch": (30, "sts3215"), - # "head_pitch": (31, "sts3215"), - # "head_yaw": (32, "sts3215"), - } - - self.joints_offsets = { - "right_hip_yaw": 0.0, - "right_hip_roll": 0.0, - "right_hip_pitch": 0.0, - "right_knee": 0.0, - "right_ankle": 0.0, - "left_hip_yaw": 0.0, - "left_hip_roll": 0.0, - "left_hip_pitch": 0.0, - "left_knee": 0.0, - "left_ankle": 0.0, - # "neck_pitch": 0.0, - # "head_pitch": 0.0, - # "head_yaw": 0.0, - } - - self.joints_sign = { - "right_hip_yaw": -1, - "right_hip_roll": -1, - "right_hip_pitch": 1, - "right_knee": -1, - "right_ankle": -1, - "left_hip_yaw": -1, - "left_hip_roll": -1, - "left_hip_pitch": 1, - "left_knee": -1, - "left_ankle": -1, - # "neck_pitch": -1, - # "head_pitch": -1, - # "head_yaw": -1, - } - - self.init_pos = { - "right_hip_yaw": 0.001171696610228082, - "right_hip_roll": 0.006726989242258406, - "right_hip_pitch": 1.0129772861831692, - "right_knee": -1.4829304760981399, - "right_ankle": 0.6444901047812701, - "left_hip_yaw": -0.002853397830292128, - "left_hip_roll": 0.01626303761810685, - "left_hip_pitch": -1.0105624704499077, - "left_knee": -1.4865015965817336, - "left_ankle": 0.6504953719748071, - # "neck_pitch": -0.17453292519943295, - # "head_pitch": -0.17453292519943295, - # "head_yaw": 0, - } - self.zero_pos = { - "right_hip_yaw": 0, - "right_hip_roll": 0, - "right_hip_pitch": 0, - "right_knee": 0, - "right_ankle": 0, - "left_hip_yaw": 0, - "left_hip_roll": 0, - "left_hip_pitch": 0, - "left_knee": 0, - "left_ankle": 0, - # "neck_pitch": 0, - # "head_pitch": 0, - # "head_yaw": 0, - } - self.baudrate = 1000000 - - self.motors_bus = FeetechMotorsBus(port=usb_port, motors=self.motors) - self.motors_bus.connect() - configure(self.motors_bus) - - # We add 180 to all dofs. They were mounted in a way that 180 degrees is the neutral position. - self.global_offset = np.deg2rad(180) - - def enable_torque(self): - self.motors_bus.write("Torque_Enable", 1) - - def disable_torque(self): - self.motors_bus.write("Torque_Enable", 0) - - def turn_on(self): - self.enable_torque() - self.set_position_all(self.init_pos) - # self.set_position_all(self.zero_pos) - time.sleep(1) - - def set_position_all(self, joints_positions: dict): - positions = np.array(list(joints_positions.values())) - signs = np.array(list(self.joints_sign.values())) - rads = positions * signs + self.global_offset - rads = list(rads) - - steps = convert_radians_to_steps(rads, ["sts3215"]) - self.motors_bus.write("Goal_Position", steps) - - def get_position_all(self): - - steps = self.motors_bus.read("Present_Position", self.motors) - rads = np.array(convert_steps_to_radians(steps, ["sts3215"])) - return rads - self.global_offset - # return rads * np.array(list(self.joints_sign.values()))# - self.global_offset - - -if __name__ == "__main__": - hwi = HWI() - hwi.turn_on() - - # hwi.disable_torque() - # time.sleep(1) - # while True: - # print(hwi.get_position_all()) - # time.sleep(0.1) diff --git a/mini_bdx_runtime/mini_bdx_runtime/io_330.py b/mini_bdx_runtime/mini_bdx_runtime/io_330.py deleted file mode 100644 index 9cc2557..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/io_330.py +++ /dev/null @@ -1,221 +0,0 @@ -import pypot.dynamixel.conversion as conv -from pypot.dynamixel.io.abstract_io import AbstractDxlIO, _DxlAccess, _DxlControl -from pypot.dynamixel.protocol import v2 as v2 - -max_pos = 4096 -max_deg = 360 -max_current = 1750 - - -def dxl_to_degree(value, model): - return round(((max_deg * float(value)) / (max_pos - 1)) - (max_deg / 2), 2) - - -def dxl_to_current(value, model): - if value > 0x7FFF: - value = value - 65536 - return value - - -def current_to_dxl(value, model): - if value < 0: - value = value + 65536 - return int(value) - - -def degree_to_dxl(value, model): - pos = int(round((max_pos - 1) * ((max_deg / 2 + float(value)) / max_deg), 0)) - pos = min(max(pos, 0), max_pos - 1) - - return pos - - -def dxl_to_velocity(value, model): - if value > 2 ** (4 * 8 - 1): - value = value - 2 ** (4 * 8) - return value * 0.229 - - -class Dxl330IO(AbstractDxlIO): - _protocol = v2 - - -controls = { - # EEPROM - "model": { - "address": 0x00, - "access": _DxlAccess.readonly, - "dxl_to_si": conv.dxl_to_model, - }, - "firmware": {"address": 0x026, "length": 1, "access": _DxlAccess.readonly}, - "id": { - "address": 0x07, - "length": 1, - "access": _DxlAccess.writeonly, - "setter_name": "change_id", - }, - "baudrate": { - "address": 0x08, - "length": 1, - "access": _DxlAccess.writeonly, - "setter_name": "change_baudrate", - "si_to_dxl": conv.baudrate_to_dxl, - }, - "return delay time": { - "address": 0x09, - "length": 1, - "dxl_to_si": conv.dxl_to_rdt, - "si_to_dxl": conv.rdt_to_dxl, - }, - "angle limit": { - "address": 0x30, - "nb_elem": 2, - "dxl_to_si": lambda value, model: ( - dxl_to_degree(value[0], model), - dxl_to_degree(value[1], model), - ), - "si_to_dxl": lambda value, model: ( - degree_to_dxl(value[0], model), - degree_to_dxl(value[1], model), - ), - }, - "control mode": { - "address": 0x10, - "length": 1, - "dxl_to_si": conv.dxl_to_control_mode, - "si_to_dxl": conv.control_mode_to_dxl, - }, - "operating mode": { - "address": 0xB, - "length": 1, - }, - "highest temperature limit": { - "address": 0x1F, - "length": 1, - "dxl_to_si": conv.dxl_to_temperature, - "si_to_dxl": conv.temperature_to_dxl, - }, - "voltage limit": { - "address": 0x20, - "length": 1, - "nb_elem": 2, - "dxl_to_si": lambda value, model: ( - conv.dxl_to_voltage(value[0], model), - conv.dxl_to_voltage(value[1], model), - ), - "si_to_dxl": lambda value, model: ( - conv.voltage_to_dxl(value[0], model), - conv.voltage_to_dxl(value[1], model), - ), - }, - "current limit": { - "address": 0x26, - "length": 2, - }, - # RAM - "torque_enable": { - "address": 0x40, - "length": 1, - "dxl_to_si": conv.dxl_to_bool, - "si_to_dxl": conv.bool_to_dxl, - "getter_name": "is_torque_enabled", - "setter_name": "_set_torque_enable", - }, - "LED": { - "address": 0x41, - "length": 1, - "dxl_to_si": conv.dxl_to_bool, - "si_to_dxl": conv.bool_to_dxl, - "setter_name": "_set_LED", - "getter_name": "is_led_on", - }, - "LED color": { - "address": 0x19, - "length": 1, - "dxl_to_si": conv.dxl_to_led_color, - "si_to_dxl": conv.led_color_to_dxl, - }, - "pid gain": { - "address": 0x50, - "length": 2, - "nb_elem": 3, - # "dxl_to_si": conv.dxl_to_pid, - # "si_to_dxl": conv.pid_to_dxl, - }, - "goal position": { - "address": 0x74, - "length": 4, - "dxl_to_si": dxl_to_degree, - "si_to_dxl": degree_to_dxl, - }, - "present velocity": { - "address": 0x80, - "length": 4, - "access": _DxlAccess.readonly, - "dxl_to_si": dxl_to_velocity, - }, - "present position": { - "address": 0x84, - "length": 4, - "access": _DxlAccess.readonly, - "dxl_to_si": dxl_to_degree, - }, - "present current": { - "address": 0x7E, - "length": 2, - "access": _DxlAccess.readonly, - "dxl_to_si": dxl_to_current, - }, - "goal current": { - "address": 0x66, - "length": 2, - "dxl_to_si": dxl_to_current, - "si_to_dxl": current_to_dxl, - }, - "present input voltage": { - "address": 0x90, - "length": 2, - "access": _DxlAccess.readonly, - "dxl_to_si": conv.dxl_to_voltage, - }, - "present temperature": { - "address": 0x92, - "length": 1, - "access": _DxlAccess.readonly, - "dxl_to_si": conv.dxl_to_temperature, - }, -} - - -def _add_control( - name, - address, - length=2, - nb_elem=1, - access=_DxlAccess.readwrite, - models=[ - "XL-330", - ], - dxl_to_si=lambda val, model: val, - si_to_dxl=lambda val, model: val, - getter_name=None, - setter_name=None, -): - control = _DxlControl( - name, - address, - length, - nb_elem, - access, - models, - dxl_to_si, - si_to_dxl, - getter_name, - setter_name, - ) - - Dxl330IO._generate_accessors(control) - - -for name, args in controls.items(): - _add_control(name, **args) diff --git a/mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py b/mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py deleted file mode 100644 index 9f6db31..0000000 --- a/mini_bdx_runtime/mini_bdx_runtime/rustypot_control_hwi.py +++ /dev/null @@ -1,214 +0,0 @@ -import time -from typing import List - -import numpy as np - -# from pypot.feetech import FeetechSTS3215IO -# from mini_bdx_runtime.feetech_pwm_control import FeetechPWMControl -# from mini_bdx_runtime.feetech_pwm_control_rustypot import FeetechPWMControl -import rustypot - - -class HWI: - def __init__(self, usb_port="/dev/ttyACM0"): - - # Order matters here - self.joints = { - "left_hip_yaw": 20, - "left_hip_roll": 21, - "left_hip_pitch": 22, - "left_knee": 23, - "left_ankle": 24, - "neck_pitch": 30, - "head_pitch": 31, - "head_yaw": 32, - "head_roll": 33, - # "left_antenna": None, - # "right_antenna": None, - "right_hip_yaw": 10, - "right_hip_roll": 11, - "right_hip_pitch": 12, - "right_knee": 13, - "right_ankle": 14, - } - - self.zero_pos = { - "left_hip_yaw": 0, - "left_hip_roll": 0, - "left_hip_pitch": 0, - "left_knee": 0, - "left_ankle": 0, - "neck_pitch": 0, - "head_pitch": 0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna":0, - # "right_antenna":0, - "right_hip_yaw": 0, - "right_hip_roll": 0, - "right_hip_pitch": 0, - "right_knee": 0, - "right_ankle": 0, - } - - self.init_pos = { - "left_hip_yaw": 0.002, - "left_hip_roll": 0.053, - "left_hip_pitch": -0.63, - "left_knee": 1.368, - "left_ankle": -0.784, - "neck_pitch": 0.0, - "head_pitch": 0.0, - "head_yaw": 0, - "head_roll": 0, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw": -0.003, - "right_hip_roll": -0.065, - "right_hip_pitch": 0.635, - "right_knee": 1.379, - "right_ankle": -0.796, - } - - # self.init_pos = self.zero_pos # TODO REMOVE - - # self.joints_offsets = { - # "left_hip_yaw": 0.07, - # "left_hip_roll": -0.1, - # "left_hip_pitch": 0.0, - # "left_knee": 0.05, - # "left_ankle": -0.1, - # "neck_pitch": 0.1, - # "head_pitch": 0.1, - # "head_yaw": 0, - # "head_roll": 0.1, - # # "left_antenna": 0, - # # "right_antenna": 0, - # "right_hip_yaw": -0.15, - # "right_hip_roll": 0.15, - # "right_hip_pitch": 0.05, - # "right_knee": -0.05, - # "right_ankle": -0.08, - # } - self.joints_offsets = { - "left_hip_yaw": 0.084, - "left_hip_roll": -0.112, - "left_hip_pitch": -0.024, - "left_knee": 0.005, - "left_ankle": -0.09, - "neck_pitch": 0.017, - "head_pitch": 0.114, - "head_yaw": -0.083, - "head_roll": 0.072, - # "left_antenna": 0, - # "right_antenna": 0, - "right_hip_yaw": -0.13499999999999998, - "right_hip_roll": 0.205, - "right_hip_pitch": 0.064, - "right_knee": -0.027999999999999997, - "right_ankle": -0.09799999999999999, - } - - init_pos_with_offsets = { - joint: np.rad2deg(pos + self.joints_offsets[joint]) - for joint, pos in self.init_pos.items() - } - - # self.control = FeetechPWMControl( - # ids=list(self.joints.values()), - # init_pos_rad=list(init_pos_with_offsets.values()), - # usb_port=usb_port, - # ) - - self.kps = np.ones(len(self.joints)) * 32 # default kp - self.low_torque_kps = np.ones(len(self.joints)) * 8 # default kp - - self.control = rustypot.FeetechController( - usb_port, 1000000, 100, list(self.joints.values()), list(self.kps), list(init_pos_with_offsets.values()) - ) - - def set_kps(self, kps): - self.kps = kps - self.control.set_new_kps(self.kps) - - def turn_on(self): - self.control.set_new_kps(self.low_torque_kps) - print("turn on : low KPS set") - time.sleep(1) - - self.set_position_all(self.init_pos) - print("turn on : init pos set") - - time.sleep(1) - - self.control.set_new_kps(self.kps) - print("turn on : high kps") - - def turn_off(self): - self.control.disable_torque() - - def freeze(self): - self.control.freeze() - - def set_position_all(self, joints_positions): - """ - joints_positions is a dictionary with joint names as keys and joint positions as values - Warning: expects radians - """ - ids_positions = { - self.joints[joint]: np.rad2deg(position + self.joints_offsets[joint]) - for joint, position in joints_positions.items() - } - - self.control.set_new_target(list(ids_positions.values())) - # self.control.goal_positions = list(ids_positions.values()) - - def get_present_positions(self, ignore=[]): - """ - Returns the present positions in radians - """ - - # present_positions = np.deg2rad( - # self.control.io.get_present_position(self.joints.values()) - # ) - - present_positions = np.deg2rad(self.control.get_present_position()) - present_positions = [ - pos - self.joints_offsets[joint] - for joint, pos in zip(self.joints.keys(), present_positions) - if joint not in ignore - ] - return np.array(np.around(present_positions, 3)) - - def get_present_velocities(self, rad_s=True, ignore=[]): - """ - Returns the present velocities in rad/s (default) or rev/min - """ - # deg/s - present_velocities = np.array(self.control.get_current_speed()) - present_velocities = [ - vel - for joint, vel in zip(self.joints.keys(), present_velocities) - if joint not in ignore - ] - # present_velocities = np.array( - # self.control.io.get_present_speed(self.joints.values()) - # ) - if rad_s: - present_velocities = np.deg2rad(present_velocities) # rad/s - return np.array(np.around(present_velocities, 3)) - - # def get_present_voltages(self): - # return np.array(self.control.io.get_present_voltage(self.joints.values())) * 0.1 - - # def get_present_velocities(self, rad_s=True): - # """ - # Returns the present velocities in rad/s (default) or rev/min - # """ - # # rev/min - # present_velocities = np.array( - # self.control.io.get_present_speed(self.joints.values()) - # ) - # if rad_s: - # present_velocities = (2 * np.pi * present_velocities) / 60 # rad/s - # return np.array(np.around(present_velocities, 3)) diff --git a/scripts/check_voltage.py b/scripts/check_voltage.py deleted file mode 100644 index cef2a86..0000000 --- a/scripts/check_voltage.py +++ /dev/null @@ -1,31 +0,0 @@ -from pypot.feetech import FeetechSTS3215IO - -io = FeetechSTS3215IO( - "/dev/ttyACM0", - baudrate=1000000, - use_sync_read=True, -) - -joints = { - "left_hip_yaw": 20, - "left_hip_roll": 21, - "left_hip_pitch": 22, - "left_knee": 23, - "left_ankle": 24, - "neck_pitch": 30, - "head_pitch": 31, - "head_yaw": 32, - "head_roll": 33, - # "left_antenna": None, - # "right_antenna": None, - "right_hip_yaw": 10, - "right_hip_roll": 11, - "right_hip_pitch": 12, - "right_knee": 13, - "right_ankle": 14, -} - - -voltages = io.get_present_voltage(list(joints.values())) -for i, name in enumerate(joints.keys()): - print(name, round(voltages[i] * 0.1, 2), "V") diff --git a/scripts/ear_servo_test.py b/scripts/ear_servo_test.py deleted file mode 100644 index 8c6f595..0000000 --- a/scripts/ear_servo_test.py +++ /dev/null @@ -1,35 +0,0 @@ -import numpy as np - -import RPi.GPIO as GPIO -import time -# from time import sleep - -GPIO.setwarnings(False) -GPIO.setmode(GPIO.BCM) -GPIO.setup(12, GPIO.OUT) -pwm = GPIO.PWM(12, 50) -pwm.start(0) # Start the servo with 0 duty cycle ( at 0 deg position ) - -# pwm.ChangeDutyCycle(0) -# time.sleep(1) - -# pwm.stop(0) # Stop the servo with 0 duty cycle ( at 0 deg position ) -# GPIO.cleanup() # Clean up all the ports we've used. - -start = time.time() -while True: - if time.time() - start > 3: - break - target_pwm = 10.0*np.sin(2*np.pi*1*time.time()) + 10 - pwm.ChangeDutyCycle(target_pwm) - time.sleep(0.02) - -# # Set up the PWM on pin #16 at 50Hz -# pwm.ChangeDutyCycle(5) # Tells the servo to turn to the left ( -90 deg position ) -# sleep(0.5) # Tells the servo to Delay for 5sec -# pwm.ChangeDutyCycle(7.5) # Tells the servo to turn to the neutral position ( at 0 deg position ) -# sleep(0.5) # Tells the servo to Delay for 5sec -# pwm.ChangeDutyCycle(10) # Tells the servo to turn to the right ( +90 deg position ) -# sleep(0.5) # Tells the servo to Delay for 5sec -pwm.stop(0) # Stop the servo with 0 duty cycle ( at 0 deg position ) -GPIO.cleanup() # Clean up all the ports we've used. diff --git a/scripts/head_puppet.py b/scripts/head_puppet.py index 613be22..424ae51 100644 --- a/scripts/head_puppet.py +++ b/scripts/head_puppet.py @@ -2,9 +2,6 @@ Sets up the robot in init position, you control the head with the xbox controller """ - -# from pypot.feetech import FeetechSTS3215IO -# from mini_bdx_runtime.hwi_feetech_pypot import HWI from mini_bdx_runtime.rustypot_position_hwi import HWI from mini_bdx_runtime.eyes import Eyes import time diff --git a/scripts/imu_client_raw.py b/scripts/imu_client_raw.py deleted file mode 100644 index 31e8b0f..0000000 --- a/scripts/imu_client_raw.py +++ /dev/null @@ -1,88 +0,0 @@ -import socket -import time -import numpy as np -import pickle -from queue import Queue -from threading import Thread -from scipy.spatial.transform import Rotation as R -from FramesViewer.viewer import Viewer - -from mini_bdx_runtime.rl_utils import quat_rotate_inverse - - -class IMUClient: - def __init__(self, host, port=1234, freq=30): - self.host = host - self.port = port - self.freq = freq - self.client_socket = socket.socket() - self.connected = False - while not self.connected: - try: - self.client_socket.connect((self.host, self.port)) - self.connected = True - except Exception as e: - print(e) - time.sleep(0.5) - self.imu_queue = Queue(maxsize=1) - self.last_imu = [0, 0, 0, 0] - - Thread(target=self.imu_worker, daemon=True).start() - - def imu_worker(self): - while True: - try: - data = self.client_socket.recv(1024) # receive response - data = pickle.loads(data) - - self.imu_queue.put(data) - except: - print("missed imu") - - time.sleep(1 / self.freq) - - def get_imu(self): - try: - self.last_imu = self.imu_queue.get(False) - except: - pass - - return self.last_imu - - -if __name__ == "__main__": - - client = IMUClient("192.168.248.253") - fv = Viewer() - fv.start() - pose = np.eye(4) - pose[:3, 3] = [0.1, 0.1, 0.1] - projected_gravities = [] - try: - while True: - quat = client.get_imu() - try: - rot_mat = R.from_quat(quat).as_matrix() - pose[:3, :3] = rot_mat - fv.pushFrame(pose, "aze") - - # euler = R.from_quat(quat).as_euler("xyz") - # euler[2] = 0 - # quat = R.from_euler("xyz", euler).as_quat() - - projected_gravity = quat_rotate_inverse(quat, [0, 0, -1]) - projected_gravities.append(projected_gravity) - except Exception as e: - print("error", e) - pass - time.sleep(1 / 30) - except KeyboardInterrupt: - pass - - # plot projected_gravities [[x, y, z], [x, y, z], ...] - - # from matplotlib import pyplot as plt - - # projected_gravities = np.array(projected_gravities) - # plt.plot(projected_gravities) - # plt.show() diff --git a/scripts/imu_server_raw.py b/scripts/imu_server_raw.py deleted file mode 100644 index 22f5810..0000000 --- a/scripts/imu_server_raw.py +++ /dev/null @@ -1,62 +0,0 @@ -import socket -import time -import pickle -from mini_bdx_runtime.raw_imu import Imu -from threading import Thread -import time - -import argparse - - -class IMUServer: - def __init__(self, imu=None): - self.host = "0.0.0.0" - self.port = 1234 - - self.server_socket = socket.socket() - self.server_socket.setsockopt( - socket.SOL_SOCKET, socket.SO_REUSEADDR, 1 - ) # enable address reuse - - self.server_socket.bind((self.host, self.port)) - - if imu is None: - self.imu = Imu(50, user_pitch_bias=args.pitch_bias) - else: - self.imu = imu - self.stop = False - - Thread(target=self.run, daemon=True).start() - - def run(self): - while not self.stop: - self.server_socket.listen(1) - conn, address = self.server_socket.accept() # accept new connection - print("Connection from: " + str(address)) - try: - while True: - data = self.imu.get_data() - data = pickle.dumps(data) - conn.send(data) # send data to the client - time.sleep(1 / 30) - except: - pass - - self.server_socket.close() - print("thread closed") - time.sleep(1) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("--pitch_bias", type=float, default=0, help="deg") - args = parser.parse_args() - imu_server = IMUServer() - try: - while True: - time.sleep(0.01) - except KeyboardInterrupt: - print("Closing server") - imu_server.stop = True - - time.sleep(2) diff --git a/scripts/led_test.py b/scripts/led_test.py deleted file mode 100644 index 1133157..0000000 --- a/scripts/led_test.py +++ /dev/null @@ -1,39 +0,0 @@ -import RPi.GPIO as GPIO #Importe la bibliothèque pour contrôler les GPIOs -import numpy as np -import time - -GPIO.setmode(GPIO.BCM) # Use physical pin numbering -# GPIO.setmode(GPIO.BOARD) #Définit le mode de numérotation (Board) -GPIO.setwarnings(False) #On désactive les messages d'alerte -LED = 24 #Définit le numéro du port GPIO qui alimente la led - -GPIO.setup(LED, GPIO.OUT) #Active le contrôle du GPIO - -# state = GPIO.input(LED) #Lit l'état actuel du GPIO, vrai si allumé, faux si éteint - -GPIO.output(LED, GPIO.HIGH) #On l'allume - -clignottement_time = 0.1 - -cli_s = time.time() -low = False -while True: - if not low and np.random.rand() > 0.99: - print("cli") - GPIO.output(LED, GPIO.LOW) - cli_s = time.time() - low = True - - if time.time() - cli_s > clignottement_time: - GPIO.output(LED, GPIO.HIGH) - low = False - - time.sleep(0.01) - - - - -# if state : #Si GPIO allumé -# GPIO.output(LED, GPIO.LOW) #On l’éteint -# else : #Sinon -# GPIO.output(LED, GPIO.HIGH) #On l'allume diff --git a/scripts/plot_imu_data.py b/scripts/plot_imu_data.py deleted file mode 100644 index de401e3..0000000 --- a/scripts/plot_imu_data.py +++ /dev/null @@ -1,14 +0,0 @@ -import pickle - -imu_data = pickle.load(open("imu_data.pkl", "rb")) - -gyros = imu_data["gyros"] -acceleros = imu_data["accels"] - -import matplotlib.pyplot as plt - -plt.plot(gyros) -plt.show() - -plt.plot(acceleros) -plt.show() \ No newline at end of file diff --git a/scripts/servo_test.py b/scripts/servo_test.py deleted file mode 100644 index 9252ff1..0000000 --- a/scripts/servo_test.py +++ /dev/null @@ -1,62 +0,0 @@ -import RPi.GPIO as GPIO -import time - -class ServoController: - def __init__(self, pin1, pin2): - self.pin1 = pin1 - self.pin2 = pin2 - - GPIO.setmode(GPIO.BCM) - GPIO.setup(self.pin1, GPIO.OUT) - GPIO.setup(self.pin2, GPIO.OUT) - - # Create PWM objects for both servos (50Hz frequency) - self.pwm1 = GPIO.PWM(self.pin1, 50) - self.pwm2 = GPIO.PWM(self.pin2, 50) - - self.pwm1.start(0) - self.pwm2.start(0) - - def map_input_to_angle(self, value): - """ - Maps an input range of [-1, 1] to an angle range of [0°, 180°]. - """ - return 90 + (value * 90) # Maps -1 to 0°, 0 to 90°, and 1 to 180° - - def set_position(self, servo, value): - """ - Moves the servo based on an input value in the range [-1, 1]. - :param servo: 1 for the first servo, 2 for the second servo - :param value: A float between -1 and 1 - """ - if -1 <= value <= 1: - angle = self.map_input_to_angle(value) - duty = 2 + (angle / 18) # Convert angle to duty cycle (1ms-2ms) - if servo == 1: - self.pwm1.ChangeDutyCycle(duty) - elif servo == 2: - self.pwm2.ChangeDutyCycle(duty) - else: - print("Invalid servo number!") - # time.sleep(0.3) # Allow time for movement - else: - print("Invalid input! Enter a value between -1 and 1.") - - def stop(self): - """Stops PWM and cleans up GPIO.""" - self.pwm1.stop() - self.pwm2.stop() - GPIO.cleanup() - -# Initialize the ServoController for GPIO12 and GPIO13 -servo_control = ServoController(pin1=12, pin2=13) - -try: - while True: - servo_num = int(input("Enter servo number (1 or 2): ")) - value = float(input("Enter position (-1 to 1): ")) - servo_control.set_position(servo_num, value) - -except KeyboardInterrupt: - print("Stopping servos...") - servo_control.stop() diff --git a/scripts/sound_test.py b/scripts/sound_test.py deleted file mode 100644 index 3b7a35f..0000000 --- a/scripts/sound_test.py +++ /dev/null @@ -1,75 +0,0 @@ -''' pg_midi_sound101.py -play midi music files (also mp3 files) using pygame -tested with Python273/331 and pygame192 by vegaseat -''' -#code modified by James DeVito from here: https://www.daniweb.com/programming/software-development/code/454835/let-pygame-play-your-midi-or-mp3-files - - -import sys -import pygame as pg -import os -import time - - -def play_music(music_file): - ''' - stream music with mixer.music module in blocking manner - this will stream the sound from disk while playing - ''' - clock = pg.time.Clock() - try: - pg.mixer.music.load(music_file) - print("Music file {} loaded!".format(music_file)) - except pygame.error: - print("File {} not found! {}".format(music_file, pg.get_error())) - return - - pg.mixer.music.play() - - # If you want to fade in the audio... - # for x in range(0,100): - # pg.mixer.music.set_volume(float(x)/100.0) - # time.sleep(.0075) - # # check if playback has finished - while pg.mixer.music.get_busy(): - clock.tick(30) - - -freq = 44100 # audio CD quality -bitsize = -16 # unsigned 16 bit -channels = 1 # 1 is mono, 2 is stereo -buffer = 2048 # number of samples (experiment to get right sound) -pg.mixer.init(freq, bitsize, channels, buffer) - - -if len(sys.argv) > 1: - - try: - user_volume = float(sys.argv[1]) - except ValueError: - print("Volume argument invalid. Please use a float (0.0 - 1.0)") - pg.mixer.music.fadeout(1000) - pg.mixer.music.stop() - raise SystemExit - - print("Playing at volume: " + str(user_volume)+ "\n") - pg.mixer.music.set_volume(user_volume) - mp3s = [] - for file in os.listdir("."): - if file.endswith(".wav"): - mp3s.append(file) - - print(mp3s) - - for x in mp3s: - try: - play_music(x) - time.sleep(.25) - except KeyboardInterrupt: - # if user hits Ctrl/C then exit - # (works only in console mode) - pg.mixer.music.fadeout(1000) - pg.mixer.music.stop() - raise SystemExit -else: - print("Please specify volume as a float! (0.0 - 1.0)") diff --git a/scripts/test_feet.py b/scripts/test_feet.py deleted file mode 100644 index a2952a8..0000000 --- a/scripts/test_feet.py +++ /dev/null @@ -1,15 +0,0 @@ -import RPi.GPIO as GPIO - -LEFT_FOOT_PIN = 22 -RIGHT_FOOT_PIN = 27 -GPIO.setwarnings(False) # Ignore warning for now -GPIO.setmode(GPIO.BCM) # Use physical pin numbering -GPIO.setup(LEFT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) -GPIO.setup(RIGHT_FOOT_PIN, GPIO.IN, pull_up_down=GPIO.PUD_UP) - -while True: # Run forever - print("=") - if GPIO.input(LEFT_FOOT_PIN) == GPIO.LOW: - print("LEFT!") - if GPIO.input(RIGHT_FOOT_PIN) == GPIO.LOW: - print("RIGHT! ") diff --git a/scripts/test_xbox_controller.py b/scripts/test_xbox_controller.py deleted file mode 100644 index 92d50d8..0000000 --- a/scripts/test_xbox_controller.py +++ /dev/null @@ -1,7 +0,0 @@ -from mini_bdx_runtime.xbox_controller import XBoxController -import time - -xbox = XBoxController(50) -while True: - print(xbox.get_last_command()) - time.sleep(0.1) \ No newline at end of file diff --git a/scripts/turn_off.py b/scripts/turn_off.py index 1a3bb91..55199c4 100644 --- a/scripts/turn_off.py +++ b/scripts/turn_off.py @@ -1,4 +1,3 @@ -# from mini_bdx_runtime.hwi_feetech_pypot import HWI from mini_bdx_runtime.rustypot_control_hwi import HWI hwi = HWI() diff --git a/scripts/turn_on.py b/scripts/turn_on.py index f8c2119..fe9de05 100644 --- a/scripts/turn_on.py +++ b/scripts/turn_on.py @@ -1,4 +1,3 @@ -# from mini_bdx_runtime.hwi_feetech_pypot import HWI from mini_bdx_runtime.rustypot_position_hwi import HWI import time diff --git a/scripts/v2_rl_walk_mujoco.py b/scripts/v2_rl_walk_mujoco.py index ce35f94..b514355 100644 --- a/scripts/v2_rl_walk_mujoco.py +++ b/scripts/v2_rl_walk_mujoco.py @@ -1,11 +1,8 @@ import time import pickle -# import pygame import numpy as np -# from mini_bdx_runtime.hwi_feetech_pwm_control import HWI -# from mini_bdx_runtime.rustypot_control_hwi import HWI from mini_bdx_runtime.rustypot_position_hwi import HWI from mini_bdx_runtime.onnx_infer import OnnxInfer from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter