Online path replanning when encountering an obstacle MoveIt

asked 2021-09-22 08:52:55 -0600

zahid990170
135 ●30 ●43 ●48

updated 2021-09-27 06:07:03 -0600

Hi all,

I have been using MoveIt MoveGroupInterface to plan and execute simple pose goals and joint-space goals for UR5 robotic arm. Static obstacles, added before planning, are taken into account. Robotic arm plans trajectory avoiding such obstacles or it informs when no plan can be found given the static obstacle.

However, I want to achieve a functionality where a an obstacle appears after the motion command has been issued and then the arm can move to the same or nearby goal position by re-planning / adapting its trajectory. I have a separate node that formulates collision objects and publishes on "/collision_object" topic. Also I have another node that is responsible to monitor the planning scene and keep track of any updates that occur. I am using PlanningSceneMonitorPtr. Currently, I can see, whenever new collision objects are added. Also, I am using the following function checkCollision(collision_request, collision_result). This function can report if there will be a collision.

If I have a way to publish collision objects at a certain rate (bounding boxes retrieved for a human and published on the "/collision_object" topic), is this a feasible way to account for dynamic objects in the scene? What are the options to adapt the trajectory, do I need to stop (move_group.stop) and plan again to the same goal, What if the collision objects are frequently appearing in the scene?

thanks, Zahid

edit retag flag offensive close merge delete

Comments

In the future, please limit yourself to one question per post. It's harder to provide a complete answer when you're asking about many things.

Mike Scheutzow ( 2021-09-23 07:34:33 -0600 )edit

thanks @Mike Scheutzow. My main question is about dealing with dynamic collision objects that are being published on /collision_object topic with some rate. My system can know when this happens, What is the best way to adapt the trajectory in this case? Please see the edit to the question.

zahid990170 ( 2021-09-27 05:59:23 -0600 )edit

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2021-09-28 03:03:27 -0600

v4hn
2950 ●18 ●53 ●57

What are the options to adapt the trajectory

When you send a MoveGroup request to plan and execute, the path will be checked against every geometry update in the scene. If you additionally set replan == true (and the associate parameters) in your request, the planner will be invoked again in this case to generate a new collision-free trajectory and execute the new one.

Many people do not see this functionality because it will not work if you execute a previously generated/planned trajectory through the ExecuteTrajectory action, e.g., plan first, and then pass the trajectory to execution. That's weird, but in this case the engine should not be allowed to replan on it's own and execute a different trajectory. If you would like to see trajectory execution abort in this case, please write a pull-request. :)

During execution, the remaining path, which is actually collision-checked, is estimated based on passed time since triggering the trajectory controller. That's a caveat of the concrete implementation when execution time is not sufficiently deterministic, e.g., when working with compliant controllers or time scaling.

Additionally, the planner is not informed of the aborted trajectory, so the new trajectory might take a very different path when a minor adaptation would have been sufficient. That's also something that could be improved via the GenericTrajectory field that was added long after.

moveit_servo (is be a very different interface with overlapping use-cases) will automatically slow-down and stop in front of dynamic obstacles, but does not replan - a higher-level interface would have to be added.

In MoveIt2 there is ongoing work on a hybrid planner framework to implement such a schema, but this is still in development/redesign phases and I haven't seen it run on a real system.

For anything more sophisticated than the logic above you will need to implement a custom MoveGroupCapability (or run your own node instead of move_group, if you prefer that). The approach could be quite similar to 1), but you could adapt it properly for your use-case.

edit flag offensive delete link

Comments

thank you @v4hn that was very helpful. I tried using move_group.move and it re-plans, However, concerning parameters that you mentioned, I have moveit::planning_interface::MoveGroupInterface m_MoveGroup;. Do I set these with

m_MoveGroup.allowLooking(true);
  m_MoveGroup.allowReplanning(true);
  m_MoveGroup.setNumPlanningAttempts(20);
  m_MoveGroup.setMaxVelocityScalingFactor(0.10);

which interface to use for GenericTrajectory. Thanks.

zahid990170 ( 2021-10-01 05:25:19 -0600 )edit

another comment, I have searched a bit on moveit_servo. It appears to provide fast movements, and collision avoidance. But, do we need to provide a continuous goal pose for moveit_servo? The usual MoveIt plan and execute, computes the plan and then, the arm follows it. How does motion planning work with moveit_servo? The trajectory to the goal point, how is it laid down for moveit_servo to follow, thanks,

zahid990170 ( 2021-10-02 13:28:27 -0600 )edit

only allowReplanning in your list is related to the mechanism I mentioned above. Thanks for pointing out that not all parameters here are exposed in the MoveGroupInterface, I just created a patch for them.

There is no interface for GenericTrajectory either. If you want that as well (stomp might support it or can be readily extended to use it?) you could use constructMotionPlanRequest and send the request to the action server by hand.

moveit_servo is a servoing node, it's not a planner. It expects that you send online endeffector waypoints it can reach via PTP motions. Adding a planner on top of that is what hybrid planning is about.

v4hn ( 2021-10-03 14:35:22 -0600 )edit

thanks @v4hn. I do not have good knowledge of git. With your patch, you've made changes to these files: move_group_interface.h and move_group_interface.cpp. To reflect these changes on my local files, I just issued the following two commands, sudo apt-get update followed by sudo apt-get upgrade. However, I noticed the location, find / -type f -name "move_group_interface.h" is

/opt/ros/melodic/include/moveit/move_group_interface/move_group_interface.h

and, if I replicate the path given here, which is,

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

I do not find on my system; what I have at /opt/ros/melodic/share/moveit_ros, is only a pcakage.xml file.

What is the correct way to reflect these changes to my local files.

thanks,

zahid990170 ( 2021-10-04 08:09:11 -0600 )edit

thank you @v4hn, I am returning again, concerning the option 4 that you suggested i.e., "implement a custom MoveGroupCapability" can you please refer me to some help / tutorial / example as how to achieve that. thanks.

zahid990170 ( 2022-01-11 11:36:34 -0600 )edit

add a comment

answered 2021-09-27 08:05:54 -0600

Mike Scheutzow
4918 ●2 ●430 ●45

I last used MoveIt under kinetic, so my info may be stale. I believe MoveIt did frequent collision checking during both planning and execution when in cartesian mode. My recollection is that MoveIt does NOT do collision checking when executing a joint trajectory. I think that if MoveIt detects a collision during execution, it halts the motion and aborts - then it's completely left to your programming to determine what happens next.

I don't know what /collision_object topic does as I never used it. i used some class method to add new objects to the Scene. My code never advanced to the point of dynamic moving objects while the arm was also moving.

edit flag offensive delete link

add a comment

Online path replanning when encountering an obstacle MoveIt

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

Online path replanning when encountering an obstacle MoveIt edit

Comments

2 Answers

Comments

Question Tools

Stats

Related questions

Online path replanning when encountering an obstacle MoveIt