Backport/Add AICO external axis support to Humble release

.github/workflows/humble-binary-build.yml

@@ -47,21 +47,21 @@ jobs:
           git clone https://github.com/flexivrobotics/flexiv_rdk.git -b release/v1.9
           cd flexiv_rdk/thirdparty
           source /opt/ros/humble/setup.bash
-          bash build_and_install_dependencies_not_in_ros2.sh ~/rdk_install
+          bash build_and_install_dependencies_not_in_ros2.sh ~/flexiv_install
           cd ..
           mkdir -p build && cd build
-          cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+          cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install
           cmake --build . --target install --config Release
       - name: Build and install flexiv_drdk
         shell: bash
         run: |
-          git clone https://github.com/flexivrobotics/flexiv_drdk.git -b v1.1
+          git clone https://github.com/flexivrobotics/flexiv_drdk.git -b release/v1.2
           cd flexiv_drdk/thirdparty
           source /opt/ros/humble/setup.bash
-          bash build_and_install_dependencies.sh ~/drdk_install
+          bash build_and_install_dependencies.sh ~/flexiv_install
           cd ..
           mkdir -p build && cd build
-          cmake .. -DCMAKE_INSTALL_PREFIX=~/drdk_install
+          cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install
           cmake --build . --target install --config Release
       - name: Build and run tests
         id: action-ros-ci
@@ -82,7 +82,7 @@ jobs:
             {
               "build": {
                 "cmake-args": [
-                    "-DCMAKE_PREFIX_PATH=~/rdk_install;~/drdk_install"
+                    "-DCMAKE_PREFIX_PATH=~/flexiv_install"
                 ]
               }
             }

README.md

@@ -36,6 +36,7 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04), Humble (Ubuntu 22.04)
    python3-colcon-common-extensions \
    python3-rosdep2 \
    libeigen3-dev \
+   wget \
    ros-humble-control-toolbox \
    ros-humble-hardware-interface \
    ros-humble-joint-state-publisher \
@@ -56,7 +57,7 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04), Humble (Ubuntu 22.04)
    ```bash
    mkdir -p ~/flexiv_ros2_ws/src
    cd ~/flexiv_ros2_ws/src
-   git clone https://github.com/flexivrobotics/flexiv_ros2.git
+   git clone https://github.com/flexivrobotics/flexiv_ros2.git -b humble
    ```
 
 4. Install dependencies:
@@ -69,20 +70,20 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04), Humble (Ubuntu 22.04)
    rosdep install --from-paths src --ignore-src --rosdistro humble -r -y
    ```
 
-5. Choose a directory for installing `flexiv_rdk` library and all its dependencies. For example, a new folder named `rdk_install` under the home directory: `~/rdk_install`. Compile and install to the installation directory:
+5. Choose a directory for installing `flexiv_rdk` library and all its dependencies. For example, a new folder named `flexiv_install` under the home directory: `~/flexiv_install`. Compile and install to the installation directory:
 
    ```bash
    cd ~/flexiv_ros2_ws/src/flexiv_rdk/thirdparty
    source /opt/ros/humble/setup.bash
-   bash build_and_install_dependencies_not_in_ros2.sh ~/rdk_install
+   bash build_and_install_dependencies_not_in_ros2.sh ~/flexiv_install
    ```
 
 6. Configure and install `flexiv_rdk`:
 
    ```bash
    cd ~/flexiv_ros2_ws/src/flexiv_rdk
    rm -rf build && mkdir build && cd build
-   cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+   cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install
    cmake --build . --target install --config Release
    ```
 
@@ -91,7 +92,7 @@ This project was developed for ROS 2 Foxy (Ubuntu 20.04), Humble (Ubuntu 22.04)
    ```bash
    cd ~/flexiv_ros2_ws
    source /opt/ros/humble/setup.bash
-   colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/rdk_install
+   colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/flexiv_install
    source install/setup.bash
    ```
 
@@ -103,34 +104,32 @@ If you are using a Flexiv dual robot setup, you can install `flexiv_drdk` as wel
 
    ```bash
    cd ~/flexiv_ros2_ws/src
