diff --git a/ros2/.devcontainer/Dockerfile b/ros2/.devcontainer/Dockerfile index 90ab6cd3..1eba4b28 100644 --- a/ros2/.devcontainer/Dockerfile +++ b/ros2/.devcontainer/Dockerfile @@ -16,6 +16,36 @@ RUN apt-get update && apt-get install -y \ python3-vcstool \ && rm -rf /var/lib/apt/lists/* +# Install robotiq gripper +ARG ROS2_ROBOTIQ_GRIPPER_COMMIT_HASH="2ff85455d4b9f973c4b0bab1ce95fb09367f0d26" +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + mkdir tmp_build && \ + cd tmp_build && \ + git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git && \ + cd ros2_robotiq_gripper && git checkout ${ROS2_ROBOTIQ_GRIPPER_COMMIT_HASH} && cd .. && \ + sed -i 's/kGripperMaxSpeed = 0.150;/kGripperMaxSpeed = 1.0;/g' ros2_robotiq_gripper/robotiq_driver/src/hardware_interface.cpp && \ + vcs import . --input ros2_robotiq_gripper/ros2_robotiq_gripper.humble.repos && \ + apt-get update && \ + rosdep update && \ + rosdep install --from-paths . --ignore-src -r -y && \ + colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + cd .. && \ + rm -rf tmp_build && \ + rm -rf /var/lib/apt/lists/*" + +# Install rqt reconfigure from source (for array parameters) +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + mkdir -p /tmp/rqt_reconfigure && cd /tmp && \ + git clone https://github.com/ros-visualization/rqt_reconfigure.git --branch humble && \ + cd rqt_reconfigure && \ + apt-get update && \ + rosdep update && \ + rosdep install --from-paths . --ignore-src -r -y && \ + colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + cd .. && \ + rm -rf /tmp/rqt_reconfigure && \ + rm -rf /var/lib/apt/lists/*" + # Install libfranka ARG LIBFRANKA_VERSION=0.17.0 RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ @@ -44,21 +74,4 @@ RUN /bin/bash -c 'source /opt/ros/humble/setup.bash && \ cd .. && \ rm -rf /tmp/franka_ros2 && \ echo "source /opt/ros/humble/franka/setup.bash" >> ~/.bashrc && \ - echo "source /opt/ros/humble/franka/setup.sh" >> ~/.profile' - -# Install robotiq gripper -ARG ROS2_ROBOTIQ_GRIPPER_COMMIT_HASH="2ff85455d4b9f973c4b0bab1ce95fb09367f0d26" -RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ - mkdir tmp_build && \ - cd tmp_build && \ - git clone https://github.com/PickNikRobotics/ros2_robotiq_gripper.git && \ - cd ros2_robotiq_gripper && git checkout ${ROS2_ROBOTIQ_GRIPPER_COMMIT_HASH} && cd .. && \ - sed -i 's/kGripperMaxSpeed = 0.150;/kGripperMaxSpeed = 1.0;/g' ros2_robotiq_gripper/robotiq_driver/src/hardware_interface.cpp && \ - vcs import . --input ros2_robotiq_gripper/ros2_robotiq_gripper.humble.repos && \ - apt-get update && \ - rosdep update && \ - rosdep install --from-paths . --ignore-src -r -y && \ - colcon build --install-base /opt/ros/humble/franka --cmake-args -DCMAKE_BUILD_TYPE=Release && \ - cd .. && \ - rm -rf tmp_build && \ - rm -rf /var/lib/apt/lists/*" \ No newline at end of file + echo "source /opt/ros/humble/franka/setup.sh" >> ~/.profile' \ No newline at end of file diff --git a/ros2/.devcontainer/docker-compose.yml b/ros2/.devcontainer/docker-compose.yml index c9247bf8..beea6d00 100644 --- a/ros2/.devcontainer/docker-compose.yml +++ b/ros2/.devcontainer/docker-compose.yml @@ -7,4 +7,10 @@ services: volumes: - ../../:/workspace - /dev/serial/by-id:/dev/serial/by-id + - /tmp/.X11-unix:/tmp/.X11-unix:rw + - /tmp/.docker.xauth:/tmp/.docker.xauth:rw + environment: + DISPLAY: ${DISPLAY} + QT_X11_NO_MITSH: "1" + XAUTHORITY: /tmp/.docker.xauth \ No newline at end of file diff --git a/ros2/README.md b/ros2/README.md index 96dd10e7..1531551b 100644 --- a/ros2/README.md +++ b/ros2/README.md @@ -2,7 +2,7 @@ This folder contains all required ROS 2 packages for using GELLO. -## Packages +## Packages Overview ### 1. `franka_fr3_arm_controllers` This package provides a Joint Impedance controller for the Franka FR3. It subscribes to the GELLO joint states and sends torque commands to the robot. @@ -20,6 +20,7 @@ This package provides a ROS 2 node that reads input from the GELLO and publishes #### Key Features: - Publishes GELLO state to the `/gello/joint_states` topic. +- Optionally sets the internal control parameters of the Dynamixel motors. This allows for [virtual springs and dampers](#virtual-springs-dampers) in the GELLO joints. #### Launch Files: - **`main.launch.py`**: Launches the GELLO publisher node. @@ -35,17 +36,19 @@ This package provides a ROS 2 node for managing the gripper connected to the Fra - **`franka_gripper_client.launch.py`**: Launches the gripper manager node for the `Franka Hand`. - **`robotiq_gripper_controller_client.launch.py`**: Launches the gripper manager node for the `Robotiq 2F-85`. -## VS-Code Dev-Container +## Setup Environment + +### Option 1: VS-Code Dev-Container (recommended) We recommend working inside the provided VS-Code Dev-Container for a seamless development experience. Dev-Containers allow you to use a consistent environment with all necessary dependencies pre-installed. To start the Dev-Container, open the `ros2` sub-folder of this repository (not the entire `gello_software` folder) in VS Code. If prompted, select **"Reopen in Container"** to launch the workspace inside the Dev-Container. If you are not prompted, open the Command Palette (`Ctrl+Shift+P`) and select **"Dev Containers: Reopen in Container"**. Building the container for the first time will take a few minutes. For more information, refer to the [VS-Code Dev-Containers documentation](https://code.visualstudio.com/docs/devcontainers/containers). -If you choose not to use the Dev-Container, please refer to the [Local Setup](#local-setup) section below for manual installation instructions. +If you choose not to use the Dev-Container, please refer to the [Local Setup](#option-2-local-setup) section below for manual installation instructions. -## Local Setup +### Option 2: Local Setup -### Prerequisites +#### Prerequisites - **ROS 2 Humble Desktop** must be installed. See the [official installation guide](https://docs.ros.org/en/humble/Installation.html) for instructions. @@ -57,7 +60,7 @@ If you choose not to use the Dev-Container, please refer to the [Local Setup](#l > 💡 **Hint:** > You can also find example installation commands for `libfranka`, `franka_ros2`, and `ros2_robotiq_gripper` in the [Dockerfile](./.devcontainer/Dockerfile) located in the `ros2/.devcontainer` directory. These commands can be copy-pasted for your local setup. -### Further Dependency Installations +#### Further Dependency Installations After installing the prerequisites, you may need to install additional dependencies required by this workspace. For this you can run the `install_workspace_dependencies.bash` script. @@ -65,7 +68,10 @@ If you add new dependencies to your packages, remember to update the relevant `r ## Build and Test -### Building the Project +> ⚠️ **Important:** +> All commands for building and testing must be executed from the `ros2` directory of this repository. + +### Building the project To build the project, use the following `colcon` command with CMake arguments, required for clang-tidy: @@ -73,7 +79,7 @@ To build the project, use the following `colcon` command with CMake arguments, r colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCHECK_TIDY=ON ``` -### Testing the Project +### Testing the project The packages come with a set of tests, which can be executed using the following command: @@ -81,9 +87,6 @@ The packages come with a set of tests, which can be executed using the following colcon test ``` -> ⚠️ **Important:** -> All commands for building and testing must be executed from the `ros2` directory of this repository. - ## Getting Started ### 1. **Run the GELLO Publisher** @@ -105,20 +108,14 @@ In this case, the `GELLO_USB_ID` would be `/dev/serial/by-id/usb-FTDI_USB__-__Se #### Step 2: Configure your GELLO -If not done already, follow the instructions of the `Create the GELLO configuration and determining joint ID's` section in the main README.md. Use the provided script to configure the GELLO for Franka FR3: +If not done already, follow the instructions of the [`Create the GELLO configuration and determining joint ID's` section in the main README.md](../README.md#create-the-gello-configuration-and-determining-joint-ids). -```bash -python3 scripts/gello_get_offset.py \ ---start-joints 0 0 0 -1.57 0 1.57 0 \ ---joint-signs 1 1 1 1 1 -1 1 \ ---port /dev/serial/by-id/ -``` +Use the output of the `gello_get_offset.py` script to update the `best_offsets` and `gripper_range_rad` in the `/workspace/ros2/src/franka_gello_state_publisher/config/gello_config.yaml` file. -To apply your configuration: -- Update the `/workspace/src/franka_gello_state_publisher/config/gello_config.yaml` file. -- Rebuild the project to ensure the updated configuration is applied: +Rebuild the project to ensure the updated configuration is applied: ```bash +cd /workspace/ros2 colcon build ``` @@ -140,13 +137,39 @@ The `config_file` argument is **optional**. If not provided, it defaults to `exa - `joint_signs`: as used for calibration - `gripper`: true if Gello gripper state shall be used - `best_offsets` and `gripper_range_rad`: as determined with calibration routine +- Dynamixel control parameters: `dynamixel_...` (see below) + +**Virtual Springs and Dampers:** + +Each Dynamixel motor has an internal PID controller that can be configured to behave like a virtual spring and damper. Damping is useful to prevent the operator from moving the GELLO arm faster than the real robot can follow. The spring-like behavior allows to support individual joints that could sag to an unwanted position when having 7 degrees of freedom. + +This is done by setting the following parameters in the configuration file. Each parameter is an array of integers, where each value corresponds to a GELLO joint in ascending order. + + - `dynamixel_torque_enable`: Enable (1) / disable (0) torque of a joint. Must be enabled for the other control parameters to take effect. + - `dynamixel_goal_position`: Goal positions in pulses. 4095 = 360 degrees. Choose values that correspond to the desired joint angles. + - `dynamixel_kp_p`: Proportional gains. Determines the rotary spring-like behavior. Sensible values: 0 to ~1000. + - `dynamixel_kp_i`: Integral gains. Recommended to be 0 for all joints. + - `dynamixel_kd_d`: Derivative gains. Determines the damping behavior. Sensible values: 0 to ~1000. + +When the GELLO publisher is started, these parameters are used to configure the Dynamixel motors. The motors then operate in their internal "Current-based Position Control" mode with a current limit set to 600mA. Check the [Dynamixel documentation](https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/) for more information on the control parameters. + +> 💡 **Hint:** +> The example configuration files give a good starting point for these values: +> - springs in joints 1, 2, 4 and gripper. +> - damping in all joints, stronger for lower joints +> - `goal_position` values correspond to the calibration pose. +> +> You can also adjust these ROS2 parameters during runtime, e.g. using rqt with the `rqt_reconfigure` plugin. + +> 💡 **How strong are the virtual springs?** +> According to the Dynamixel XL330-M288 datasheet, the stall torque of the motors is 0.52 Nm (at 5.0 V, 1.47 A). However, continuous operation at this current will quickly overload and overheat the motors. Through testing, a current limit of 600 mA was found to be both safe and sufficient for our use case, corresponding to a maximum stall torque of approximately 0.21 Nm. Setting `dynamixel_kp_p` to 280 yields a spring constant of approximately 0.25 Nm/rad, which matches the physical torsion spring (McMaster-Carr 9271K53) used in the GELLO FR3 assembly — up to a deflection of about 48° where the current limit is reached. ### 2. **Launch the Joint Impedance Controller** Create a configuration file in `src/franka_fr3_arm_controllers/config/` or modify one of the provided example configuration files. Then launch the controller to send torque commands to the Franka robot: ```bash - ros2 launch franka_fr3_arm_controllers franka_fr3_arm_controllers.launch.py [robot_config_file:=your_config.yaml] +ros2 launch franka_fr3_arm_controllers franka_fr3_arm_controllers.launch.py [robot_config_file:=your_config.yaml] ``` The `robot_config_file` argument is **optional**. If not provided, it defaults to `example_fr3_config.yaml` in the `franka_fr3_arm_controllers/config/` directory. @@ -159,9 +182,9 @@ The `robot_config_file` argument is **optional**. If not provided, it defaults t Create a configuration file in `src/franka_gripper_manager/config/` or modify one of the provided example configuration files. Then launch the gripper manager node to control the gripper: - **For the Franka Hand**: - ```bash - ros2 launch franka_gripper_manager franka_gripper_client.launch.py [config_file:=your_config.yaml] - ``` + ```bash + ros2 launch franka_gripper_manager franka_gripper_client.launch.py [config_file:=your_config.yaml] + ``` - **For the Robotiq 2F-85**: ```bash @@ -188,6 +211,23 @@ The open com port could not be opened. Possible reasons are: The libfranka version and robot system version are not compatible. More information can be found [here](https://frankarobotics.github.io/docs/compatibility.html). Fix this by correcting the `LIBFRANKA_VERSION=0.15.0` in the [Dockerfile](./.devcontainer/Dockerfile) and update the `FRANKA_ROS2_VERSION` and `FRANKA_DESCRIPTION_VERSION` accordingly. +### The movement of the follower robot is slightly jerky + +If the movements of the follower robot do not feel smooth or you experience frequent force threshold errors, this may be related to high USB latency on your machine. To fix this, try the following (non-permanent) fix **on your host PC**: + +1. Check which `ttyUSBx`/`ttyACMx` is mapped to your U2D2 or OpenRB-150 device: `ls -la /dev/serial/by-id/` +2. Reduce the USB latency from the default 16ms to 1ms: `echo 1 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer` (replace `ttyUSB0` with your actual device) + +If this helps, you can add a permanent udev rule: +1. Create a new file `/etc/udev/rules.d/99-gello.rules` **on your host PC**: + ``` + # Lower latency_timer (1 instead of default 16 ms) & permission fix for U2D2 and OpenRB-150 devices + ACTION=="add", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6014" MODE="0666", ATTR{device/latency_timer}="1" + ACTION=="add", ATTRS{idVendor}=="2f5d", ATTRS{idProduct}=="2202" MODE="0666", ATTR{device/latency_timer}="1" + ``` + > Note: This also sets the device permissions (`MODE="0666"`), this part is optional. +2. Reload and trigger the new rules: `sudo udevadm control --reload-rules && sudo udevadm trigger` +3. Unplug and replug your U2D2 or OpenRB-150 devices ## Acknowledgements The source code for the Robotiq gripper control is based on diff --git a/ros2/requirements.txt b/ros2/requirements.txt index 16b0ef50..001b4f2f 100644 --- a/ros2/requirements.txt +++ b/ros2/requirements.txt @@ -1,2 +1,3 @@ dynamixel_sdk +PyYAML tyro \ No newline at end of file diff --git a/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp b/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp index 093867db..b286a66c 100644 --- a/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp +++ b/ros2/src/franka_fr3_arm_controllers/include/franka_fr3_arm_controllers/joint_impedance_controller.hpp @@ -53,6 +53,7 @@ class JointImpedanceController : public controller_interface::ControllerInterfac Vector7d d_gains_; double k_alpha_; bool move_to_start_position_finished_{false}; + bool motion_generator_initialized_{false}; rclcpp::Time start_time_; std::unique_ptr motion_generator_; rclcpp::Subscription::SharedPtr joint_state_subscriber_ = nullptr; @@ -62,7 +63,7 @@ class JointImpedanceController : public controller_interface::ControllerInterfac Vector7d calculateTauDGains_(const Vector7d& q_goal); bool validateGains_(const std::vector& gains, const std::string& gains_name); - void initializeMotionGenerator_(); + bool initializeMotionGenerator_(); void updateJointStates_(); void validateGelloPositions_(const sensor_msgs::msg::JointState& msg); void jointStateCallback_(const sensor_msgs::msg::JointState msg); diff --git a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp index 70cac900..f7f139ed 100644 --- a/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp +++ b/ros2/src/franka_fr3_arm_controllers/src/joint_impedance_controller.cpp @@ -55,7 +55,24 @@ controller_interface::return_type JointImpedanceController::update( Vector7d q_goal; Vector7d tau_d_calculated; + if (!motion_generator_initialized_) { + // After starting the controller we wait for valid joint states from the input topic + // Until we get valid joint states we will send zero torques to the robot + // to allow the user to reposition the robot + motion_generator_initialized_ = initializeMotionGenerator_(); + + if (!motion_generator_initialized_) { + for (int i = 0; i < num_joints; ++i) { + command_interfaces_[i].set_value(0.0); + } + + return controller_interface::return_type::OK; + } + } + if (!move_to_start_position_finished_) { + // We have received valid joint states and initialized the motion generator + // Now we move smoothly to the first joint position received from the input topic auto trajectory_time = this->get_node()->now() - start_time_; auto motion_generator_output = motion_generator_->getDesiredJointPositions(trajectory_time); move_to_start_position_finished_ = motion_generator_output.second; @@ -64,6 +81,8 @@ controller_interface::return_type JointImpedanceController::update( } if (move_to_start_position_finished_) { + // After reaching the start position we follow the joint position from the input topic + // This is the normal operation mode of the controller if (!gello_position_values_valid_) { RCLCPP_FATAL(get_node()->get_logger(), "Timeout: No valid joint states received from Gello"); rclcpp::shutdown(); // Exit the node permanently @@ -92,6 +111,7 @@ void JointImpedanceController::jointStateCallback_(const sensor_msgs::msg::Joint "Received joint state size is smaller than expected size."); return; } + std::copy(msg.position.begin(), msg.position.begin() + gello_position_values_.size(), gello_position_values_.begin()); @@ -165,8 +185,7 @@ CallbackReturn JointImpedanceController::on_configure( CallbackReturn JointImpedanceController::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - initializeMotionGenerator_(); - + last_joint_state_time_ = get_node()->now(); dq_filtered_.setZero(); start_time_ = this->get_node()->now(); @@ -225,12 +244,12 @@ void JointImpedanceController::updateJointStates_() { } } -void JointImpedanceController::initializeMotionGenerator_() { - last_joint_state_time_ = get_node()->now(); - while (!gello_position_values_valid_) { - RCLCPP_WARN(get_node()->get_logger(), "Waiting for valid joint states..."); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - last_joint_state_time_ = get_node()->now(); +bool JointImpedanceController::initializeMotionGenerator_() { + if (!gello_position_values_valid_) { + // Only send a warning once every 10 seconds in order not to spam the log + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000, + "Waiting for valid joint states..."); + return false; } Vector7d q_goal; @@ -243,6 +262,7 @@ void JointImpedanceController::initializeMotionGenerator_() { const double motion_generator_speed_factor = 0.2; motion_generator_ = std::make_unique(motion_generator_speed_factor, q_, q_goal); + return true; } } // namespace franka_fr3_arm_controllers diff --git a/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp b/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp index 9f8d1001..e79897ad 100644 --- a/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp +++ b/ros2/src/franka_fr3_arm_controllers/test/setup_test.cpp @@ -32,11 +32,13 @@ TEST_F(JointImpedanceControllerTest, TestActivate) { TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorOnly) { static const std::vector kInitialRobotPosition = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; static constexpr double kExpectedValue = -0.05; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); @@ -49,11 +51,13 @@ TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorOnly) { TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorAndGelloPositionValues) { static const std::vector kInitialRobotPosition = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; static constexpr double kExpectedValue = -0.05; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); @@ -66,11 +70,13 @@ TEST_F(JointImpedanceControllerTest, TestUpdateMotionGeneratorAndGelloPositionVa TEST_F(JointImpedanceControllerTest, TestUpdateGelloPositionValuesOnly) { static const std::vector kInitialRobotPosition = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; static constexpr double kExpectedValue = -0.075; - static constexpr double kTolerance = 1e-6; + static constexpr double kTolerance = 0.01; setRobotPosition(kInitialRobotPosition); startController(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + rclcpp::Time time; rclcpp::Duration period = rclcpp::Duration::from_seconds(1.0); EXPECT_EQ(controller_->update(time, period), controller_interface::return_type::OK); diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml index 6ea596fc..2b5e8abd 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_fr3_config.yaml @@ -1,8 +1,14 @@ robot1: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0" namespace: "" - num_joints: 7 + num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] - gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file + gripper_range_rad: [2.77856, 3.50931] + # Optional Dynamixel control parameters for all arm joints (+ gripper): + dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad + dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] + dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] + dynamixel_kp_d: [ 250, 100, 80, 60, 30, 10, 5, 0] \ No newline at end of file diff --git a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml b/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml index dc421cdb..9019d408 100644 --- a/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml +++ b/ros2/src/franka_gello_state_publisher/config/example_fr3_duo_config.yaml @@ -1,17 +1,29 @@ LEFT: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFSIO-if00-port0" namespace: "left" - num_joints: 7 + num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true best_offsets: [3.142, 3.142, 3.142, 4.712, 3.142, 4.712, 1.571] gripper_range_rad: [2.77856, 3.50931] + # Optional Dynamixel control parameters for all arm joints (+ gripper): + dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad + dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] + dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] + dynamixel_kp_d: [ 250, 100, 80, 60, 30, 10, 5, 0] RIGHT: com_port: "usb-FTDI_USB__-__Serial_Converter_FT9MFR6C-if00-port0" namespace: "right" - num_joints: 7 + num_arm_joints: 7 joint_signs: [1, 1, 1, 1, 1, -1, 1] gripper: true best_offsets: [3.142, 3.142, -3.142, 4.712, 3.142, 4.712, 1.571] - gripper_range_rad: [2.77856, 3.50931] \ No newline at end of file + gripper_range_rad: [2.77856, 3.50931] + # Optional Dynamixel control parameters for all arm joints (+ gripper): + dynamixel_torque_enable: [ 1, 1, 1, 1, 1, 1, 1, 1] # 1 on, 0 off + dynamixel_goal_position: [ 0.0, 0.0, 0.0,-1.571, 0.0, 1.571, 0.0, 3.509] # rad + dynamixel_kp_p: [ 30, 60, 0, 30, 0, 0, 0, 50] + dynamixel_kp_i: [ 0, 0, 0, 0, 0, 0, 0, 0] + dynamixel_kp_d: [ 250, 100, 80, 60, 30, 10, 5, 0] \ No newline at end of file diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md new file mode 100644 index 00000000..40806908 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/README.md @@ -0,0 +1,75 @@ +# Dynamixel Motor Configuration + +This directory contains the configuration-driven Dynamixel driver that supports multiple motor types through YAML configuration files. + +## Configuration File Structure + +Motor configurations are stored in the `motor_configs/` subdirectory, with one YAML file per motor type. The filename (without extension) is used as the motor type identifier. + +### File Structure: +``` +franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/ +├── driver.py +├── motor_configs/ +│ ├── xl330.yaml +│ ├── .yaml +│ └── ... +└── README.md +``` + +### Configuration File Format + +Each motor configuration file has the following structure: + +```yaml +# Motor configuration for +# Reference: https://example.com/motor/documentation + +pulses_per_revolution: 4095 # Motor-specific resolution +control_table: + parameter_name: + addr: 64 # Control table address + len: 1 # Data length in bytes + min: 0 # Optional: minimum value + max: 1 # Optional: maximum value + unit: "1 [mA]" # Optional: factor and unit (for reference) + read_only: true # Optional: if parameter is read-only + # ... more parameters +eeprom_area_end: 63 # Last address of the EEPROM area. Adresses above are in RAM area. +operating_modes: + 0: "Mode Name" + 1: "Another Mode" + # ... more modes +``` + +## Usage + +### Regular Driver + +```python +from gello.dynamixel.driver import DynamixelDriver + +driver = DynamixelDriver( + ids=[1, 2, 3], + port="/dev/ttyUSB0", + baudrate=57600, + motor_type="xl330" +) +``` + +### Fake Driver for Testing + +```python +from gello.dynamixel.driver import FakeDynamixelDriver + +fake_driver = FakeDynamixelDriver(ids=[1, 2, 3], motor_type="xl330") +``` + +## Adding New Motor Types + +To add support for a new motor type: + +1. Create a new YAML file in `motor_configs/` directory +2. Name the file after your motor type (e.g., `my_motor.yaml`) +3. Follow the format shown above +4. Use the filename (without extension) as the motor type in your driver diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/__init__.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py new file mode 100644 index 00000000..0c53b927 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/driver.py @@ -0,0 +1,346 @@ +import time +from pathlib import Path +from threading import Event, Lock, Thread +from typing import Protocol, Sequence + +import numpy as np +import yaml +from dynamixel_sdk.group_sync_read import GroupSyncRead +from dynamixel_sdk.group_sync_write import GroupSyncWrite +from dynamixel_sdk.packet_handler import PacketHandler +from dynamixel_sdk.port_handler import PortHandler +from dynamixel_sdk.robotis_def import COMM_SUCCESS + + +# Configuration loader for motor types +def load_motor_config(motor_type: str = "xl330") -> dict: + """ + Load motor configuration from YAML file. + + Parameters + ---------- + motor_type : str, optional + The type of motor to load configuration for (default is "xl330"). + + Returns + ------- + dict + The motor configuration dictionary. + + """ + config_dir = Path(__file__).parent / "motor_configs" + config_path = config_dir / f"{motor_type}.yaml" + + if not config_path.exists(): + available_types = [f.stem for f in config_dir.glob("*.yaml")] + raise ValueError( + f"Motor type '{motor_type}' not found. Available types: {available_types}" + ) + + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + return config + + +class DynamixelDriverProtocol(Protocol): + def get_joints(self) -> np.ndarray: + """ + Get the current joint angles in radians. + + Returns + ------- + np.ndarray + An array of joint angles. + + """ + ... + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + """ + Set a group of values by name. + + Parameters + ---------- + name : str + The name of the control parameter. + values : Sequence[int | None] + A list of values to set for each servo. + + """ + ... + + def read_value_by_name(self, name: str) -> Sequence[int]: + """ + Read a group of values by name. + + Parameters + ---------- + name : str + The name of the control parameter to read. + + Returns + ------- + Sequence[int] + A list of values read from the servos. + + """ + ... + + def close(self): + """Close the driver.""" + ... + + +class FakeDynamixelDriver(DynamixelDriverProtocol): + def __init__(self, ids: Sequence[int], motor_type: str = "xl330"): + self._ids = ids + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] + + self._storage_map = { + "goal_position": np.zeros(len(ids), dtype=int), + "goal_current": np.zeros(len(ids), dtype=int), + "kp_d": np.zeros(len(ids), dtype=int), + "kp_i": np.zeros(len(ids), dtype=int), + "kp_p": np.zeros(len(ids), dtype=int), + "operating_mode": np.zeros(len(ids), dtype=int), + "torque_enable": np.zeros(len(ids), dtype=int), + } + + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + def _rad_to_pulses(self, rad: float) -> int: + """Convert radians to pulses using motor-specific configuration.""" + return int(rad / (2 * np.pi) * self._pulses_per_revolution) + + def _set_group( + self, + name: str, + values: Sequence[int | None], + value_min: int | None = None, + value_max: int | None = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + self._storage_map[name][self._ids.index(dxl_id)] = value + print(f"Set {name} {value} for ID {dxl_id}", flush=True) + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), + ) + + def _read_group(self, name: str) -> Sequence[int]: + if name == "present_position": + name = "goal_position" + elif name == "present_current": + name = "goal_current" + if name not in self._storage_map: + raise ValueError(f"Register {name} not found.") + return self._storage_map[name].copy().tolist() + + def read_value_by_name(self, name: str) -> Sequence[int]: + return self._read_group(name) + + def get_joints(self) -> np.ndarray: + return self._pulses_to_rad(self._storage_map["goal_position"].copy()) + + def close(self): + pass + + +class DynamixelDriver(DynamixelDriverProtocol): + def __init__( + self, + ids: Sequence[int], + port: str = "/dev/ttyUSB0", + baudrate: int = 57600, + motor_type: str = "xl330", + ): + """ + Initialize the DynamixelDriver class. + + Parameters + ---------- + ids : Sequence[int] + A list of IDs for the Dynamixel servos. + port : str, optional + The USB port to connect to the arm (default is "/dev/ttyUSB0"). + baudrate : int, optional + The baudrate for communication (default is 57600). + motor_type : str, optional + The type of motor to use, e.g., "xl330" (default is "xl330"). + + """ + self._ids = ids + self._joint_angles = None + self._lock = Lock() + + # Load motor configuration + self._motor_config = load_motor_config(motor_type) + self._ctrl_table = self._motor_config["control_table"] + self._operating_modes = self._motor_config["operating_modes"] + self._pulses_per_revolution = self._motor_config["pulses_per_revolution"] + + # Initialize the port handler, packet handler, and group sync read/write + self._portHandler = PortHandler(port) + self._packetHandler = PacketHandler(2.0) + + # Create group sync read/write handlers for each control table entry + self._groupSyncReadHandlers = {} + self._groupSyncWriteHandlers = {} + for key, entry in self._ctrl_table.items(): + self._groupSyncReadHandlers[key] = GroupSyncRead( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) + # Add parameters for each Dynamixel servo to the group sync read + for dxl_id in self._ids: + if not self._groupSyncReadHandlers[key].addParam(dxl_id): + raise RuntimeError(f"Failed to add parameter for Dynamixel with ID {dxl_id}") + if "read_only" not in entry or not entry["read_only"]: + self._groupSyncWriteHandlers[key] = GroupSyncWrite( + self._portHandler, + self._packetHandler, + entry["addr"], + entry["len"], + ) + + # Open the port and set the baudrate + if not self._portHandler.openPort(): + raise RuntimeError("Failed to open the port") + + if not self._portHandler.setBaudRate(baudrate): + raise RuntimeError(f"Failed to change the baudrate, {baudrate}") + + # Disable torque for all Dynamixel servos + try: + self.write_value_by_name("torque_enable", [0] * len(self._ids)) + except Exception as e: + print(f"port: {port}, {e}") + + self._stop_thread = Event() + self._start_reading_thread() + + def _set_group( + self, + name: str, + values: Sequence[int | None], + groupSyncWriteHandler, + value_length: int, + value_min: int | None = None, + value_max: int | None = None, + ): + if len(values) != len(self._ids): + raise ValueError(f"The length of {name} must match the number of servos") + with self._lock: + for dxl_id, value in zip(self._ids, values): + if value is None: + continue + if value_min is not None: + value = max(value, value_min) + if value_max is not None: + value = min(value, value_max) + # Convert value to little-endian byte array of length value_length + param = [(value >> (8 * i)) & 0xFF for i in range(value_length)] + result = groupSyncWriteHandler.addParam(dxl_id, param) + if not result: + raise RuntimeError(f"Failed to set {name} for Dynamixel with ID {dxl_id}") + comm_result = groupSyncWriteHandler.txPacket() + if comm_result != COMM_SUCCESS: + raise RuntimeError(f"Failed to syncwrite {name}") + groupSyncWriteHandler.clearParam() + + def write_value_by_name(self, name: str, values: Sequence[int | None]): + self._set_group( + name=name, + values=values, + groupSyncWriteHandler=self._groupSyncWriteHandlers[name], + value_length=self._ctrl_table[name]["len"], + value_min=self._ctrl_table[name].get("min", None), + value_max=self._ctrl_table[name].get("max", None), + ) + + def _read_group(self, name: str, groupSyncReadHandler, value_length: int) -> list[int]: + with self._lock: + result = groupSyncReadHandler.txRxPacket() + if result != COMM_SUCCESS: + raise RuntimeError(f"Failed to sync read {name}, comm result: {result}") + values = [] + for dxl_id in self._ids: + if groupSyncReadHandler.isAvailable( + dxl_id, self._ctrl_table[name]["addr"], value_length + ): + value = groupSyncReadHandler.getData( + dxl_id, self._ctrl_table[name]["addr"], value_length + ) + value = int(np.int32(np.uint32(value))) + values.append(value) + else: + raise RuntimeError(f"Failed to get {name} for Dynamixel with ID {dxl_id}") + return values + + def read_value_by_name(self, name: str) -> list[int]: + return self._read_group( + name=name, + groupSyncReadHandler=self._groupSyncReadHandlers[name], + value_length=self._ctrl_table[name]["len"], + ) + + def _start_reading_thread(self): + self._reading_thread = Thread(target=self._read_joint_angles_loop) + self._reading_thread.daemon = True + self._reading_thread.start() + + def _read_joint_angles_loop(self): + # Continuously read joint angles and update the joint_angles array + while not self._stop_thread.is_set(): + time.sleep(0.001) + try: + _joint_angles = self._read_group( + "present_position", + self._groupSyncReadHandlers["present_position"], + self._ctrl_table["present_position"]["len"], + ) + self._joint_angles = np.array(_joint_angles, dtype=int) + except RuntimeError as e: + print(f"warning, comm failed: {e}") + continue + + def get_joints(self) -> np.ndarray: + # Return a copy of the joint_angles array to avoid race conditions + while self._joint_angles is None: + time.sleep(0.1) + _j = self._joint_angles.copy() + return self._pulses_to_rad(_j) + + def close(self): + self._stop_thread.set() + self._reading_thread.join() + self._portHandler.closePort() + + def _pulses_to_rad(self, pulses) -> np.ndarray: + """Convert pulses to radians using motor-specific configuration.""" + return np.array(pulses) / self._pulses_per_revolution * 2 * np.pi + + def _rad_to_pulses(self, rad: float) -> int: + """Convert radians to pulses using motor-specific configuration.""" + return int(rad / (2 * np.pi) * self._pulses_per_revolution) diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/motor_configs/xl330.yaml b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/motor_configs/xl330.yaml new file mode 100644 index 00000000..3dd7902b --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/dynamixel/motor_configs/xl330.yaml @@ -0,0 +1,67 @@ +# Motor configuration for Dynamixel XL330-M288-T and XL330-M077-T +# Reference: https://emanual.robotis.com/docs/en/dxl/x/xl330-m288/ + +pulses_per_revolution: 4095 +control_table: + model_number: { addr: 0, len: 2, read_only: true } + model_information: { addr: 2, len: 4, read_only: true } + firmware_version: { addr: 6, len: 1, read_only: true } + id: { addr: 7, len: 1, min: 0, max: 252 } + baud_rate: { addr: 8, len: 1, min: 0, max: 6 } + return_delay_time: { addr: 9, len: 1, min: 0, max: 254, unit: "2 [μsec]" } + drive_mode: { addr: 10, len: 1, min: 0, max: 13 } + operating_mode: { addr: 11, len: 1, min: 0, max: 16 } + secondary_id: { addr: 12, len: 1, min: 0, max: 252 } + protocol_type: { addr: 13, len: 1, min: 2, max: 22 } + homing_offset: { addr: 20, len: 4, min: -1044479, max: 1044479, unit: "1 [pulse]" } + moving_threshold: { addr: 24, len: 4, min: 0, max: 1023, unit: "0.229 [rev/min]" } + temperature_limit: { addr: 31, len: 1, min: 0, max: 100, unit: "1 [°C]" } + max_voltage_limit: { addr: 32, len: 2, min: 31, max: 70, unit: "0.1 [V]" } + min_voltage_limit: { addr: 34, len: 2, min: 31, max: 70, unit: "0.1 [V]" } + pwm_limit: { addr: 36, len: 2, min: 0, max: 885, unit: "0.113 [%]" } + current_limit: { addr: 38, len: 2, min: 0, max: 1750, unit: "1 [mA]" } + velocity_limit: { addr: 44, len: 4, min: 0, max: 2047, unit: "0.229 [rev/min]" } + max_position_limit: { addr: 48, len: 4, min: 0, max: 4095, unit: "1 [pulse]" } + min_position_limit: { addr: 52, len: 4, min: 0, max: 4095, unit: "1 [pulse]" } + startup_configuration: { addr: 60, len: 1, min: 0, max: 3 } + pwm_slope: { addr: 62, len: 1, min: 1, max: 255, unit: "1.977 [mV/msec]" } + shutdown: { addr: 63, len: 1 } + torque_enable: { addr: 64, len: 1, min: 0, max: 1} + led: { addr: 65, len: 1, min: 0, max: 1 } + status_return_level: { addr: 68, len: 1, min: 0, max: 2 } + registered_instruction: { addr: 69, len: 1, min: 0, max: 1, read_only: true } + hardware_error_status: { addr: 70, len: 1, read_only: true } + velocity_i_gain: { addr: 76, len: 2, min: 0, max: 16383 } + velocity_p_gain: { addr: 78, len: 2, min: 0, max: 16383 } + kp_d: { addr: 80, len: 2, min: 0, max: 16383 } + kp_i: { addr: 82, len: 2, min: 0, max: 16383 } + kp_p: { addr: 84, len: 2, min: 0, max: 16383 } + feedforward_2nd_gain: { addr: 88, len: 2, min: 0, max: 16383 } + feedforward_1st_gain: { addr: 90, len: 2, min: 0, max: 16383 } + bus_watchdog: { addr: 98, len: 1, min: 1, max: 127, unit: "20 [msec]" } + goal_pwm: { addr: 100, len: 2 } + goal_current: { addr: 102, len: 2, unit: "1 [mA]"} + goal_velocity: { addr: 104, len: 4, unit: "0.229 [rev/min]" } + profile_acceleration: { addr: 108, len: 4, min: 0, max: 32767, unit: "214.577 [rev/min2]" } + profile_velocity: { addr: 112, len: 4, min: 0, max: 32767, unit: "0.229 [rev/min]" } + goal_position: { addr: 116, len: 4, unit: "1 [pulse]"} + realtime_tick: { addr: 120, len: 2, min: 0, max: 32767, unit: "1 [msec]", read_only: true } + moving: { addr: 122, len: 1, min: 0, max: 1, read_only: true } + moving_status: { addr: 123, len: 1, read_only: true } + present_pwm: { addr: 124, len: 2, read_only: true } + present_current: { addr: 126, len: 2, unit: "1 [mA]", read_only: true } + present_velocity: { addr: 128, len: 4, unit: "0.229 [rev/min]", read_only: true } + present_position: { addr: 132, len: 4, unit: "1 [pulse]", read_only: true } + velocity_trajectory: { addr: 136, len: 4, unit: "0.229 [rev/min]", read_only: true } + position_trajectory: { addr: 140, len: 4, unit: "1 [pulse]", read_only: true } + present_input_voltage: { addr: 144, len: 2, unit: "0.1 [V]", read_only: true } + present_temperature: { addr: 146, len: 1, unit: "1 [°C]", read_only: true } + backup_ready: { addr: 147, len: 1, min: 0, max: 1, read_only: true } +eeprom_area_end: 63 +operating_modes: + 0: "Current Control Mode" + 1: "Velocity Control Mode" + 3: "Position Control Mode (Default)" + 4: "Extended Position Control Mode" + 5: "Current-based Position Control Mode" + 6: "PWM Control Mode" diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py new file mode 100644 index 00000000..3fb6714a --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_hardware.py @@ -0,0 +1,157 @@ +import numpy as np +from dataclasses import dataclass, field +from typing import List, TypedDict, Iterator, Tuple +from franka_gello_state_publisher.dynamixel.driver import DynamixelDriver + + +class GelloHardwareParams(TypedDict): + """Type-safe parameter dictionary for GelloHardware initialization.""" + + com_port: str + gello_name: str + num_arm_joints: int + joint_signs: List[int] + gripper: bool + gripper_range_rad: List[float] + best_offsets: List[float] + dynamixel_kp_p: List[int] + dynamixel_kp_i: List[int] + dynamixel_kp_d: List[int] + dynamixel_torque_enable: List[int] + dynamixel_goal_position: List[float] + + +@dataclass +class DynamixelControlConfig: + """Tracks current dynamixel parameter state.""" + + kp_p: List[int] = field(default_factory=list) + kp_i: List[int] = field(default_factory=list) + kp_d: List[int] = field(default_factory=list) + torque_enable: List[int] = field(default_factory=list) + goal_position: List[int] = field(default_factory=list) + goal_current: List[int] = field(default_factory=list) + operating_mode: List[int] = field(default_factory=list) + + # Define the order for parameter updates + _UPDATE_ORDER = [ + "operating_mode", # resets kp_p, kp_i, kp_d, goal_current, goal_position + "goal_current", + "kp_p", + "kp_i", + "kp_d", + "torque_enable", # resets goal_position + "goal_position", + ] + + def __contains__(self, param_name: str) -> bool: + """Check if parameter exists in this configuration.""" + return hasattr(self, param_name) + + def __iter__(self) -> Iterator[Tuple[str, List[int]]]: + """Iterate through parameters in correct update order.""" + for param_name in self._UPDATE_ORDER: + if hasattr(self, param_name): + yield param_name, getattr(self, param_name) + + def __getitem__(self, param_name: str) -> List[int]: + """Enable dictionary-style access for getting values.""" + if not hasattr(self, param_name): + raise KeyError(f"Parameter '{param_name}' not found") + return getattr(self, param_name) + + def __setitem__(self, param_name: str, value: List[int]) -> None: + """Enable dictionary-style access for setting values.""" + if not hasattr(self, param_name): + raise KeyError(f"Parameter '{param_name}' not found") + setattr(self, param_name, value) + + +class GelloHardware: + """Hardware interface for GELLO teleoperation device.""" + + def __init__( + self, + hardware_config: GelloHardwareParams, + ) -> None: + self._com_port = hardware_config["com_port"] + self._gello_name = hardware_config["gello_name"] + self._num_arm_joints = hardware_config["num_arm_joints"] + self._joint_signs = np.array(hardware_config["joint_signs"]) + self._gripper = hardware_config["gripper"] + self._num_total_joints = self._num_arm_joints + (1 if self._gripper else 0) + self._gripper_range_rad = hardware_config["gripper_range_rad"] + self._best_offsets = np.array(hardware_config["best_offsets"]) + + self._initialize_driver() + + OPERATING_MODE = 5 # CURRENT_BASED_POSITION_MODE + CURRENT_LIMIT = 600 # mA + self._dynamixel_control_config = DynamixelControlConfig( + kp_p=hardware_config["dynamixel_kp_p"].copy(), + kp_i=hardware_config["dynamixel_kp_i"].copy(), + kp_d=hardware_config["dynamixel_kp_d"].copy(), + torque_enable=hardware_config["dynamixel_torque_enable"].copy(), + goal_position=self._goal_position_to_pulses( + hardware_config["dynamixel_goal_position"] + ).copy(), + goal_current=[CURRENT_LIMIT] * self._num_total_joints, + operating_mode=[OPERATING_MODE] * self._num_total_joints, + ) + + self._initialize_parameters() + + def _initialize_driver(self) -> None: + """Initialize dynamixel driver with joint IDs and port.""" + joint_ids = list(range(1, self._num_total_joints + 1)) + self._driver = DynamixelDriver(joint_ids, port=self._com_port, baudrate=57600) + + def _initialize_parameters(self) -> None: + """Write all dynamixel configuration parameters to hardware.""" + for param_name, param_value in self._dynamixel_control_config: + self._driver.write_value_by_name(param_name, param_value) + + def update_dynamixel_control_parameter( + self, param_name: str, param_value: list[float] | list[int] + ) -> None: + """Update a single dynamixel parameter and handle dependencies.""" + clean_name = param_name.replace("dynamixel_", "") + + if clean_name == "goal_position": + param_value = self._goal_position_to_pulses(param_value) + + self._dynamixel_control_config[clean_name] = param_value + self._driver.write_value_by_name(clean_name, self._dynamixel_control_config[clean_name]) + if clean_name == "torque_enable": + self._driver.write_value_by_name( + "goal_position", self._dynamixel_control_config["goal_position"] + ) + + def read_joint_states(self) -> tuple[np.ndarray, float]: + """Read current joint positions and gripper state.""" + gello_joints_raw = self._driver.get_joints() + gello_arm_joints_raw = gello_joints_raw[: self._num_arm_joints] + gello_arm_joints = (gello_arm_joints_raw - self._best_offsets) * self._joint_signs + + gripper_position = 0.0 + if self._gripper: + gripper_position = self._gripper_readout_to_percent(gello_joints_raw[-1]) + + return gello_arm_joints, gripper_position + + def disable_torque(self) -> None: + """Disable torque on all joints.""" + self._driver.write_value_by_name("torque_enable", [0] * len(self._driver._ids)) + + def _goal_position_to_pulses(self, goals: list[float]) -> list[int]: + """Convert goal positions from radians to dynamixel pulses.""" + arm_goals_raw = (goals[: self._num_arm_joints] * self._joint_signs) + self._best_offsets + goals_raw = np.append(arm_goals_raw, goals[-1]) if self._gripper else arm_goals_raw + return [self._driver._rad_to_pulses(rad) for rad in goals_raw] + + def _gripper_readout_to_percent(self, gripper_position: float) -> float: + """Convert gripper position to percentage (0-1).""" + gripper_position_percent = (gripper_position - self._gripper_range_rad[0]) / ( + self._gripper_range_rad[1] - self._gripper_range_rad[0] + ) + return max(0.0, min(1.0, gripper_position_percent)) diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py new file mode 100644 index 00000000..174ca8b9 --- /dev/null +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_parameter_config.py @@ -0,0 +1,140 @@ +from dataclasses import dataclass +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from typing import Any, Iterator + + +@dataclass +class ParameterConfig: + descriptor: ParameterDescriptor + default: Any + + +class GelloParameterConfig: + """Configuration class for GELLO ROS2 parameters.""" + + DEFAULT_NUM_JOINTS = 7 + DEFAULT_JOINT_SIGNS = [1] * DEFAULT_NUM_JOINTS + DEFAULT_BEST_OFFSETS = [0.0] * DEFAULT_NUM_JOINTS + DEFAULT_GRIPPER_RANGE_RAD = (0.0, 0.0) + DEFAULT_CONTROL_GAINS = [0] * DEFAULT_NUM_JOINTS + DEFAULT_GOAL_POSITION = [0.0] * DEFAULT_NUM_JOINTS + + def __init__(self, default_com_port: str): + self.hardware_params = [ + ParameterConfig( + ParameterDescriptor( + name="com_port", + type=ParameterType.PARAMETER_STRING, + description="USB serial port path", + read_only=True, + ), + default_com_port, + ), + ParameterConfig( + ParameterDescriptor( + name="gello_name", + type=ParameterType.PARAMETER_STRING, + description="GELLO device identifier", + read_only=True, + ), + default_com_port, + ), + ParameterConfig( + ParameterDescriptor( + name="num_arm_joints", + type=ParameterType.PARAMETER_INTEGER, + description="Number of arm joints", + read_only=True, + ), + self.DEFAULT_NUM_JOINTS, + ), + ParameterConfig( + ParameterDescriptor( + name="joint_signs", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Joint direction signs", + read_only=True, + ), + self.DEFAULT_JOINT_SIGNS, + ), + ParameterConfig( + ParameterDescriptor( + name="gripper", + type=ParameterType.PARAMETER_BOOL, + description="Enable gripper control", + read_only=True, + ), + True, + ), + ParameterConfig( + ParameterDescriptor( + name="gripper_range_rad", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description="Gripper range in radians", + read_only=True, + ), + self.DEFAULT_GRIPPER_RANGE_RAD, + ), + ParameterConfig( + ParameterDescriptor( + name="best_offsets", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description="Joint offset calibration", + read_only=True, + ), + self.DEFAULT_BEST_OFFSETS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_p", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Proportional gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_i", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Integral gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_kp_d", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Derivative gains", + additional_constraints="0 to ~1000", + read_only=False, + ), + self.DEFAULT_CONTROL_GAINS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_torque_enable", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="GELLO Torque enabled", + additional_constraints="0 (disabled), 1 (enabled)", + read_only=False, + ), + [0] * self.DEFAULT_NUM_JOINTS, + ), + ParameterConfig( + ParameterDescriptor( + name="dynamixel_goal_position", + type=ParameterType.PARAMETER_INTEGER_ARRAY, + description="Goal positions", + read_only=False, + ), + self.DEFAULT_GOAL_POSITION, + ), + ] + + def __iter__(self) -> Iterator[ParameterConfig]: + """Return an iterator over the parameter configurations.""" + return iter(self.hardware_params) diff --git a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py index 85620ba9..15793a76 100644 --- a/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py +++ b/ros2/src/franka_gello_state_publisher/franka_gello_state_publisher/gello_publisher.py @@ -1,51 +1,60 @@ -import os -import sys -import glob import rclpy +from glob import glob +from sys import exit +from signal import signal, SIGINT, SIGTERM from rclpy.node import Node from sensor_msgs.msg import JointState from std_msgs.msg import Float32 -from ament_index_python.packages import get_package_prefix +from rcl_interfaces.msg import ParameterEvent +from rclpy.parameter import parameter_value_to_python +from franka_gello_state_publisher.gello_hardware import GelloHardware, GelloHardwareParams +from franka_gello_state_publisher.gello_parameter_config import ( + ParameterConfig, + GelloParameterConfig, +) class GelloPublisher(Node): - def __init__(self): + """ROS2 node for publishing GELLO device joint states and handling parameter updates.""" + + def __init__(self) -> None: super().__init__("gello_publisher") + self.PUBLISHING_RATE = 25 # Hz - default_com_port = self.determine_default_com_port() - self.declare_parameter("com_port", default_com_port) - self.declare_parameter("gello_name", default_com_port) - self.declare_parameter("num_joints", 7) - self.declare_parameter("joint_signs", [1] * 7) - self.declare_parameter("gripper", True) - self.declare_parameter("gripper_range_rad", (0.0, 0.0)) - self.declare_parameter("best_offsets", [0.0] * 7) - self.com_port = self.get_parameter("com_port").get_parameter_value().string_value - self.gello_name = self.get_parameter("gello_name").get_parameter_value().string_value - self.num_robot_joints = ( - self.get_parameter("num_joints").get_parameter_value().integer_value - ) - self.joint_signs = ( - self.get_parameter("joint_signs").get_parameter_value().integer_array_value - ) - self.gripper = self.get_parameter("gripper").get_parameter_value().bool_value - self.gripper_range_rad = ( - self.get_parameter("gripper_range_rad").get_parameter_value().double_array_value - ) - self.best_offsets = ( - self.get_parameter("best_offsets").get_parameter_value().double_array_value - ) + hardware_params: GelloHardwareParams = self._setup_hardware_parameters() + + self.gello_hardware = GelloHardware(hardware_params) - self.initialize_gello_driver() + signal(SIGINT, self._signal_handler) # Handle Ctrl+C gracefully + signal(SIGTERM, self._signal_handler) # Handle termination signals - self.robot_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) + self.arm_joint_publisher = self.create_publisher(JointState, "gello/joint_states", 10) self.gripper_joint_publisher = self.create_publisher( Float32, "gripper/gripper_client/target_gripper_width_percent", 10 ) - self.timer = self.create_timer(1 / 25, self.publish_joint_jog) + # Subscribe to parameter events to allow dynamic updates of parameters + self.parameter_subscription = self.create_subscription( + ParameterEvent, "/parameter_events", self.parameter_event_callback, 10 + ) + + self.timer = self.create_timer(1 / self.PUBLISHING_RATE, self.publish_joint_jog) + + def parameter_event_callback(self, event: ParameterEvent) -> None: + """Handle parameter change events for this node.""" + if event.node != self.get_fully_qualified_name(): + return - self.joint_names = [ + for param in event.changed_parameters: + # Skip parameters that are not related to the Dynamixel control parameters + if not param.name.startswith("dynamixel_"): + continue + param_value = parameter_value_to_python(param.value) + self.gello_hardware.update_dynamixel_control_parameter(param.name, param_value) + + def publish_joint_jog(self) -> None: + """Publish current joint states and gripper position.""" + JOINT_NAMES = [ "fr3_joint1", "fr3_joint2", "fr3_joint3", @@ -54,9 +63,27 @@ def __init__(self): "fr3_joint6", "fr3_joint7", ] + [gello_arm_joints, gripper_position] = self.gello_hardware.read_joint_states() + + arm_joint_states = JointState() + arm_joint_states.header.stamp = self.get_clock().now().to_msg() + arm_joint_states.name = JOINT_NAMES + arm_joint_states.header.frame_id = "fr3_link0" + arm_joint_states.position = gello_arm_joints.tolist() - def determine_default_com_port(self) -> str: - matches = glob.glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") + gripper_joint_states = Float32() + gripper_joint_states.data = gripper_position + self.arm_joint_publisher.publish(arm_joint_states) + self.gripper_joint_publisher.publish(gripper_joint_states) + + def destroy_node(self) -> None: + """Override the destroy_node method to disable torque mode before shutting down.""" + self.gello_hardware.disable_torque() + super().destroy_node() + + def _determine_default_com_port(self) -> str: + """Auto-detect GELLO device COM port or return invalid placeholder.""" + matches = glob("/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter*") if matches: self.get_logger().info(f"Auto-detected com_ports: {matches}") return matches[0] @@ -64,55 +91,42 @@ def determine_default_com_port(self) -> str: self.get_logger().warn("No com_ports detected. Please specify the com_port manually.") return "INVALID_COM_PORT" - def initialize_gello_driver(self: str): - joint_ids = list(range(1, self.num_joints + 1)) - self.add_dynamixel_driver_path() - from gello.dynamixel.driver import DynamixelDriver - - self.driver = DynamixelDriver(joint_ids, port=self.com_port, baudrate=57600) - """The driver for the Dynamixel motors.""" + def _declare_ros2_param(self, param: ParameterConfig): + """Declare ROS2 parameters.""" + parameter_value = self.declare_parameter( + param.descriptor.name, param.default, param.descriptor + ).get_parameter_value() - def __post_init__(self): - assert len(self.joint_signs) == self.num_robot_joints - for idx, j in enumerate(self.joint_signs): - assert j == -1 or j == 1, f"Joint idx: {idx} should be -1 or 1, but got {j}." + return parameter_value_to_python(parameter_value) - @property - def num_joints(self) -> int: - extra_joints = 1 if self.gripper else 0 - return self.num_robot_joints + extra_joints + def _setup_hardware_parameters(self): + """Declare and setup all hardware configuration parameters.""" + default_com_port = self._determine_default_com_port() + config = GelloParameterConfig(default_com_port) - def publish_joint_jog(self): - current_joints = self.driver.get_joints() - current_robot_joints = current_joints[: self.num_robot_joints] - current_joints_corrected = (current_robot_joints - self.best_offsets) * self.joint_signs + hardware_params: GelloHardwareParams = {} + for param in config: + hardware_params[param.descriptor.name] = self._declare_ros2_param(param) - robot_joint_states = JointState() - robot_joint_states.header.stamp = self.get_clock().now().to_msg() - robot_joint_states.name = self.joint_names - robot_joint_states.header.frame_id = "fr3_link0" - robot_joint_states.position = [float(joint) for joint in current_joints_corrected] + return hardware_params - gripper_joint_states = Float32() - if self.gripper: - gripper_position = current_joints[-1] - gripper_joint_states.data = self.gripper_readout_to_percent(gripper_position) + def _signal_handler(self, signum, frame): + """Handle system signals for graceful shutdown.""" + if signum == SIGINT or signum == SIGTERM: + self.get_logger().info(f"Received signal {signum}, shutting down gracefully...") + self._safe_shutdown() + exit(0) else: - gripper_joint_states.data = 0.0 - self.robot_joint_publisher.publish(robot_joint_states) - self.gripper_joint_publisher.publish(gripper_joint_states) - - def gripper_readout_to_percent(self, gripper_position: float) -> float: - gripper_percent = (gripper_position - self.gripper_range_rad[0]) / ( - self.gripper_range_rad[1] - self.gripper_range_rad[0] - ) - return max(0.0, min(1.0, gripper_percent)) - - def add_dynamixel_driver_path(self): - gello_path = os.path.abspath( - os.path.join(get_package_prefix("franka_gello_state_publisher"), "../../../") - ) - sys.path.insert(0, gello_path) + self.get_logger().warn(f"Received unexpected signal {signum}") + + def _safe_shutdown(self): + """Safely shutdown hardware.""" + try: + self.get_logger().info("Disabling GELLO torque...") + self.gello_hardware.disable_torque() + self.get_logger().info("GELLO torque disabled successfully") + except Exception as e: + self.get_logger().error(f"Error disabling torque: {e}") def main(args=None): diff --git a/ros2/src/franka_gello_state_publisher/launch/main.launch.py b/ros2/src/franka_gello_state_publisher/launch/main.launch.py index fd2e5624..b3c9d3a8 100755 --- a/ros2/src/franka_gello_state_publisher/launch/main.launch.py +++ b/ros2/src/franka_gello_state_publisher/launch/main.launch.py @@ -32,11 +32,16 @@ def generate_robot_nodes(context): parameters=[ {"com_port": "/dev/serial/by-id/" + config["com_port"]}, {"gello_name": item_name}, - {"num_joints": config["num_joints"]}, + {"num_arm_joints": config["num_arm_joints"]}, {"joint_signs": config["joint_signs"]}, {"gripper": config["gripper"]}, {"gripper_range_rad": config["gripper_range_rad"]}, {"best_offsets": config["best_offsets"]}, + {"dynamixel_kp_p": config["dynamixel_kp_p"]}, + {"dynamixel_kp_i": config["dynamixel_kp_i"]}, + {"dynamixel_kp_d": config["dynamixel_kp_d"]}, + {"dynamixel_goal_position": config["dynamixel_goal_position"]}, + {"dynamixel_torque_enable": config["dynamixel_torque_enable"]}, ], ) ) diff --git a/ros2/src/franka_gello_state_publisher/setup.py b/ros2/src/franka_gello_state_publisher/setup.py index 61fb1949..ed7665e4 100644 --- a/ros2/src/franka_gello_state_publisher/setup.py +++ b/ros2/src/franka_gello_state_publisher/setup.py @@ -7,6 +7,9 @@ name=package_name, version="0.2.0", packages=find_packages(exclude=["test"]), + package_data={ + "franka_gello_state_publisher.dynamixel": ["motor_configs/*.yaml"], + }, data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), diff --git a/ros2/src/franka_gello_state_publisher/test/test_pep257.py b/ros2/src/franka_gello_state_publisher/test/test_pep257.py index 647b3c33..a2c3deb8 100644 --- a/ros2/src/franka_gello_state_publisher/test/test_pep257.py +++ b/ros2/src/franka_gello_state_publisher/test/test_pep257.py @@ -14,12 +14,10 @@ from ament_pep257.main import main import pytest -from pathlib import Path @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - excluded_file = Path(__file__).resolve().parents[1] / "franka_gello_state_publisher/driver.py" - rc = main(argv=[".", "test", "--exclude", str(excluded_file)]) + rc = main(argv=[".", "test"]) assert rc == 0, "Found code style errors / warnings"