Different planning results between c++ and python interface

[jsbyysheng]

Description

Overview of your issue here.

Your environment

• ROS Distro: [Noetic]
• OS Version: e.g. Ubuntu 20.04
• Source build from master branch

Steps to reproduce

In rivz, using motion planning plugin to plan robot move in z+0.05 get correct results. (Have tried for 10 times)

[ INFO] [1654768863.379805972]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1654768863.379852136]: Using planning pipeline 'ompl'
[ INFO] [1654768863.381383438]: Planner configuration 'manipulator[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1654768863.381999212]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.382085290]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.382173744]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.382265368]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.393834397]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654768863.394000046]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.394359550]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.395009513]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.395179181]: ParallelPlan::solve(): Solution found by one or more threads in 0.013464 seconds
[ INFO] [1654768863.395600964]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.395681381]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.395912000]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.396021091]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.396571780]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.397003931]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.397546423]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
[ INFO] [1654768863.397604034]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654768863.397793616]: ParallelPlan::solve(): Solution found by one or more threads in 0.002359 seconds
[ INFO] [1654768863.398122700]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.398200117]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654768863.399551581]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654768863.399995843]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
[ INFO] [1654768863.400192984]: ParallelPlan::solve(): Solution found by one or more threads in 0.002201 seconds
[ INFO] [1654768863.403346263]: SimpleSetup: Path simplification took 0.003037 seconds and changed from 3 to 2 states

However, when using python interface, the results are all weird. (Have tried for many times.)

move_group.set_planning_pipeline_id('ompl')
move_group.set_planner_id('manipulator[RRTConnect]')
move_group.set_planning_time(1.0)
move_group.set_num_planning_attempts(10)
move_group.set_max_velocity_scaling_factor(0.1)
move_group.set_max_acceleration_scaling_factor(0.1)

pose_goal = move_group.get_current_pose().pose
pose_goal.position.x = pose_goal.position.x
pose_goal.position.y = pose_goal.position.y 
pose_goal.position.z = pose_goal.position.z + 0.05
move_group.clear_pose_targets()
move_group.set_pose_target(pose_goal)
result, plan, fraction, others = move_group.plan()

[ INFO] [1654769004.327905298]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1654769004.327931945]: Using planning pipeline 'ompl'
[ INFO] [1654769004.329302351]: Planner configuration 'manipulator[RRTConnect]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1654769004.329758681]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.329808425]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.329859890]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.329941552]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.351190517]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654769004.351274890]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654769004.351531769]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
[ INFO] [1654769004.352135813]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654769004.352277536]: ParallelPlan::solve(): Solution found by one or more threads in 0.022740 seconds
[ INFO] [1654769004.352579709]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.352647052]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.352729478]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.352785743]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.354010976]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
[ INFO] [1654769004.354815474]: manipulator/manipulator[RRTConnect]: Created 5 states (3 start + 2 goal)
[ INFO] [1654769004.355670929]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654769004.356622430]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654769004.356788503]: ParallelPlan::solve(): Solution found by one or more threads in 0.004378 seconds
[ INFO] [1654769004.357012408]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.357073448]: manipulator/manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1654769004.358520461]: manipulator/manipulator[RRTConnect]: Created 4 states (2 start + 2 goal)
[ INFO] [1654769004.358965464]: manipulator/manipulator[RRTConnect]: Created 5 states (2 start + 3 goal)
[ INFO] [1654769004.359146759]: ParallelPlan::solve(): Solution found by one or more threads in 0.002230 seconds
[ INFO] [1654769004.376688942]: SimpleSetup: Path simplification took 0.004986 seconds and changed from 3 to 2 states

[christinaionescu]

Hi,

I think I have a similar issue.
For me planning in rviz with the planning plugin also works fine but if I use the C++ interface (similar as you did with python) it does find a path but its completely non-optimal and makes huge movements although the goal is just a small translation.
Have you found a solution?

[jsbyysheng]

Hi,

I think I have a similar issue. For me planning in rviz with the planning plugin also works fine but if I use the C++ interface (similar as you did with python) it does find a path but its completely non-optimal and makes huge movements although the goal is just a small translation. Have you found a solution?

Unfortunately, I did not find a solution. Before, I reviewed the plugin code and thought this problem would not occur in c++. Ops.....

[christinaionescu]

I used the same code before on melodic which was fine so I guess it is a noetic problem?

[jsbyysheng]

I used the same code before on melodic which was fine so I guess it is a noetic problem?

Maybe. However, the rviz and the motion planning plugin work well in noetic.

[christinaionescu]

Would you mind sending me your c++ code? maybe I can find some error in my c++ code then :)
Are you also using https://github.com/fmauch/universal_robot/tree/calibration_devel in calibration devel?

[rhaschke]

Thanks for pointing out this regression! The bug here is related to #3119 and was introduced by #2872:
Using sample() instead of project(), the IK goal sampler uses a random seed state instead of the current state. This lead to a weird joint pose as the target resulting in the observed behavior. rviz computes the goal (joint) pose directly via IK and thus isn't affected by the problem.
@v4hn, can we agree on a solution to #3119? I'm fine to follow your suggestion:

... we should actually remove project and remove the random initialization from our sample calls.

[jsbyysheng]

Would you mind sending me your c++ code? maybe I can find some error in my c++ code then :) Are you also using https://github.com/fmauch/universal_robot/tree/calibration_devel in calibration devel?

I am using calibration devel. I am sorry I am too busy to try the cpp code in noetic. I advise you to take a look at the official motion planning plugin code, it will be helpful.

[christinaionescu]

@rhaschke thank you! changing this line from sample() to project() already worked for me. :) that saved me a lot of more debugging time.

[v4hn]

@captain-yoshi This test clearly demonstrates the expected performance drop that you could not show in your tests. It's probably worth looking into this for the benchmark suite because this kind of bug (picking an inappropriate IK solution) has come up many times in the past.

[captain-yoshi]

Responded in appropriate issue.

[jsbyysheng]

@rhaschke thank you! changing this line from sample() to project() already worked for me. :) that saved me a lot of more debugging time.

Can I know which line did you change? @christinaionescu Unfortunately, I have tested the newest commit but nothing changed in python interface. @rhaschke

[rhaschke]

@jsbyysheng, you can either revert the offending PR: #2872
Or you can apply the not-yet-merged fixup/cleanup PR: #3170

[jsbyysheng]

@jsbyysheng, you can either revert the offending PR: #2872 Or you can apply the not-yet-merged fixup/cleanup PR: #3170

Thank you for so quick reply, it works.