diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 6fe2b1c..45a4065 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -18,15 +18,32 @@ RUN apt-get update && apt-get install -y \ # Clean out the apt lists after `apt-get update` && rm -rf /var/lib/apt/lists/* - #Install moteus python library https://github.com/mjbots/moteus/tree/main/lib/python RUN pip3 install moteus RUN pip3 install pyusb +# Pybind build capability for the RMD lib +RUN pip3 install pybind11 +RUN pip3 install pybind11-stubgen + # Update pydocstyle to remove a deprecation warning when testing for PEP257 RUN pip install --upgrade pydocstyle +# Add the RMD lib +RUN git clone https://github.com/2b-t/myactuator_rmd.git \ + && cd myactuator_rmd \ + # Build instructions taken from the github repo + && mkdir build \ + && cd build \ + && cmake .. -D PYTHON_BINDINGS=on \ + && make -j $(nproc) \ + && sudo make install \ + # Install python bindings + && pip3 install ../ + +RUN pybind11-stubgen myactuator_rmd_py -o /usr/local/lib/python3.10/dist-packages + # Add a user so we can remote into this container with a non-root user RUN useradd trickfire \ # Bash will be its default shell diff --git a/rmd_test/CMakeLists.txt b/rmd_test/CMakeLists.txt new file mode 100644 index 0000000..a0c64bf --- /dev/null +++ b/rmd_test/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.20) +project(your_project) + +find_package(myactuator_rmd REQUIRED) + +add_executable(your_node + main.cpp +) +target_link_libraries(your_node PUBLIC myactuator_rmd::myactuator_rmd) \ No newline at end of file diff --git a/rmd_test/main.cpp b/rmd_test/main.cpp new file mode 100644 index 0000000..6f2c117 --- /dev/null +++ b/rmd_test/main.cpp @@ -0,0 +1,15 @@ +#include +#include + +#include + + +int main() { + myactuator_rmd::CanDriver driver {"can0"}; + myactuator_rmd::ActuatorInterface actuator {driver, 1}; + + std::cout << actuator.getVersionDate() << std::endl; + std::cout << actuator.sendPositionAbsoluteSetpoint(180.0, 500.0) << std::endl; + actuator.shutdownMotor(); + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/src/can_moteus/can_moteus/moteus_motor.py b/src/can_moteus/can_moteus/moteus_motor.py index b2b5c67..752b374 100644 --- a/src/can_moteus/can_moteus/moteus_motor.py +++ b/src/can_moteus/can_moteus/moteus_motor.py @@ -8,7 +8,7 @@ from std_msgs.msg import String from lib.configs import MoteusMotorConfig -from lib.moteus_motor_state import MoteusMotorState, MoteusRunSettings +from lib.motor_state.moteus_motor_state import MoteusMotorState, MoteusRunSettings class MoteusMotor: diff --git a/src/lib/configs.py b/src/lib/configs.py index 77a6dce..1b6e0e2 100644 --- a/src/lib/configs.py +++ b/src/lib/configs.py @@ -4,14 +4,14 @@ """ import math -from dataclasses import dataclass +from dataclasses import dataclass, field from typing import Any -@dataclass(frozen=True) -class MoteusMotorConfig: +@dataclass(frozen=True, kw_only=True) +class MotorConfig: """ - A dataclass that contains config values relating to moteus motors connected via the can bus. + A dataclass that contains config values relating to motors connected via the can bus. """ can_id: int @@ -19,31 +19,72 @@ class MoteusMotorConfig: The can id of the motor. """ - config: dict[str, float | int] + motor_type: str = field(default="oh no", init=False) """ - The config of the motor. See: - https://github.com/mjbots/moteus/blob/main/docs/reference.md#c-configurable-values - - The `id.id` will be ignored. + The type of the motor. """ def getCanTopicName(self) -> str: """ Gets the motor's topic name for data sourced from can. - Returns the following format: `moteusmotor__from_can` + Returns the following format: `motor__from_can` """ - return f"moteusmotor_{self.can_id}_from_can" + return f"this_is_wrong_if_youre_seeing_this_{self.can_id}_from_can" def getInterfaceTopicName(self) -> str: """ Gets the motor's topic name for data sourced from robot interface. - Returns the following format: `moteusmotor__from_interface` + Returns the following format: `motor__from_interface` """ + return f"this_is_wrong_if_youre_seeing_this_{self.can_id}_from_interface" + + +@dataclass(frozen=True, kw_only=True) +class MoteusMotorConfig(MotorConfig): + """ + A dataclass that contains config values relating to moteus motors connected via the can bus. + """ + + config: dict[str, float | int] + """ + The config of the motor. See: + https://github.com/mjbots/moteus/blob/main/docs/reference.md#c-configurable-values + + The `id.id` will be ignored. + """ + + # Set the value of motor_type + def __post_init__(self) -> None: + # Can't use simple assignment since class is frozen + object.__setattr__(self, "motor_type", "moteus") + + def getCanTopicName(self) -> str: + return f"moteusmotor_{self.can_id}_from_can" + + def getInterfaceTopicName(self) -> str: return f"moteusmotor_{self.can_id}_from_interface" +@dataclass(frozen=True, kw_only=True) +class RMDX8MotorConfig(MotorConfig): + """ + A dataclass that contains config values relating to RMDX8 motors connected via the can bus. + """ + + # Set the value of motor_type + def __post_init__(self) -> None: + # Can't use simple assignment since class is frozen + object.__setattr__(self, "motor_type", "rmdx8") + + def getCanTopicName(self) -> str: + return f"rmdmotor_{self.can_id}_from_can" + + def getInterfaceTopicName(self) -> str: + return f"rmdmotor_{self.can_id}_from_interface" + + class MotorConfigs: """ A constants class that contains motor constants. @@ -180,7 +221,7 @@ def __setattr__(self, _: str, __: Any) -> Any: raise AttributeError("Trying to set attribute on a frozen instance") @classmethod - def getAllMotors(cls) -> list[MoteusMotorConfig]: + def getAllMotors(cls) -> list[MotorConfig]: """ Returns a list of every motor in this constants class. """ diff --git a/src/lib/interface/robot_info.py b/src/lib/interface/robot_info.py index ed8b10b..e06f4a2 100644 --- a/src/lib/interface/robot_info.py +++ b/src/lib/interface/robot_info.py @@ -3,12 +3,15 @@ motor on the robot. """ +from collections import defaultdict +from typing import Callable + from rclpy.node import Node from rclpy.subscription import Subscription from std_msgs.msg import String -from lib.configs import MoteusMotorConfig, MotorConfigs -from lib.moteus_motor_state import MoteusMotorState +from lib.configs import MotorConfig, MotorConfigs +from lib.motor_state.can_motor_state import CANMotorState class RobotInfo: # pylint: disable=too-few-public-methods @@ -19,19 +22,25 @@ class RobotInfo: # pylint: disable=too-few-public-methods def __init__(self, ros_node: Node): self._ros_node = ros_node self.sub_list: list[Subscription] = [] # empty array - self.can_id_to_json: dict[int, MoteusMotorState] = {} # Dict + self.can_id_to_json: defaultdict[str, dict[int, CANMotorState]] = defaultdict(dict) for motor_config in MotorConfigs.getAllMotors(): self._ros_node.create_subscription( - String, motor_config.getCanTopicName(), self._subCallback, 10 + String, + motor_config.getCanTopicName(), + self._createSubCallback(motor_config.motor_type), + 10, ) - self.can_id_to_json[motor_config.can_id] = MoteusMotorState() + self.can_id_to_json[motor_config.motor_type][motor_config.can_id] = CANMotorState() + + def _createSubCallback(self, motor_type: str) -> Callable[[String], None]: + def _subCallback(msg: String) -> None: + state = CANMotorState.fromJsonMsg(msg) + self.can_id_to_json[motor_type][state.can_id] = state - def _subCallback(self, msg: String) -> None: - state = MoteusMotorState.fromJsonMsg(msg) - self.can_id_to_json[state.can_id] = state + return _subCallback - def getMotorState(self, motor: MoteusMotorConfig) -> MoteusMotorState: + def getMotorState(self, motor: MotorConfig) -> CANMotorState: """ Gets the state of the motor with the given can_id. @@ -40,4 +49,4 @@ def getMotorState(self, motor: MoteusMotorConfig) -> MoteusMotorState: can_id: int The can id of the motor to get the state of. """ - return self.can_id_to_json[motor.can_id] + return self.can_id_to_json[motor.motor_type][motor.can_id] diff --git a/src/lib/interface/robot_interface.py b/src/lib/interface/robot_interface.py index ce6332d..1c091db 100644 --- a/src/lib/interface/robot_interface.py +++ b/src/lib/interface/robot_interface.py @@ -4,13 +4,14 @@ """ import math +from collections import defaultdict from rclpy.node import Node from rclpy.publisher import Publisher from std_msgs.msg import String -from lib.configs import MoteusMotorConfig, MotorConfigs -from lib.moteus_motor_state import MoteusRunSettings +from lib.configs import MotorConfig, MotorConfigs +from lib.motor_state.can_motor_state import CanMotorRunSettings REVS_TO_RADIANS = math.pi * 2 RADIANS_TO_REVS = 1 / REVS_TO_RADIANS @@ -23,81 +24,81 @@ class RobotInterface: def __init__(self, ros_node: Node) -> None: self._ros_node = ros_node - self._publishers: dict[int, Publisher] = {} + self._publishers: defaultdict[str, dict[int, Publisher]] = defaultdict(dict) for motor_config in MotorConfigs.getAllMotors(): - self._publishers[motor_config.can_id] = self._ros_node.create_publisher( - String, motor_config.getInterfaceTopicName(), 10 + self._publishers[motor_config.motor_type][motor_config.can_id] = ( + self._ros_node.create_publisher(String, motor_config.getInterfaceTopicName(), 10) ) - def runMotor(self, motor: MoteusMotorConfig, run_settings: MoteusRunSettings) -> None: + def runMotor(self, motor: MotorConfig, run_settings: CanMotorRunSettings) -> None: """ Runs the specified motor with the specified settings. Parameters ------- - motor : MoteusMotorConfig + motor : MotorConfig The config of the motor to run. - run_settings : MoteusRunSettings + run_settings : CanMotorRunSettings The settings to run the motor with. """ - self._publishers[motor.can_id].publish(run_settings.toMsg()) + self._publishers[motor.motor_type][motor.can_id].publish(run_settings.toMsg()) - def runMotorSpeed(self, motor: MoteusMotorConfig, target_radians_per_second: float) -> None: + def runMotorSpeed(self, motor: MotorConfig, target_radians_per_second: float) -> None: """ Runs the specified motor with the specified target speed. Parameters ------- - motor : MoteusMotorConfig + motor : MotorConfig The config of the motor to run. target_radians_per_second: float The target speed in radians per second to run the motor. """ self.runMotor( motor, - MoteusRunSettings( + CanMotorRunSettings( position=math.nan, velocity=target_radians_per_second * RADIANS_TO_REVS, set_stop=False, ), ) - def runMotorPosition(self, motor: MoteusMotorConfig, target_radians: float) -> None: + def runMotorPosition(self, motor: MotorConfig, target_radians: float) -> None: """ Runs the specified motor to reach the specified position. Parameters ------- - motor : MoteusMotorConfig + motor : MotorConfig The config of the motor to run. target_radians: float The target position in revolutions. """ self.runMotor( - motor, MoteusRunSettings(position=target_radians * RADIANS_TO_REVS, set_stop=False) + motor, CanMotorRunSettings(position=target_radians * RADIANS_TO_REVS, set_stop=False) ) - def stopMotor(self, motor: MoteusMotorConfig) -> None: + def stopMotor(self, motor: MotorConfig) -> None: """ Stops the specified motor. This will stop the motor in place, not make it go limp unlike `disableMotor()`. Parameters ------ - motor: MoteusMotorConfig + motor: MotorConfig The config of the motor to stop. """ - self.runMotor(motor, MoteusRunSettings(position=math.nan, velocity=0, set_stop=False)) + self.runMotor(motor, CanMotorRunSettings(position=math.nan, velocity=0, set_stop=False)) - def disableMotor(self, motor: MoteusMotorConfig) -> None: + def disableMotor(self, motor: MotorConfig) -> None: """ Disables the specified motor. This essentially turns off the motor, stopping all output and making it go limp. Parameters ------- - motor : MoteusMotorConfig + motor : MotorConfig The config of the motor to disable. """ - self.runMotor(motor, MoteusRunSettings(velocity=0, set_stop=True)) + self.runMotor(motor, CanMotorRunSettings(velocity=0, set_stop=True)) diff --git a/src/lib/motor_state/can_motor_state.py b/src/lib/motor_state/can_motor_state.py new file mode 100644 index 0000000..828bfcf --- /dev/null +++ b/src/lib/motor_state/can_motor_state.py @@ -0,0 +1,59 @@ +""" +This module contains objects representing a can motor state and settings for setting position +setpoint. +""" + +from dataclasses import dataclass + +from lib.json_msg import JsonMsg + + +@dataclass(frozen=True, kw_only=True) +class CANMotorState(JsonMsg["CANMotorState"]): + """ + A dataclass representing the state of the motor. + """ + + can_id: int = -1 + position: float | None = None + """ + Current position of the motor in revolutions. + """ + velocity: float | None = None + """ + Current velocity of the motor in revolutions per second. + """ + current: float | None = None + """ + Current of the motor in Amperes. + """ + temperature: float | None = None + """ + Current temperature of the controller in celcius. + """ + power: float | None = None + """ + Current power draw of the controller in watts. + """ + input_voltage: float | None = None + """ + Current voltage of the controller in volts. + """ + + +@dataclass(frozen=True, kw_only=True) +class CanMotorRunSettings(JsonMsg["CanMotorRunSettings"]): + """ + A dataclass representing the different settings while running a motor. + """ + + position: float | None = None + """ + Requested position of the motor in revolutions. + """ + velocity: float | None = None + """ + Requested velocity of the motor in revolutions per second. + """ + velocity_limit: float | None = None + set_stop: bool = True diff --git a/src/lib/moteus_motor_state.py b/src/lib/motor_state/moteus_motor_state.py similarity index 97% rename from src/lib/moteus_motor_state.py rename to src/lib/motor_state/moteus_motor_state.py index 681bd0d..add7724 100644 --- a/src/lib/moteus_motor_state.py +++ b/src/lib/motor_state/moteus_motor_state.py @@ -10,7 +10,7 @@ from lib.json_msg import JsonMsg -@dataclass(frozen=True) +@dataclass(frozen=True, kw_only=True) class MoteusMotorState(JsonMsg["MoteusMotorState"]): """ A dataclass representing the state of the moteus motor. @@ -70,7 +70,7 @@ def fromMoteusData(cls, can_id: int, moteus_result: Result) -> "MoteusMotorState return cls(**dict_) -@dataclass(frozen=True) +@dataclass(frozen=True, kw_only=True) class MoteusRunSettings(JsonMsg["MoteusRunSettings"]): """ A dataclass representing the different settings while running a moteus motor. diff --git a/src/lib/motor_state/rmd_motor_state.py b/src/lib/motor_state/rmd_motor_state.py new file mode 100644 index 0000000..f26c210 --- /dev/null +++ b/src/lib/motor_state/rmd_motor_state.py @@ -0,0 +1,56 @@ +""" +This module contains objects representing the RMD-X8 motor state and settings for `set_position`. +""" + +from dataclasses import dataclass + +from myactuator_rmd_py.actuator_state import MotorStatus1, MotorStatus2 + +from lib.json_msg import JsonMsg +from lib.motor_state.can_motor_state import CanMotorRunSettings, CANMotorState + +DEGS_TO_REVS = 1 / 360 + + +@dataclass(frozen=True, kw_only=True) +class RMDX8MotorState(CANMotorState, JsonMsg["RMDX8MotorState"]): + """ + A dataclass representing the state of the RMD-X8 motor. + """ + + acceleration: float | None = None + error_code: int | None = None + """ + Current error code reported by the motor. + """ + + @classmethod + def fromRMDX8Data( + cls, + can_id: int, + motor_status_1: MotorStatus1, + motor_status_2: MotorStatus2, + power: float, + acceleration: float, + ) -> "RMDX8MotorState": + """ + Creates an RMDX8MotorState from the RMD-X8 data response. + """ + return cls( + can_id=can_id, + position=motor_status_2.shaft_angle * DEGS_TO_REVS, + velocity=motor_status_2.shaft_speed * DEGS_TO_REVS, + current=motor_status_2.current, + temperature=motor_status_1.temperature, + power=power, + input_voltage=motor_status_1.voltage, + acceleration=acceleration, + error_code=motor_status_1.error_code.value, + ) + + +@dataclass(frozen=True, kw_only=True) +class RMDX8RunSettings(CanMotorRunSettings, JsonMsg["RMDX8RunSettings"]): + """ + A dataclass representing the different settings while running an RMD-X8 motor. + """ diff --git a/src/mission_control_updater/mission_control_updater/info_to_json_helper.py b/src/mission_control_updater/mission_control_updater/info_to_json_helper.py index b537ac9..4318a65 100644 --- a/src/mission_control_updater/mission_control_updater/info_to_json_helper.py +++ b/src/mission_control_updater/mission_control_updater/info_to_json_helper.py @@ -1,6 +1,6 @@ import json -from lib.moteus_motor_state import MoteusMotorState +from lib.motor_state.moteus_motor_state import MoteusMotorState class InfoToJSONHelper: