GELLO actuation for ros2 franka support

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/*"
+  echo "source /opt/ros/humble/franka/setup.sh" >> ~/.profile'

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
 

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,33 +60,33 @@ 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.
 
 If you add new dependencies to your packages, remember to update the relevant `requirements.txt`, `requirements_dev.txt` or `package.xml` files and re-run the script.
 
 ## 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:
 
 ```bash
 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:
 
 ```bash
 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/<GELLO_USB_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:**<a name="virtual-springs-dampers"></a>
+
+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

ros2/requirements.txt

@@ -1,2 +1,3 @@
 dynamixel_sdk
+PyYAML
 tyro

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<MotionGenerator> motion_generator_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::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<double>& 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);

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<MotionGenerator>(motion_generator_speed_factor, q_, q_goal);
+  return true;
 }
 
 }  // namespace franka_fr3_arm_controllers