Feature/Add AICO external axis control and MoveIt SRDF files

README.md

@@ -156,7 +156,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
 
@@ -187,6 +191,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:
@@ -204,7 +222,19 @@ 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

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)

flexiv_bringup/config/aico2_4_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_4_v1_joint1
+      - $(var external_axis_prefix)aico2_4_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)