Mobile base Clearpath Warthog Mobile Base with UR10e arm Unable to see 'position' in URDF ROS2 Humble

[Marius-Juston]

Problem

When trying to follow https://moveit.picknik.ai/humble/doc/examples/mobile_base_arm/mobile_base_arm_tutorial.html and
https://docs.picknik.ai/how_to/configuration_tutorials/configure_mobile_base/ (I know that this is not meant for Humble, but it has the same ideas as above, except for the chainable DiffDriveTrainController).

The planning does work in RViz; however, (as expected since the hardware interfaces do not work ), nothing happens in Gazebo, which is our end goal. The UR10e arm works perfectly fine there, just when I have the mobile base that this does not work.

The current software does not work for Humble. We would like to work on Gazebo and then also on the real robot that we have. Do you have suggestions?

The problem that you get is:

[WARN] [1751434371.502945793] [w200_0079.gz_ros2_control]: Skipping joint in the URDF named 'position' which is not in the gazebo model.

Which causes the controllers not to be able to see the joint and thus not be able to publish to the joint position trajectory controller

[resource_manager]: Not acceptable command interface combination:
[ign gazebo-1] Start interfaces:
[ign gazebo-1] [
[ign gazebo-1]   position/position
[ign gazebo-1] ]
[ign gazebo-1] Stop interfaces:
[ign gazebo-1] [
[ign gazebo-1] ]
[ign gazebo-1] Not existing:
[ign gazebo-1] [
[ign gazebo-1]  position/position
[ign gazebo-1] ]
[ign gazebo-1]

Setup

Packages

Installation of Clearpath Simulation libraries:

sudo apt-get update && sudo apt-get install wget -y
sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
sudo apt-get update && sudo apt-get install ignition-fortress -y

sudo apt-get update
sudo apt-get install ros-humble-clearpath-simulator ros-humble-clearpath-desktop -y 

mkdir ~/clearpath_ws/src -p

source /opt/ros/humble/setup.bash
sudo apt install python3-vcstool -y
cd ~/clearpath_ws
wget https://raw.githubusercontent.com/clearpathrobotics/clearpath_simulator/humble/dependencies.repos
vcs import src < dependencies.repos
sudo rosdep init; rosdep update
rosdep install -r --from-paths src -i -y

cd ~/clearpath_ws/src
git clone https://github.com/AUVSL/clearpath_gz.git

cp robot.yaml ~/clearpath_ws/robot.yaml
sed -i "s|administrator|$(whoami)|" robot.yaml

cp ~/my_robot_calibration.yaml ~

colcon build --symlink-install

Robot definition configs

ros2 run clearpath_generator_common generate_bash -s $HOME/clearpath_ws/
ros2 run clearpath_generator_common generate_description -s $HOME/clearpath_ws/
ros2 run clearpath_generator_common generate_semantic_description -s $HOME/clearpath_ws/

This creates the robot.srdf and robot.srdf.xacro and robot.urdf.xacro

Run for the first time

Run ONCE!!!

source ~/clearpath_ws/install/setup.bash; ros2 launch clearpath_gz_auvsl simulation.launch.py setup_path:=$HOME/clearpath_ws generate:=true

OTHERWISE ALL THE CONFIG SETUP FROM BEFORE WILL GET OVERWRITTEN AND YOU WILL HAVE TO DO IT AGAIN!!!!

From now on ONLY use

source ~/clearpath_ws/install/setup.bash; ros2 launch clearpath_gz_auvsl simulation.launch.py setup_path:=$HOME/clearpath_ws generate:=false

Mobile Base Setup

As per the instructions in the https://moveit.picknik.ai/humble/doc/examples/mobile_base_arm/mobile_base_arm_tutorial.html I modified the robot.srdf and included

<group name="position">
    <joint name="position" />
</group>
<group name="mobile_base">
    <group name="arm_0" />
    <group name="position" />
