Planning for differential drive base

[Giovadess]

Hello! I have been trying to setup MoveIt2 with a differential drive base provided by robotnik called summit-xl.

I would like to control my platform in Gazebo for now and then switch to the real robot after doing some testing in the simulation. I also went trough the stretch robot documentation but I am struggling with understanding how to control a real robot with MoveIt.

I was able to follow the tutorial and I get to a point where the planning trough rviz is happening and is being completed for a virtual joint called position, which is defined as follows:

<virtual_joint name="position" type="planar" parent_frame="odom" child_link="base_footprint"/>
<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" />
<joint_property joint_name="position" property_name="angular_distance_weight" value="0.5" />

Now the issue comes with the execution command, the execution fails. I have tried three different approaches but none seems to be working. I will list all of them and also attach a github repo with all the details to reproduce them:

-I set up the ros2 control for the robot with a standard differential drive base controller, for the four wheeled platform. But from my understanding moveit is not able to interface with this type of controller. Is that correct?

-The final way I would like to follow trough I tried is to read the trajectory msg trough a custom action server but I obtain what seems like an empty trajectory message, as follows:

gdessy@gsdow:~/new_sum_moveit$ /bin/python3 /home/gdessy/new_sum_moveit/src/moveit_action/src/moveittalker.py [INFO] [1688743778.339598819] [follow_joint_trajectory_action_server]: FollowJointTrajectoryActionServer initialized this runned [INFO] [1688743789.707057567] [follow_joint_trajectory_action_server]: Executing action... <rclpy.action.server.ServerGoalHandle object at 0x7fd9c5dc5af0> <rclpy.action.server.ServerGoalHandle object at 0x7fd9c5dc5af0> Do i get here? YES trajectory_msgs.msg.JointTrajectory(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), joint_names=[], points=[]) [INFO] [1688743789.707677109] [follow_joint_trajectory_action_server]: Action execution complete [ERROR] [1688743789.708245848] [follow_joint_trajectory_action_server]: Error raised in execute callback: succeed() takes 1 positional argument but 2 were given Traceback (most recent call last): File "/opt/ros/galactic/lib/python3.8/site-packages/rclpy/action/server.py", line 324, in _execute_goal execute_result = await await_or_execute(execute_callback, goal_handle) File "/opt/ros/galactic/lib/python3.8/site-packages/rclpy/executors.py", line 107, in await_or_execute return callback(*args) File "/home/gdessy/new_sum_moveit/src/moveit_action/src/moveittalker.py", line 70, in execute_callback goal_handle.succeed(result_msg) TypeError: succeed() takes 1 positional argument but 2 were given [WARN] [1688743789.711623166] [follow_joint_trajectory_action_server]: Goal state not set, assuming aborted. Goal ID: [105 167 235 37 38 80 84 79 26 136 197 77 61 219 70 209]

I am not sure on what I am doing wrong if I am setting up the controllers in the wrong way or if the problem is before since the planning part seems to be happening.

Ideally I would like to make the execution work trough the diff_drive controller if possible or trough the action server which would allow me to interface better with the trajectory. If you can help me solve this issue because I have been stuck for the last couple of days. Thanks in advance for the help.

Your environment

• ROS Distro: [Galactic]
• OS Version: Ubuntu 20.04

Steps to reproduce

Download the package from: https://github.com/Giovadess/Sum_Moveit_project Launch gazebo in one window with the command ros2 launch summit_control_sim launch_sim.launch.py Launch Moveit in another terminal as ros2 launch summit_moveit2 summit_moveit2.launch.py Perform planning for the group mobile_base_armand execute it. Launch the moveittalker.py node from the moveit_action pkg src folder.

[sjahr]

I am unsure how you're using MoveIt in this scenario. If you're only working with a mobile robot you should take a look at https://navigation.ros.org/.

But from my understanding moveit is not able to interface with this type of controller. Is that correct?

From my understanding, the diff drive controller needs a stream of velocity commands and moveit does not transform a trajectory into such a stream. To execute trajectories we use the joint_trajectory_controller

The final way I would like to follow trough I tried is to read the trajectory msg trough a custom action server but I obtain what seems like an empty trajectory message,

You need to provide more information how you use moveit to generate the trajectory message

Furthermore, I'd recommend switching to a newer version of MoveIt (Humble, Rolling or Iron) since Galactic is not maintained anymore.

[Giovadess]

I am using moveIT with the mobile base only as the first step. I wanted to understand what features it has to offer since I only need to move the base in a limited space for now, I am already aware of the nav_stack and its functionalities. The final goal is to perform a whole-bode planning for the mobile base with a kinova arm mounted on top.

Concerning the controller it is exactly what I understood as well. The problem is that diff drive mobile base do not work properly with that type of controller since the motor work base on a velocity input.

The way I generate the trajectory is similar to the one implented in this tutorial (https://moveit.picknik.ai/humble/doc/examples/mobile_base_arm/mobile_base_arm_tutorial.html). I plan through Rviz and send the execution command (which does not work). Then I tried to read this trajectory by sending it to a custom action server bt it prints out an empty trajectory message. I agree on the ros version but galactic seemed to be working fine for now. I will be looking into moving to a newer version.

[sjahr]

Ok :+1: If galactic works for you that's fine. Unfortunately, I cannot tell you why your trajectory message is empty. It might be related to the wrong joint group being selected for planning or a configuration error in the joints you're trying to control. You could also try using another planner to see if it is related to that.

Regarding the diff-drive: Maybe the ros2 control controller chaining feature can help with this :thinking:

[Giovadess]

Hey Sjahr, Thanks for the reply. So I will try to avoid planning from Rviz from now and try planning using the API, if I remember correctly from moveit for ros 1 I should be able to get the trajectory message from there if the planning is performed correctly. In that case it may be that the action server I am using to receive the message from MoveIT2 may be setup incorrectly.

I will also look into the ros2 control chain thing thanks for the suggestion.

I also tried to post on the stretch-robot github since in my opinion the moveit tutorial for differential drive bases planning is not completed since it is not explaining how to control the robot itself.

I will leave the issue open in case somenone else will have some suggestions about this issue.

[Giovadess]

Hello, I will comment back on this. I solved all of my previous issues. I am able to plan and execute trough the rviz interface. Although when I tried planning trough the C++ interface the all thing does not work again. I followed this tutorial: https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html

Trying to send a trajectory to my planning group. But I get a catastrophic error with the following output:

[Giovadess]

Do you have any suggestions? Is the code from the tutorial not suited to the mobile base application?

[sjahr]

Awesome that you got the original issue resolved. Regarding your question: I don't have an immediate solution based on this screenshot. Do you mind opening another issue with that and providing the full command line output? That will also create more visibility for your question. I'll close this one