-   git clone https://github.com/flexivrobotics/flexiv_drdk.git
-   cd flexiv_drdk
-   git checkout v1.1
-   touch COLCON_IGNORE
+   git clone https://github.com/flexivrobotics/flexiv_drdk.git -b release/v1.2
+   touch flexiv_drdk/COLCON_IGNORE
    ```
 
-2. Install dependencies and build `flexiv_drdk` by choosing an installation directory, e.g., `~/drdk_install`:
+2. Install dependencies and build `flexiv_drdk` by choosing an installation directory, e.g., `~/flexiv_install`:
 
    ```bash
    cd ~/flexiv_ros2_ws/src/flexiv_drdk/thirdparty
    source /opt/ros/humble/setup.bash
-   bash build_and_install_dependencies.sh ~/drdk_install
+   bash build_and_install_dependencies.sh ~/flexiv_install
    ```
 
 3. Configure and install `flexiv_drdk`:
 
    ```bash
    cd ~/flexiv_ros2_ws/src/flexiv_drdk
    rm -rf build && mkdir build && cd build
-   cmake .. -DCMAKE_INSTALL_PREFIX=~/drdk_install
+   cmake .. -DCMAKE_INSTALL_PREFIX=~/flexiv_install
    cmake --build . --target install --config Release
    ```
 
 4. Rebuild the workspace with both RDK and DRDK installation paths:
 
    ```bash
    cd ~/flexiv_ros2_ws
-   colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH="~/rdk_install;~/drdk_install"
+   colcon build --symlink-install --cmake-args -DCMAKE_PREFIX_PATH=~/flexiv_install
    ```
 
 > [!IMPORTANT]
@@ -159,7 +158,11 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
 - `fake_sensor_commands` (default: *false*) - enables fake command interfaces for sensors used for simulations. Used only if `use_fake_hardware` parameter is true.
 - `robot_controller` (default: *rizon_arm_controller*) - robot controller to start. Available controllers: *rizon_arm_controller*
 
-*(Details about other launch files can be found in [`flexiv_bringup`](/flexiv_bringup))*
+There are extra or different launch arguments for Flexiv AICO1, AICO2, and dual robot setups. *(Details about other launch files can be found in [`flexiv_bringup`](/flexiv_bringup))*
+
+- `robot_sn_left` (*required for dual robot setup*) - Serial number of the left robot to connect to. Remove any space, for example: Rizon4-123456
+- `robot_sn_right` (*required for dual robot setup*) - Serial number of the right robot to connect to. Remove any space, for example: Rizon4R-654321
+- `external_axis_type` (default: *AICO1-4-V1*) - type of the Flexiv AICO1 robot platform. Options: *AICO1-4-V1* or *AICO1-4-V2*
 
 ### Example Commands
 
@@ -190,6 +193,20 @@ The main launch file to start the robot driver is the `rizon.launch.py` - it loa
 
      The joint position goals can be changed in `flexiv_bringup/config/joint_trajectory_position_publisher.yaml`
 
+#### AICO1 and AICO2 Example Commands
+
+**AICO1-4** robot:
+
+```bash
+ros2 launch flexiv_bringup aico1.launch.py robot_sn:=[robot_sn] rizon_type:=Rizon4 external_axis_type:=AICO1-4-V1
+```
+
+**AICO2-4** robot:
+
+```bash
+ros2 launch flexiv_bringup aico2.launch.py rizon_type:=Rizon4 robot_sn_left:=[robot_sn_left] robot_sn_right:=[robot_sn_right] external_axis_type:=AICO2-4-V1
+```
+
 ### Using MoveIt
 
 You can also run the MoveIt example and use the `MotionPlanning` plugin in RViZ to start planning:
@@ -207,9 +224,20 @@ ros2 launch flexiv_bringup rizon_moveit.launch.py robot_sn:=Rizon4-123456 use_fa
 With dual robot setup:
 
 ```bash
-ros2 launch flexiv_bringup rizon_dual_moveit.launch.py robot_sn_left:=Rizon4-123456 robot_sn_right:=Rizon4R-654321
+ros2 launch flexiv_bringup rizon_dual_moveit.launch.py robot_sn_left:=[robot_sn_left] robot_sn_right:=[robot_sn_right]
+```
+
+With AICO1-4 setup:
+
+```bash
+ros2 launch flexiv_bringup aico1_moveit.launch.py robot_sn:=[robot_sn] rizon_type:=Rizon4 external_axis_type:=AICO1-4-V1
 ```
 
+With AICO2-4 setup:
+
+```bash
+ros2 launch flexiv_bringup aico2_moveit.launch.py rizon_type:=Rizon4 robot_sn_left:=[robot_sn_left] robot_sn_right:=[robot_sn_right] external_axis_type:=AICO2-4-V1
+
 ### Robot States
 
 The robot driver (`rizon.launch.py`) publishes the following feedback states to the respective ROS topics:

