ros-planning
How can I plan a trajectory for Continuous target pose？
Version
Humble
OS
Ubuntu22.04
Install
moveit2 from source on branch Humble
moveit configs
only changed move group config for octomap. Other config is generated by setup assistant for a simple 6-dof robot arm created by myself.
Issue
Hello, I've tried to plan a trajectory with pose like A->B->C. But it seems that the move_group_interface only plan for A->C. I've tried every pose, I'm sure all of them can be reached.
here is my code:
geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.7;
move_group.setPoseTarget(target_pose1);
geometry_msgs::msg::Pose target_pose2;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = -0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.7;
move_group.setPoseTarget(target_pose2);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
It return success, but the trajectory shows in rviz is only for current_pose to target_pose2. I've read the source code from moveit2 move_group_interface, I cant't find a api for this case. Is there any way to implement this function？
Many tutorials tell me to use computeCartesianPath, but it seems that this function can't avoid obstacle in planing scene. This question has been bothering me for a long time, can someone help me.
As you pointed out, the Cartesian planner or Pilz industrial motion planner can pass through specific points, but not in a way that avoids collisions: it's more straight-line paths with collision checking.
I am not sure how easy this is to do with MoveIt, but your best bet if you need to avoid obstacles may be to use something like the OMPL planner to get two paths -- one from A to B, and another from B to C. Then, you can stitch the two paths together and put them through a trajectory timing algorithm like TOTG or Ruckig.
Again, unclear how easily MoveIt exposes all these basic building blocks to users, or it's all too wrapped up in a rigid pipeline. But approach-wise, I think this is the way to go.
As you pointed out, the Cartesian planner or Pilz industrial motion planner can pass through specific points, but not in a way that avoids collisions: it's more straight-line paths with collision checking.
I am not sure how easy this is to do with MoveIt, but your best bet if you need to avoid obstacles may be to use something like the OMPL planner to get two paths -- one from A to B, and another from B to C. Then, you can stitch the two paths together and put them through a trajectory timing algorithm like TOTG or Ruckig.
Again, unclear how easily MoveIt exposes all these basic building blocks to users, or it's all too wrapped up in a rigid pipeline. But approach-wise, I think this is the way to go.
Thank you for your reply! I still have one more question, I've tried to plan A->B B->C by using ompl. But every time I call the plan interface, ompl plans a path starting from the initial point A, even if I use the setStartStates function. As a result, I get the path for A->B and A->C but that is not what I want. Do I have to move the robot to point B before I can do path planning for B ->C?
Hmm, I am not super familiar with this interface, but are you using the MotionPlanRequest ROS message? It appears to have a start state field:
https://github.com/moveit/moveit_msgs/blob/ec3ba4bfa2ec37df576bcf7a087e5a4ab88d79a4/msg/MotionPlanRequest.msg#L10
Edit: seems this is packaged up in the setStartState method: https://github.com/moveit/moveit2/blob/5946a4ef184e69ab775dd2ad42009c791367b62b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp#L296 -- seems you were already calling this, so if that isn't working maybe there is a bug?
You should not need to actually move the robot, though.
Hmm, I am not super familiar with this interface, but are you using the
MotionPlanRequestROS message? It appears to have a start state field:https://github.com/moveit/moveit_msgs/blob/ec3ba4bfa2ec37df576bcf7a087e5a4ab88d79a4/msg/MotionPlanRequest.msg#L10
Edit: seems this is packaged up in the
setStartStatemethod: https://github.com/moveit/moveit2/blob/5946a4ef184e69ab775dd2ad42009c791367b62b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp#L296 -- seems you were already calling this, so if that isn't working maybe there is a bug?You should not need to actually move the robot, though.
@sea-bass sorry for so late reply. I was trying my best to figure out what was going on. Now I've realized that the bug regarding the incorrect planning of multi-waypoint paths is not related to the number of endpoints. I think it's because there is a bug in the interpolation process of the planar joint itself.
Environment
Ubuntu 22.04 ROS2 HUMBLE Moveit2 Humble from source OMPL RRTConnect planner
Issue
I was trying to follow this tutorial: Planning For Differential-Drive Mobile Base and an Arm I designed my custom robot and created a virtual joint for the moving base which is same as the tutorial
<!--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="diff_vec" type="planar" parent_frame="odom" child_link="car_base_link"/>
<!--JOINT PROPERTIES: Purpose: Define a property for a particular joint (could be a virtual joint)-->
<joint_property joint_name="diff_vec" property_name="motion_model" value="diff_drive"/>
<joint_property joint_name="diff_vec" property_name="min_translational_distance" value="0.01"/>
<joint_property joint_name="diff_vec" property_name="angular_distance_weight" value="0.5"/>
firstly I found that the theta limit of this joint is weird. I can set it from -2.10 to 4.18 in rviz. But I think it should be from -PI to PI defined from planar joint model. And I tried to plan a trajectory from the state below which only changes the theta of the moving base. Start state theta is 1.5 and target state theta is 4.12.
I got a result trajectory like this: https://github.com/user-attachments/assets/b80fd19e-3c25-466a-848a-0296a8eb9598
I thought it is a bug, but I checked the interpolate code and can't figure out what was wrong.
Do you have any suggestion about how to fix this? Maybe it is caused by the weird limit of the virtual joint? This error only happens when the target state theta is set to 3.15-4.18