Hello everyone!

Current situation

I'm working with a mobile robot RBKairos which has a UR10 manipulator on top of an omnidirectional (Mecanum) platform. I've been able to plan and execute a simple trajectory through C++ MoveIt! interface using only the 6 joints of manipulator arm as my planning_group. Now I would like to execute a trajectory on the real robot by moving both base and arm (total 9 DOF) simultaneously.

When I changed the configuration of the robot to include wheel joints (creating a new planning group mobile_robot), I got the error from KDL:

mobile_robot is not a chain!

To solve this problem, I created dummy links and joints (2 prismatic, 1 revolute) in URDF to emulate omnidirectional kinematics of the platform:

<link name="world_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
  </visual>
</link>
<joint name="${prefix}base_x_joint" type="prismatic">
  <axis xyz="1 0 0" />
  <parent link="world_link"/>
  <child link="${prefix}base_x_link" />
  <limit lower="-10.0" upper="10.0" effort="100.0" velocity="1.0"/>
</joint>
<link name="${prefix}base_x_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0" />
  </visual>
</link>
<joint name="${prefix}base_y_joint" type="prismatic">
  <axis xyz="0 1 0" />
  <parent link="${prefix}base_x_link"/>
  <child link="${prefix}base_footprint" />
  <limit lower="-10.0" upper="10.0" effort="100.0" velocity="1.0"/>
</joint>
<joint name="${prefix}base_footprint_joint" type="revolute">
  <origin xyz="0 0 ${wheel_radius}" rpy="0 0 0" />
  <limit lower="-6.28" upper="6.28" effort="330.0" velocity="2.16"/>
  <axis xyz="0 0 1" />
  <parent link="${prefix}base_footprint"/>
  <child link="${prefix}base_link" />
</joint>

${prefix}base_link is just the link where the arm is attached to the platform. With this, MoveIt! can plan the kind of movement I want, but only in simulation since I'm using those dummy joints and don't actuate the wheels. I'm using computeCartesianPath method of MoveGroupInterface, just as in MoveIt! tutorial to follow a linear path between 5 waypoints.

My question

What should I do with the calculated moveit_msgs::RobotTrajectory to generate valid commands for the platform? Should I publish the x, y and theta velocities to /robot/move/cmd_vel (and how)? Or do I override moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute with a custom transformation from x, y and theta velocities to the 4 wheel angular velocities (although I'm not so sure how it would actually look in code)?

Maybe somebody has a totally different idea? Hope this will be interesting for mobile robotics enthusiasts.

Thanks!