flexiv_bringup/README.md

@@ -6,4 +6,13 @@ This package contains launch files: the main driver launcher, the MoveIt launch
 - `rizon_moveit.launch.py` - runs MoveIt together with the main driver. The controller for robot joints started in this launch file is *rizon_arm_controller*.
 - `test_joint_trajectory_controller.launch` - sends joint trajectory goals to the *rizon_arm_controller*.
 
+There are also launch files for other robot setups:
+
+- `aico1.launch.py` - the main launcher for Flexiv AICO1 robot.
+- `aico1_moveit.launch.py` - runs MoveIt together with the main driver for Flexiv AICO1 robot.
+- `aico2.launch.py` - the main launcher for Flexiv AICO2 robot.
+- `aico2_moveit.launch.py` - runs MoveIt together with the main driver for Flexiv AICO2 robot.
+- `rizon_dual.launch.py` - the main launcher for Flexiv Rizon dual robot setup.
+- `rizon_dual_moveit.launch.py` - runs MoveIt together with the main driver for Flexiv Rizon dual robot setup.
+
 **NOTE**: The example launch files run the demo nodes from the `flexiv_test_nodes` package, with the parameters defined in `/config`.

flexiv_bringup/config/aico1_4_v1_controllers.yaml

@@ -0,0 +1,38 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    gpio_controller:
+      type: gpio_controller/GPIOController
+
+    flexiv_robot_states_broadcaster:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
+
+rizon_arm_controller:
+  ros__parameters:
+    joints:
+      - $(var external_axis_prefix)aico1_4_v1_joint2
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0

flexiv_bringup/config/aico1_4_v2_controllers.yaml

@@ -0,0 +1,40 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    gpio_controller:
+      type: gpio_controller/GPIOController
+
+    flexiv_robot_states_broadcaster:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
+
+rizon_arm_controller:
+  ros__parameters:
+    joints:
+      - $(var external_axis_prefix)aico1_4_v2_joint1
+      - $(var external_axis_prefix)aico1_4_v2_joint2
+      - $(var external_axis_prefix)aico1_4_v2_joint_head
+      - $(var robot_sn)_joint1
+      - $(var robot_sn)_joint2
+      - $(var robot_sn)_joint3
+      - $(var robot_sn)_joint4
+      - $(var robot_sn)_joint5
+      - $(var robot_sn)_joint6
+      - $(var robot_sn)_joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0

flexiv_bringup/config/aico2_10_v1_controllers.yaml

@@ -0,0 +1,86 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    left_rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    right_rizon_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    gpio_controller_left:
+      type: gpio_controller/GPIOController
+
+    gpio_controller_right:
+      type: gpio_controller/GPIOController
+
+    flexiv_robot_states_broadcaster_left:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
+
+    flexiv_robot_states_broadcaster_right:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
+
+left_rizon_arm_controller:
+  ros__parameters:
+    joints:
+      - $(var external_axis_prefix)aico2_10_v1_joint1
+      - $(var external_axis_prefix)aico2_10_v1_joint2
+      - $(var prefix_left)joint1
+      - $(var prefix_left)joint2
+      - $(var prefix_left)joint3
+      - $(var prefix_left)joint4
+      - $(var prefix_left)joint5
+      - $(var prefix_left)joint6
+      - $(var prefix_left)joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: true
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+right_rizon_arm_controller:
+  ros__parameters:
+    joints:
+      - $(var prefix_right)joint1
+      - $(var prefix_right)joint2
+      - $(var prefix_right)joint3
+      - $(var prefix_right)joint4
+      - $(var prefix_right)joint5
+      - $(var prefix_right)joint6
+      - $(var prefix_right)joint7
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.01
+      goal_time: 0.0
+
+flexiv_robot_states_broadcaster_left:
+  ros__parameters:
+    robot_sn: left_$(var robot_sn_left)
+
+flexiv_robot_states_broadcaster_right:
+  ros__parameters:
+    robot_sn: right_$(var robot_sn_right)
+
+gpio_controller_left:
+  ros__parameters:
+    robot_sn: left_$(var robot_sn_left)
+
+gpio_controller_right:
+  ros__parameters:
+    robot_sn: right_$(var robot_sn_right)