From 54a4397d769ac4b00b2ff69f929af2c5515a36c9 Mon Sep 17 00:00:00 2001 From: Nathan DuPont Date: Sat, 30 Mar 2024 18:37:14 -0700 Subject: [PATCH] Adding Dynamixel hardware abstraction layer and binding to API --- .gitignore | 55 +++- MODULE.bazel.lock | 2 +- src/api/v1/api.py | 21 +- src/api/v1/robot.py | 405 +++++++++++++++++++++++++ src/robot/v1/README.md | 5 + src/robot/v1/{nodelete => __init__.py} | 0 src/robot/v1/publisher.py | 38 +++ src/robot/v1/robot_control.py | 141 +++++++++ src/robot/v1/subscriber.py | 34 +++ src/ui/v1/src/app/page.tsx | 35 +++ 10 files changed, 732 insertions(+), 4 deletions(-) create mode 100644 src/api/v1/robot.py create mode 100644 src/robot/v1/README.md rename src/robot/v1/{nodelete => __init__.py} (100%) create mode 100644 src/robot/v1/publisher.py create mode 100644 src/robot/v1/robot_control.py create mode 100644 src/robot/v1/subscriber.py diff --git a/.gitignore b/.gitignore index 3ea1b20..516c0db 100644 --- a/.gitignore +++ b/.gitignore @@ -416,4 +416,57 @@ cython_debug/ !crates/ruff_python_resolver/resources/test/airflow/venv/ !crates/ruff_python_resolver/resources/test/airflow/venv/lib !crates/ruff_python_resolver/resources/test/airflow/venv/lib/python3.11/site-packages/_watchdog_fsevents.cpython-311-darwin.so -!crates/ruff_python_resolver/resources/test/airflow/venv/lib/python3.11/site-packages/orjson/orjson.cpython-311-darwin.so \ No newline at end of file +!crates/ruff_python_resolver/resources/test/airflow/venv/lib/python3.11/site-packages/orjson/orjson.cpython-311-darwin.so + +# ROS gitignore +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE \ No newline at end of file diff --git a/MODULE.bazel.lock b/MODULE.bazel.lock index 6c6c56b..5b95294 100644 --- a/MODULE.bazel.lock +++ b/MODULE.bazel.lock @@ -1,6 +1,6 @@ { "lockFileVersion": 6, - "moduleFileHash": "4fa47c1d36ebfbfef45d2813f94b4ba9f3e8ebfef3d74978a732860e3c6b9f37", + "moduleFileHash": "9116b2d8fb0ce39849813bdeb4be5ffcf1ad8eabf88914f56b198ba6b8ec515d", "flags": { "cmdRegistries": [ "https://bcr.bazel.build/" diff --git a/src/api/v1/api.py b/src/api/v1/api.py index ce7e322..14368cd 100644 --- a/src/api/v1/api.py +++ b/src/api/v1/api.py @@ -1,8 +1,13 @@ -from typing import Union from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect from websockets.exceptions import ConnectionClosed from fastapi.templating import Jinja2Templates from fastapi.middleware.cors import CORSMiddleware +from robot import ( + Position2D, + RobotControl, + DynamixelMotor, + DYNAMIXEL_MX_12_ADDR_CONFIG, +) import asyncio import cv2 @@ -22,6 +27,13 @@ allow_headers=["*"], # Allow all headers ) +motors = [ + DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), +] +ctrl = RobotControl("/dev/ttyUSB0", 1, motors) +ctrl.init(1000000) + # https://stackoverflow.com/a/70626324 @app.websocket("/ws") @@ -43,5 +55,10 @@ async def get_stream(websocket: WebSocket): @app.post("/command") async def command(request: Request): data = await request.json() - print(data) + + x_val = -data.get("left", 0) + data.get("right", 0) + y_val = -data.get("down", 0) + data.get("up", 0) + + ctrl.set_velocity(Position2D(x_val, y_val, 0)) + return {"status": "success"} diff --git a/src/api/v1/robot.py b/src/api/v1/robot.py new file mode 100644 index 0000000..9f84c8e --- /dev/null +++ b/src/api/v1/robot.py @@ -0,0 +1,405 @@ +# from dynamixel_sdk import * +import math +from typing import Optional +import dynamixel_sdk as dynamixel +from dataclasses import dataclass +import logging + +logger = logging.getLogger(__file__) + + +@dataclass +class Vector2D: + x: float + y: float + + def __add__(self, other): + assert isinstance(other, Vector2D) + + return Vector2D(self.x + other.x, self.y + other.y) + + def __iadd__(self, other): + assert isinstance(other, Vector2D) + + self.x += other.x + self.y += other.y + + def __sub__(self, other): + assert isinstance(other, Vector2D) + + return Vector2D(self.x - other.x, self.y - other.y) + + def __isub__(self, other): + assert isinstance(other, Vector2D) + + self.x -= other.x + self.y -= other.y + + +@dataclass +class Rotation2D: + theta: float + + +@dataclass +class Position2D: + location: Vector2D + rotation: Rotation2D + + def __init__(self, x, y, theta): + self.location = Vector2D(x, y) + self.rotation = Rotation2D(theta) + + +@dataclass +class MemorySegment: + byte_offset: int + byte_size: int + + +@dataclass +class MotorAddressConfig: + torque_enable: MemorySegment + led_enable: MemorySegment + d_gain: MemorySegment + i_gain: MemorySegment + p_gain: MemorySegment + goal_position: MemorySegment + moving_speed: MemorySegment + torque_limit: MemorySegment + present_position: MemorySegment + present_speed: MemorySegment + present_load: MemorySegment + present_input_voltage: MemorySegment + present_temperature: MemorySegment + registered: MemorySegment + moving: MemorySegment + lock: MemorySegment + punch: MemorySegment + realtime_tick: MemorySegment + goal_acceleration: MemorySegment + + +DYNAMIXEL_MX_12_ADDR_CONFIG = MotorAddressConfig( + torque_enable=MemorySegment(24, 1), + led_enable=MemorySegment(25, 1), + d_gain=MemorySegment(26, 1), + i_gain=MemorySegment(27, 1), + p_gain=MemorySegment(28, 1), + goal_position=MemorySegment(30, 2), + moving_speed=MemorySegment(32, 2), + torque_limit=MemorySegment(34, 2), + present_position=MemorySegment(36, 2), + present_speed=MemorySegment(38, 2), + present_load=MemorySegment(40, 2), + present_input_voltage=MemorySegment(42, 1), + present_temperature=MemorySegment(43, 1), + registered=MemorySegment(44, 1), + moving=MemorySegment(46, 1), + lock=MemorySegment(47, 1), + punch=MemorySegment(48, 2), + realtime_tick=MemorySegment(50, 2), + goal_acceleration=MemorySegment(73, 1), +) + + +class DynamixelMotor: + def __init__( + self, + id: int, + address_config: MotorAddressConfig, + position: Position2D, + inverted: bool = False, + ): + self.id = id + self.address_config: MotorAddressConfig = address_config + self.position = position + self.inverted = inverted + self.port_handler = None + self.packet_handler = None + self.initialized = False + + def __validate_command(self): + if not self.initialized: + raise Exception("Motor needs to be initialized before commanding") + + def __write_bytes(self, offset: int, size: int, value: int): + if size == 1: + dxl_comm_result, dxl_error = self.packet_handler.write1ByteTxRx( + self.port_handler, self.id, offset, value + ) + elif size == 2: + dxl_comm_result, dxl_error = self.packet_handler.write2ByteTxRx( + self.port_handler, self.id, offset, value + ) + elif size == 4: + dxl_comm_result, dxl_error = self.packet_handler.write4ByteTxRx( + self.port_handler, self.id, offset, value + ) + else: + raise NotImplementedError + + if dxl_comm_result != dynamixel.COMM_SUCCESS: + logger.error( + f"FAILURE | DYNAMIXEL ID: '{self.id}' | WriteBytesDXLComm: '{self.packet_handler.getTxRxResult(dxl_comm_result)}'" + ) + return False + elif dxl_error != 0: + logger.error( + f"FAILURE | DYNAMIXEL ID: '{self.id}' | WriteBytesDXLError: '{self.packet_handler.getRxPacketError(dxl_error)}'" + ) + return False + return True + + def __read_bytes(self, offset: int, size: int) -> Optional[int]: + if size == 1: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read1ByteTxRx( + self.port_handler, self.id, offset + ) + elif size == 2: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read2ByteTxRx( + self.port_handler, self.id, offset + ) + elif size == 4: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read4ByteTxRx( + self.port_handler, self.id, offset + ) + else: + raise NotImplementedError + + if dxl_comm_result != dynamixel.COMM_SUCCESS: + logger.error( + f"FAILURE | DXID: '{self.id}' | ReadBytesDXLComm: '{self.packet_handler.getTxRxResult(dxl_comm_result)}'" + ) + return False + elif dxl_error != 0: + logger.error( + f"FAILURE | DXID: '{self.id}' | ReadBytesDXLError: '{self.packet_handler.getRxPacketError(dxl_error)}'" + ) + return None + return dxl_value + + def __get_memory_property(self, property: str) -> int: + self.__validate_command() + + value = self.__read_bytes( + getattr(self.address_config, property).byte_offset, + getattr(self.address_config, property).byte_size, + ) + + if value is None: + raise Exception(f"FATAL | DXID: '{self.id}' | Value could not be read!") + + return value + + def __set_memory_property(self, property: str, value: int): + self.__validate_command() + + self.__write_bytes( + getattr(self.address_config, property).byte_offset, + getattr(self.address_config, property).byte_size, + value, + ) + + def get_torque_enable(self) -> int: + return self.__get_memory_property("torque_enable") + + def set_torque_enable(self, is_enabled: int): + self.__set_memory_property("torque_enable", is_enabled) + + def get_led_enable(self) -> int: + return self.__get_memory_property("led_enable") + + def set_led_enable(self, is_enabled: int): + self.__set_memory_property("led_enable", is_enabled) + + def get_pid_gain(self) -> tuple[int, int, int]: + p = self.__get_memory_property("p_gain") + i = self.__get_memory_property("i_gain") + d = self.__get_memory_property("d_gain") + + return (p, i, d) + + def set_pid_gain(self, p: int, i: int, d: int): + self.__set_memory_property("p_gain", p) + self.__set_memory_property("i_gain", i) + self.__set_memory_property("d_gain", d) + + def get_goal_position(self) -> int: + return self.__get_memory_property("goal_position") + + def set_goal_position(self, position: int): + self.__set_memory_property("goal_position", position) + + def get_moving_speed(self) -> int: + return self.__get_memory_property("moving_speed") + + def set_moving_speed(self, speed: int): + if self.inverted: + if speed >= 1024: + speed -= 1024 + else: + speed += 1024 + + self.__set_memory_property("moving_speed", speed) + + def get_torque_limit(self) -> int: + return self.__get_memory_property("torque_limit") + + def set_torque_limit(self, limit: int): + self.__set_memory_property("torque_limit", limit) + + def get_present_position(self) -> int: + return self.__get_memory_property("present_position") + + def get_present_speed(self) -> int: + return self.__get_memory_property("present_speed") + + def get_present_load(self) -> int: + return self.__get_memory_property("present_load") + + def get_present_input_voltage(self) -> int: + return self.__get_memory_property("present_input_voltage") + + def get_present_temperature(self) -> int: + return self.__get_memory_property("present_temperature") + + def get_moving(self) -> int: + return self.__get_memory_property("moving") + + def get_realtime_tick(self) -> int: + return self.__get_memory_property("realtime_tick") + + def get_goal_acceleration(self) -> int: + return self.__get_memory_property("goal_acceleration") + + def set_goal_acceleration(self, acceleration: int): + self.__set_memory_property("goal_acceleration") + + def init(self, port_handler, packet_handler): + self.port_handler = port_handler + self.packet_handler = packet_handler + self.initialized = True + + def configure_default(self): + # Enable LED + self.set_led_enable(1) + + # Set default PID values intenral to the motor + self.set_pid_gain(8, 0, 8) + + # Enable the motor, and set to 100% max torque + self.set_torque_enable(1) + self.set_torque_limit(1023) + + # If we've made it this far without throwing an exception, + # we've configured our defaults successfully + logger.info( + f"SUCCESS | DYNAMIXEL ID: '{self.id}' | Default configuration applied!" + ) + + +class RobotControl: + def __init__( + self, + device_name: str, + protocol_version: int, + motors: list[DynamixelMotor], + ): + self.port_handler = dynamixel.PortHandler(device_name) + self.packet_handler = dynamixel.PacketHandler(protocol_version) + self.motors = motors + self.position: Vector2D = Vector2D(0, 0) + + self.__validate_motors() + + def __validate_motors(self): + # Confirm each motor has a unique ID + motor_ids = set([motor.id for motor in self.motors]) + + if len(motor_ids) != len(self.motors): + raise Exception("One or more motors have duplicate IDs defined!") + + def __open_port(self) -> bool: + try: + self.port_handler.openPort() + logger.info(f"SUCCESS | OpenPort") + except: + logger.error(f"FAILURE | OpenPort") + return False + return True + + def __set_baud(self, baud) -> bool: + """ + Set baud rate of the motor + + Args: + port_handler: Object responsible for handling port communications with the Dynamixel motors + + Returns: + bool: True if the command was sent successfully + """ + try: + self.port_handler.setBaudRate(baud) + logger.info(f"SUCCESS | SetBaud: '{baud}'") + except: + logger.error(f"FAILURE | SetBaud: '{baud}'") + return False + return True + + def init(self, baud): + if not self.__open_port(): + raise Exception("Unable to open port for Dynamixel!") + + if not self.__set_baud(baud): + raise Exception("Unable to set baud rate for Dynamixel!") + + for motor in self.motors: + motor.init(self.port_handler, self.packet_handler) + motor.configure_default() + + # TODO fix the verbiage here + def set_velocity(self, command: Position2D) -> bool: + """ + Translates a command of unit vectors and rotations into commands for + the motors on the robot. + + Args: + command: RobotCommand - Command of a unit vector and rotation with the + desired velocity of the robot. + """ + clamped_x = min(max(command.location.x, -1), 1) + clamped_y = min(max(command.location.y, -1), 1) + # TODO figure out how to get the smallest angle correctly + smallest_theta = ((command.rotation.theta + (math.pi / 2)) % math.pi) - ( + math.pi / 2 + ) + + for motor in self.motors: + # TODO actual kinematics here + if clamped_y < 0: + motor.set_moving_speed((abs(clamped_y) * 1023) + 1023) + else: + motor.set_moving_speed(clamped_y * 1023) + + return True + + +if __name__ == "__main__": + """ + NOTE: This isn't for actual execution. This entrypoint is only for + testing. + """ + # TODO set proper locations of the motors, once determined in CAD. + motors = [ + DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), + ] + + # Set up the robot controller, with the correct baud rate for the MX12 motors + ctrl = RobotControl("/dev/ttyUSB0", 1, motors) + ctrl.init(1000000) + + from IPython import embed + + embed() diff --git a/src/robot/v1/README.md b/src/robot/v1/README.md new file mode 100644 index 0000000..791fcc3 --- /dev/null +++ b/src/robot/v1/README.md @@ -0,0 +1,5 @@ +# Robot v1 + +This is our ROS workspace for the robot facing code. This will allow us to send commands to the robot. + +ROS_DOMAIN_ID=0 diff --git a/src/robot/v1/nodelete b/src/robot/v1/__init__.py similarity index 100% rename from src/robot/v1/nodelete rename to src/robot/v1/__init__.py diff --git a/src/robot/v1/publisher.py b/src/robot/v1/publisher.py new file mode 100644 index 0000000..10a37c5 --- /dev/null +++ b/src/robot/v1/publisher.py @@ -0,0 +1,38 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalPublisher(Node): + def __init__(self): + super().__init__("minimal_publisher") + self.publisher_ = self.create_publisher(String, "topic", 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = String() + msg.data = "Hello World: %d" % self.i + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/robot/v1/robot_control.py b/src/robot/v1/robot_control.py new file mode 100644 index 0000000..a08ee47 --- /dev/null +++ b/src/robot/v1/robot_control.py @@ -0,0 +1,141 @@ +# import os +# import rclpy +# from rclpy.node import Node +# from dynamixel_sdk import * +# from std_msgs.msg import String + + +# # Pre-init configuration when running +# if os.name == 'nt': +# import msvcrt +# def getch(): +# return msvcrt.getch().decode() +# else: +# import sys, tty, termios +# fd = sys.stdin.fileno() +# old_settings = termios.tcgetattr(fd) +# def getch(): +# try: +# tty.setraw(sys.stdin.fileno()) +# ch = sys.stdin.read(1) +# finally: +# termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) +# return ch + + +# class RobotSubscriber(Node): + +# def __init__(self): +# super().__init__('robot_scriber') +# self.subscription = self.create_subscription( +# String, +# 'topic', +# self.listener_callback, +# 10) +# self.subscription # prevent unused variable warning + +# def listener_callback(self, msg): +# self.get_logger().info('I heard: "%s"' % msg.data) + + +# def main(args=None): +# rclpy.init(args=args) + +# minimal_subscriber = MinimalSubscriber() + +# rclpy.spin(minimal_subscriber) + +# # Destroy the node explicitly +# # (optional - otherwise it will be done automatically +# # when the garbage collector destroys the node object) +# minimal_subscriber.destroy_node() +# rclpy.shutdown() + + +# if __name__ == '__main__': +# main() + +# def init() + + +# # Control table address +# ADDR_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model +# ADDR_GOAL_POSITION = 116 +# ADDR_PRESENT_POSITION = 132 + +# # Protocol version +# PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# # Default setting +# DXL_ID = 1 # Dynamixel ID : 1 +# BAUDRATE = 57600 # Dynamixel default baudrate : 57600 +# DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller +# # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +# TORQUE_ENABLE = 1 # Value for enabling the torque +# TORQUE_DISABLE = 0 # Value for disabling the torque +# DXL_MINIMUM_POSITION_VALUE = 0 # Dynamixel will rotate between this value +# DXL_MAXIMUM_POSITION_VALUE = 1000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +# DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +# portHandler = PortHandler(DEVICENAME) +# packetHandler = PacketHandler(PROTOCOL_VERSION) + +# def set_goal_pos_callback(data): +# print("Set Goal Position of ID %s = %s" % (data.id, data.position)) +# dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, data.id, ADDR_GOAL_POSITION, data.position) + +# def get_present_pos(req): +# dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, req.id, ADDR_PRESENT_POSITION) +# print("Present Position of ID %s = %s" % (req.id, dxl_present_position)) +# return dxl_present_position + +# def read_write_py_node(): +# rospy.init_node('read_write_py_node') +# rospy.Subscriber('set_position', SetPosition, set_goal_pos_callback) +# rospy.Service('get_position', GetPosition, get_present_pos) +# rospy.spin() + +# def main(): +# # Open port +# try: +# portHandler.openPort() +# print("Succeeded to open the port") +# except: +# print("Failed to open the port") +# print("Press any key to terminate...") +# getch() +# quit() + +# # Set port baudrate +# try: +# portHandler.setBaudRate(BAUDRATE) +# print("Succeeded to change the baudrate") +# except: +# print("Failed to change the baudrate") +# print("Press any key to terminate...") +# getch() +# quit() + +# # Enable Dynamixel Torque +# dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE) +# if dxl_comm_result != COMM_SUCCESS: +# print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) +# print("Press any key to terminate...") +# getch() +# quit() +# elif dxl_error != 0: +# print("%s" % packetHandler.getRxPacketError(dxl_error)) +# print("Press any key to terminate...") +# getch() +# quit() +# else: +# print("DYNAMIXEL has been successfully connected") + +# print("Ready to get & set Position.") + +# read_write_py_node() + + +# if __name__ == '__main__': +# main() diff --git a/src/robot/v1/subscriber.py b/src/robot/v1/subscriber.py new file mode 100644 index 0000000..dbf74f3 --- /dev/null +++ b/src/robot/v1/subscriber.py @@ -0,0 +1,34 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalSubscriber(Node): + def __init__(self): + super().__init__("minimal_subscriber") + self.subscription = self.create_subscription( + String, "MoveCommand", self.listener_callback, 10 + ) + self.subscription # prevent unused variable warning + + def listener_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg.data) + + +def main(args=None): + rclpy.init(args=args) + + minimal_subscriber = MinimalSubscriber() + + rclpy.spin(minimal_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/ui/v1/src/app/page.tsx b/src/ui/v1/src/app/page.tsx index 0513516..ce7f697 100644 --- a/src/ui/v1/src/app/page.tsx +++ b/src/ui/v1/src/app/page.tsx @@ -119,6 +119,41 @@ export default function Home() {
+ +
+ + + + + + + + +
+