compute_cartesian_path obstacle avoidance?

Question

Hello ROS Community!

I have the following scenario in gazebo: robotic arm + wall + obstacle (see the figure)

Based on (simulated) kinect sensor data i am able to detect the obstacle and determine the main way-points trough which the selected surface of the wall can be covered. (i.e. i have the waypoints that describe the complete coverage path). see the following image:

Before i call the compute_cartesian_path i always test these waypoints individually and all of them are reachable by the robot.

Once i call the compute_cartesian_path it fails with small percentages (1-3%).

BUT, IF i remove the obstacle (ie. delete the wood cube object in gazebo) BUT KEEP the waypoints (that corresponded to the situation when the obstacle was present) than the compute_cartesian_path succesfully calculates the path and the robot can successfully execute the trajectory.

Why the compute_cartesian_path doen't succeed if the obstacle is present?

I call it in the following way:

(plan, fraction) = group.compute_cartesian_path(
          self.waypoints,  # waypoints to follow
          step,  # eef_step
          0.0,  # jump_threshold
          True) #avoid_collisions

In the moveit config package i load the following sensor:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /rgbd_camera/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

And in RVIZ i am able to see the OctoMap.

Thank you in advance!

Best regards.

---

Originally posted by akosodry on ROS Answers with karma: 121 on 2018-09-17

Post score: 0

Answer 1

setting jump_threshold to 0 may be part of the issue, see here for a description of that argument.
Also, are any of your waypoints in collision with he obstacle?

---

Originally posted by jrgnicho with karma: 486 on 2018-09-17

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by akosodry on 2018-09-18:
@jrgnicho Thanks for the reply. I tried the jump_threshold to be 0.1 and also 100000 but it did not change the outcome. I updated the second image in my post with the waypoint numbers. Every waypoint is reachable and all of them are outside of the obstacle territory. BUT

Comment by akosodry on 2018-09-18:
BUT moving on a straight line from waypoint 16 to waypoint 17 is not possible since there is the obstacle (wood cube). BUT i expected (maybe wrongly) that compute_cartesian_path can cleverly return a plan that bypasses the obstacle when moving from wp16 to wp17. Any tips,ideas? Thanks

Comment by gvdhoorn on 2018-09-18:\

BUT i expected (maybe wrongly) that compute_cartesian_path can cleverly return a plan that bypasses the obstacle when moving from wp16 to wp17. Any tips,ideas? Thanks

No, that is not how it works.

computeCartesianPath(..) does not "plan" (ie: come up with paths itself), it's simple linear ..

Comment by gvdhoorn on 2018-09-18:
.. interpolation between the waypoints, similar to how a traditional industrial robot controller would do it (ie: go through your obstacle).

So you'll have to make sure that linear interpolation between your waypoints works (or accept that only part of the interpolation will be successful).

Comment by akosodry on 2018-09-18:
@gvdhoorn Thanks for the reply. Can you recommend then what solution can be implemented to go from waypoint 16 to waypoint 17 (see the images in the post)? Should i manually add wpoints to bypass the obstacle? What is the meaning of avoid_collisions = true then?

Comment by akosodry on 2018-09-18:
Or using computeCartesianPath is not the right way to solve the described problem?

Comment by gvdhoorn on 2018-09-18:
According to the documentation:

Collisions are avoided if avoid_collisions is set to true. If collisions cannot be avoided, the function fails.

If due to robot kinematics there are configurations of your robot that allow computeCartesianPath(..) to "work around" collisions, then it will ..

Comment by gvdhoorn on 2018-09-18:
.. use those.

If all of the solutions for a waypoint are still in collision, "the function fails", as the documentation states.

http://docs.ros.org/kinetic/api/moveit_ros_planning_interface/html/classmoveit_1_1planning__interface_1_1MoveGroupInterface.html#afd29c4dc55b10564102cf393cd38c71d

Comment by gvdhoorn on 2018-09-18:
Interpolation != planning.

Afaik there are no "Cartesian planners" readily available as ROS pkgs (but I could be wrong of course, do search around), so it might require some work-arounds.

Adding additional waypoints should work.

Comment by akosodry on 2018-09-18:
Okey! This helped me a lot! Thanks! But instead of manual wp addition+computeCartesianPath isnt there a more clever way to do the complete coverage of the surface with obstacles?

Comment by akosodry on 2018-09-18:
@gvdhoorn For example i read about the Descartes cartesian path-planner. Does it suit better in the problem?

Comment by gvdhoorn on 2018-09-18:
You could see whether tesseract can help here.

Note though: this is a guess and will require you to do things differently (ie: no MoveIt commander).

Comment by akosodry on 2018-09-18:
Yea i naively thought on a moveit commander based method :) I will extend my script with wp addition and see what will the computeCartesianPath do. Thank you for the help!!

Comment by jrgnicho on 2018-09-18:
Instead of adding waypoints you could break up your trajectory into linear and free motion segments where you move linearly from the start position to the waypoint closest to the obstacle, then you "plan" from that point to the next point past the obstacle with a start-to-goal planner such as OMPL

Comment by jrgnicho on 2018-09-18:
and then you again move linearly from that point to the end of your trajectory.

Comment by akosodry on 2018-09-18:
@jrgnicho Thanks for the reply! Its sounds very good!!

Comment by akosodry on 2018-09-18:
@jrgnicho can you recommend which planner is the most suited for the described problem? there are quite a lot: KPIECE, RRT, RRTConnect, SPARS and so on.... (in the moveit setup assistant it is possible to choose the default planner)

Comment by jrgnicho on 2018-09-18:
@akosodry I believe the one planner I tend to use is RRTConnectkConfigDefault