Planning For Differential-Drive Mobile Base and an Arm: fake controllers configuration and execution of the planned path

[mmarrkk]

Hi, I'm trying to follow the tutorial (https://moveit.picknik.ai/main/doc/examples/mobile_base_arm/mobile_base_arm_tutorial.html#motion-model) and I'm having some troubles, even tho I've followed everything step by step and there are some things I've also changed since it was giving error:

• I listed position/x, position/y, position/theta inside the moveit_controllers.yaml;
• I added "control_multi_dof_joint_variables: true" in the moveit controller config since it was given as a Hint from the log messages;
• I added the planar joint "position" (the one created as virtual with moveit setup assistant) also in the URDF since the controller_manager node couldn't load the controllers and it was looking for that planar joint, but I created it as virtual.
  I use the demo.launch.py to start everything.
  I'm using Ros2 Jazzy.
  I'm working with a different robot, a combined one (Piper as the manipulator and the Scoutv2 as the mobile base).
  When launching the moveit config and I try to Plan&Execute the "mobile_base_manipulator" group (the group which include the mobile_base with the planar joint and the arm group with all the joints of the manipulator), the planning will be successfully done (I think the stretch plugin is also working since path generated is consistent with the differential drive motion type and also by just doing the planning I don't have any error showed), but the execution is failed and the Error I got is the following:

[ros2_control_node-5] [INFO] [1763995325.241594896] [mobile_arm_controller]: Received new action goal
[ros2_control_node-5] [ERROR] [1763995325.241637527] [mobile_arm_controller]: Incoming joint position/theta doesn't match the controller's joints.
[move_group-3] [ERROR] [1763995325.241786052] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-3] [INFO] [1763995325.241786537] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: mobile_arm_controller started execution
[move_group-3] [WARN] [1763995325.241849118] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-3] [ERROR] [1763995325.241843004] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller mobile_arm_controller
[move_group-3] [INFO] [1763995325.241881336] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-3] [INFO] [1763995325.250578471] [move_group.moveit.moveit.ros.move_group.move_action]: CONTROL_FAILED
[rviz2-4] [INFO] [1763995325.252068016] [moveit_1547312516.moveit.ros.move_group_interface]: Plan and Execute request aborted
[rviz2-4] [ERROR] [1763995325.252102950] [moveit_1547312516.moveit.ros.move_group_interface]: MoveGroupInterface::move() failed or timeout reached

Actually before " Incoming joint position/theta doesn't match the controller's joints." the error was "[mobile_arm_controller]: Joints on incoming trajectory don't match the controller joints.", but then I added "allow_partial_joints_goal: true" inside the ros2_controller.yaml.
Now it's seems that the "position/theta" has the problem and usually this happen when the joints from the moveit controller does not match the joints in the ros2_controller (if I'm not wrong), but if I put the same joints for the planar one (position/x, position/y and position/theta) also in the ros2_controller side, the controllers loading will be failed as the controller manager will try to fine those 3 joint in the URDF also. Those 3 joints are not defined in the URDF, and I cannot just define them just like that.

If following the steps described in the tutorial link for stretch_ros robot and using the stretch kinematic plugin should manage all the necessary configurations, what's missing and what should be done which is not described in there?

Or is this tutorial just for Planning view, just to show the stretch plugin working... ? and you need the differential drive controller for the wheels to also request the Execution of the trajectory planned... ?

This is my moveit_controlers.yaml:

This is my ros2_controller.yaml:

I've separated the controller for the arm and the mobile base, to try if I had to split them, but it still giving the same problem so...

In the URDF, this is how I added the planar joint:
<link name="odom"/> <joint name="position" type="planar"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="odom" /> <child link="base_link_scoutv2" /> <axis xyz="0 0 1"/> </joint>
I've also separated the command/state_interface for the planar joint just like in the tutorial, but actually nothing changes (I guess):

[mmarrkk]

In the ros2_controller, for the mobile_arm_controller the "command_jonts" should be "command_interfaces", it just an mistake due to copying and removing stuff, here is the actual one

[mmarrkk]

so, I managed myself to find a solution and yes, the tutorial It is only for showing the Planning, so the errors I was having are reasonable since:

• the planned trajectory execution for the mobile base is resquested from the Moveit controller (defined in the moveit_controllers.yaml) sending a Goal to the Action Server the controller defined in ros2_controller.yaml, if there is no controller (in the ros side and specifically with the same name) there won't be any Action Server an din this case the controller won't even loaded since no controller/different name;
• if there is the controller (with the same ename), but the joints to command are different (different names) the execution will fail due to mismatching;
• in order to able to define controllers in both side not specifically with the same name and even with different amount of controllers it's better to create your own launch file and not using the default demo.launch.py created with Moveit Setup Assistant.

You need also to integrate a controller for the wheels of the mobile base, in my a differential drive controller, and obviously a ros node who will translate the Trajectory Points planned into the Velocity commands to give to the wheels controller.

Here is the ros2_controller.yaml file

There si still something to improve with the configuration I've done so far, but at least the Execution part is working somehow too.

Aurevoir fellas...