</group>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="position" type="planar" parent_frame="odom" child_link="base_link"/>
<joint_property joint_name="position" property_name="motion_model" value="diff_drive" />
<joint_property joint_name="position" property_name="min_translational_distance" value="0.01" />

inside the robot.srdf.xacro and robot.urdf.xacro

<xacro:macro name="mobile_base_ros2_control" params="name">
  <ros2_control name="${name}" type="system">
      <hardware>
          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
      </hardware>
      <joint name="position">
          <command_interface name="position" />
          <state_interface name="position"/>
          <state_interface name="velocity"/>
      </joint>
   </ros2_control>
</xacro:macro>

<xacro:mobile_base_ros2_control name="WarthogBaseFakeJointDriver" />

<gazebo>
  <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
  </plugin>
</gazebo>

inside the manipulators/config/moveit.yaml to configure the moveit controllers:

moveit_controller_manager: 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
moveit_simple_controller_manager:
  controller_names: ['arm_0_joint_trajectory_controller', 'mobile_base_controller']
  arm_0_joint_trajectory_controller:
    type: 'FollowJointTrajectory'
    action_ns: 'follow_joint_trajectory'
    default: True
    joints: ['arm_0_shoulder_pan_joint', 'arm_0_shoulder_lift_joint', 'arm_0_elbow_joint', 'arm_0_wrist_1_joint', 'arm_0_wrist_2_joint', 'arm_0_wrist_3_joint']
  mobile_base_controller:
    type: 'FollowJointTrajectory'
    action_ns: 'follow_joint_trajectory'
    default: True
    joints:
      - position
robot_description_kinematics:
  arm_0:
    kinematics_solver: 'kdl_kinematics_plugin/KDLKinematicsPlugin'
    kinematics_solver_search_resolution: 0.005
    kinematics_solver_timeout: 0.005
    kinematics_solver_attempts: 3
  mobile_base:
    kinematics_solver: 'pick_ik/PickIkPlugin'
    kinematics_solver_search_resolution: 0.005
    kinematics_solver_timeout: 0.005
    kinematics_solver_attempts: 3

and for the ros2_control controllers I modified the platform/config/control.yaml:

  controller_manager:
    ros__parameters:
      update_rate: 50
      use_sim_time: True
      mobile_base_controller.type: 'joint_trajectory_controller/JointTrajectoryController'

  mobile_base_controller:
    ros__parameters:
      command_interfaces:
        - position
      state_interfaces:
        - position
        - velocity
      open_loop_control: true # disables PID
      allow_partial_joints_goal: true
      allow_integration_in_goal_trajectories: true
      joints:
        - position
      state_publish_rate: 50.0
      action_monitor_rate: 20.0
      use_sim_time: True

I believe that this should be correct and follow all the instructions that were listed? However, given that the system does not work with actual joints, the ros2_control seems just to ignore them for no apparent reason, and I seem unable to debug why this is. The ros2 control list_hardware_interfaces does not list those joints and they never show up anywhere. I am confused as to why these would not show up in the Gazebo simulation.

I know that I will need to also create a seperate MoveIt interface between the virtual position joint and the /robot/cmd_vel differential drive robot as well since in ROS2 Humble DiffDriveController is not a ChainableController; however, that is some that is much easier to implement once this is working (unless there is a better way for this as well?)

In the end I want to be able to Whole Body Planning with the current configuration.

I have included at the bottom the files necessary to get this reproduced as well:

Resources

control.yaml.txt
moveit.yaml.txt
my_robot_calibration.yaml.txt
robot.srdf.txt
robot.srdf.xacro.txt
robot.urdf.xacro.txt
robot.yaml.txt

I feel like I am doing something just extremely stupid but I have no idea what it is and it is driving me crazy (especially since all the tutorial for this use Jazzy+ which has the newer DiffDriveControllers or they just use RViz and not Gazebo as wel!!

[Marius-Juston]

I am especially confused since Humble does not seem to support multi-DOF joints inside OMPL or such so I am confused as to how this works in the tutorial?