Planning for a mobile robot

[AliaChe]

Hello,

I'm trying to use Moveit2 to plan trajectories for a mobile robot, namely the turtlebot3. I have added a planar virtual joint between the base frame of my robot (base_footprint) and the frame "world" in the srdf file. I'm struggling now to set goal pose for my robot. I'm using the setGoal() function as follows:

geometry_msgs::msg::PoseStamped goal_pose; goal_pose.header.stamp=rclcpp::Clock().now(); goal_pose.pose.position.x=2; goal_pose.pose.position.y=0; goal_pose.pose.position.z=0; goal_pose.pose.orientation.x=0; goal_pose.pose.orientation.y=0; goal_pose.pose.orientation.z=0; goal_pose.pose.orientation.w=0; moveit::planning_interface::PlanningComponent arm("turtlebot", moveit_cpp_); arm.setGoal(goal_pose, "base_footprint");

Here is the error I got:
[run_TB3_cpp-2] Parsing robot urdf xml string. [run_TB3_cpp-2] [WARN] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [run_TB3_cpp-2] [WARN] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: The complete state of the robot is not yet known. Missing virtual_joint [run_TB3_cpp-2] [WARN] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: The complete state of the robot is not yet known. Missing virtual_joint [run_TB3_cpp-2] [WARN] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: The complete state of the robot is not yet known. Missing virtual_joint [run_TB3_cpp-2] [WARN] [moveit_kinematic_constraints.kinematic_constraints]: No frame specified for position constraint on link 'base_footprint'! [run_TB3_cpp-2] [WARN] [moveit_kinematic_constraints.kinematic_constraints]: Orientation constraint for link 'base_footprint' is probably incorrect: 0.000000, 0.000000, 0.000000, 0.000000. Assuming identity instead. [run_TB3_cpp-2] [WARN] [moveit_kinematic_constraints.kinematic_constraints]: No frame specified for position constraint on link 'base_footprint'! [run_TB3_cpp-2] [ERROR] [moveit_ompl_planning.model_based_planning_context]: Unable to construct goal representation [run_TB3_cpp-2] [ERROR] [moveit.ros_planning_interface.planning_component]: Could not compute plan successfully
Any idea? Thanks.

[AliaChe]

I solved the error above by setting the goal pose differently.
I'm now using the srdf file to set a group_state :
<group name="turtlebot"> <joint name="virtual_joint" /> </group> <!--Group states--> <group_state name="home" group="turtlebot"> <joint name="virtual_joint" value="1 0 0"/> </group_state> <virtual_joint name="virtual_joint" type="planar" parent_frame="world" child_link="base_footprint" />

and then I set the goal using this goal_state name :
moveit::planning_interface::PlanningComponent arm("turtlebot", moveit_cpp_); arm.setGoal("home");

I'm now facing problems with rviz2. Knowing that robot_state_publisher is launched, a static_tf between 'world' and 'odom' frames is launched and the turtlebot3 gazebo simulation is run. Here is the errors returned by rviz2:

[rviz2-3] [ERROR] [moveit_transforms.transforms]: Given transform is to frame 'world', but frame 'base_footprint' was expected.
[rviz2-3] [ERROR] [moveit_transforms.transforms]: Unable to transform from frame 'world' to frame 'base_footprint'. Returning identity.
[rviz2-3] [WARN] [moveit_robot_state.conversions]: No joint matching multi-dof joint 'virtual_joint'
[rviz2-3] Warning: Invalid frame ID "base_footprint" passed to canTransform argument source_frame - frame does not exist
[rviz2-3] at line 133 in /tmp/binarydeb/ros-eloquent-tf2-0.12.5/src/buffer_core.cpp
[rviz2-3] [WARN] [moveit_robot_state.conversions]: The transform for multi-dof joints was specified in frame 'world' but it was not possible to transform that to frame 'base_footprint'
[rviz2-3] [ERROR] [moveit_robot_model.robot_model]: Joint 'virtual_joint' not found in model 'turtlebot3_burger'
[ERROR] [rviz2-3]: process has died

[JafarAbdi]

Hi @AliaChe,

Can you share your demo so I'm able to reproduce it locally and try to fix the error you're getting .?

[AliaChe]

Hi @JafarAbdi,

You can find my code here : https://github.com/AliaChe/ade-TB3-moveit2

Thank you.

[AliaChe]

Problem solved by adding the parameter that loads the srdf file (robot_description_semantic) to the rviz node in the launch file.

[AliaChe]

Hello again,

I'm facing a problem with Rviz and the planning plugin. All the TF are correctly published, the robot is moving in gazebo, however, in rviz it's not. The robot is placed at the origin defined in the urdf and not taking into account the TF messages. The planning is done given the robot pose in rviz and not its current pose. I have this warning also:
[WARN] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: The complete state of the robot is not yet known. Missing virtual_joint

Any help is appreciated. Thanks in advance.

[JafarAbdi]

Hi @AliaChe,

Sorry I didn't have the time to work on MoveIt 2 last week, I did now pull your repo and I was able to reproduce the warning you're getting, I think this's related to this comment here https://github.com/ros-planning/moveit2/blob/master/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp#L145, I'll try to fix it sometime soon

[AliaChe]

@JafarAbdi The comment you mentioned seems to be the error cause. Waiting for the fix then.
Thanks