search

moveit / moveit

:robot: The MoveIt motion planning framework
http://moveit.ros.org
BSD 3-Clause "New" or "Revised" License
1.72k stars 950 forks source link

Chomp planning adapter not successful #1100

Open simonschmeisser opened 6 years ago

simonschmeisser commented 6 years ago

Description

i just tried to test the new chomp planning adapter by following the moveit-tutorials PR https://github.com/ros-planning/moveit_tutorials/pull/220/files

Is it supposed to work already? Here's the output from trying to plan towards a JointPosition target. Using a pose target gives me an error that pose target are not yet supported

heres the log output:

[ INFO] [1539879169.238186970]: Trajectory execution is managing controllers
[ INFO] [1539879169.281965849]: Initializing OMPL interface using ROS parameters
[ INFO] [1539879169.311246517]: Using planning interface 'OMPL'
[ INFO] [1539879169.321049326]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1539879169.321675111]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1539879169.322173276]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1539879169.322686598]: Param planning_time_limit was not set. Using default value: 10
[ INFO] [1539879169.323866387]: Param smoothness_cost_weight was not set. Using default value: 0.1
[ INFO] [1539879169.324278401]: Param obstacle_cost_weight was not set. Using default value: 1
[ INFO] [1539879169.324805010]: Param learning_rate was not set. Using default value: 0.01
[ INFO] [1539879169.325286192]: Param smoothness_cost_velocity was not set. Using default value: 0
[ INFO] [1539879169.325709000]: Param smoothness_cost_acceleration was not set. Using default value: 1
[ INFO] [1539879169.326138907]: Param smoothness_cost_jerk_ was not set. Using default value: 0
[ INFO] [1539879169.326580601]: Param ridge_factor_ was not set. Using default value: 0
[ INFO] [1539879169.327004692]: Param use_pseudo_inverse_ was not set. Using default value: 0
[ INFO] [1539879169.327443284]: Param pseudo_inverse_ridge_factor was not set. Using default value: 0.0001
[ INFO] [1539879169.327843265]: Param joint_update_limit was not set. Using default value: 0.1
[ INFO] [1539879169.328292734]: Param min_clearence was not set. Using default value: 0.2
[ INFO] [1539879169.328770018]: Param collision_threshold_ was not set. Using default value: 0.07
[ INFO] [1539879169.329189810]: Param use_stochastic_descent was not set. Using default value: 1
[ INFO] [1539879169.329624597]: Param trajectory_initialization_method was not set. Using New value as: fillTrajectory
[ INFO] [1539879169.329655537]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1539879169.329677137]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1539879169.329698202]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1539879169.329715518]: Using planning request adapter 'CHOMP Optimizer'

[ INFO] [1539879256.305765453]: Collision in direct moving at 0,100000, will start planner
[ INFO] [1539879256.306467594]: Collision between GehaeuseKiste and hand_frame
[ INFO] [1539879256.306525777]: Collision between camera_mount and pane_back
[ INFO] [1539879256.307241481]: Planner configuration 'manipulator[RRTConnectkConfigDefault]' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1539879256.310127868]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1539879256.310387997]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1539879256.310867051]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1539879256.311219996]: manipulator[RRTConnectkConfigDefault]: Starting planning with 1 states already in datastructure
[ INFO] [1539879259.407594113]: manipulator[RRTConnectkConfigDefault]: Created 2895 states (959 start + 1936 goal)
[ INFO] [1539879259.625266513]: manipulator[RRTConnectkConfigDefault]: Created 2947 states (1112 start + 1835 goal)
[ INFO] [1539879259.720092189]: manipulator[RRTConnectkConfigDefault]: Created 3051 states (1154 start + 1897 goal)
[ INFO] [1539879260.045767322]: manipulator[RRTConnectkConfigDefault]: Created 3175 states (1178 start + 1997 goal)
[ INFO] [1539879261.680332197]: ParallelPlan::solve(): Solution found by one or more threads in 5,370625 seconds
[ INFO] [1539879265.757746021]: SimpleSetup: Path simplification took 4,077072 seconds and changed from 219 to 5 states
[ INFO] [1539879265.759121355]: Configuring Planning Scene for CHOMP ....
[ INFO] [1539879266.612294819]: Setting joint joint_a1 to position -8.68638e-13
[ INFO] [1539879266.612367460]: Setting joint joint_a2 to position -0.495995
[ INFO] [1539879266.612402780]: Setting joint joint_a3 to position 2.33913
[ INFO] [1539879266.612415060]: Setting joint joint_a4 to position -9.14823e-14
[ INFO] [1539879266.612453823]: Setting joint joint_a5 to position -0.271432
[ INFO] [1539879266.612496122]: Setting joint joint_a6 to position -6.95887e-13
[ INFO] [1539879266.618006915]: CHOMP trajectory initialized using method: fillTrajectory 
[ INFO] [1539879266.618048381]: The following collision detectors are active in the planning scene.
[ INFO] [1539879266.618077491]: HYBRID
[ INFO] [1539879266.618129026]: Active collision detector is: HYBRID
[ INFO] [1539879267.141899747]: First coll check took 0.523736416
[ INFO] [1539879267.183473549]: Forward kinematics took 0.030875923
[ INFO] [1539879267.183902314]: iteration: 0
[ INFO] [1539879267.207643360]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1539879267.207672332]: Chomp path is collision free
[ INFO] [1539879267.207683834]: Terminated after 1 iterations, using path from iteration -1
[ INFO] [1539879267.207694742]: Optimization core finished in 0,055113 sec
[ INFO] [1539879267.207708129]: Time per iteration 0.0551256
result: val: -14

num waypoints: 621

From MoveItErrorCodes: GOAL_CONSTRAINTS_VIOLATED = -14,

Your environment

chomp_planning.yaml


trajectory_initialization_method: "fillTrajectory"
max_iterations: 10
max_iterations_after_collision_free: 5
simonschmeisser commented 6 years ago

There are two cases,

if you seed it with a valid trajectory (almost always true in this use case) it will ignore max_iterations_after_collision_free ... so it does nothing

https://github.com/ros-planning/moveit/blob/75c5f32329b59fbf0bac43e7253e54ef0c9f5a97/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp#L404

commenting out that line however only brings me so far ... now it will always iterate into collision:

[ INFO] [1539957256.575202782]: Configuring Planning Scene for CHOMP ....
[ INFO] [1539957257.407219061]: Setting joint joint_a1 to position -1.20428
[ INFO] [1539957257.407275121]: Setting joint joint_a2 to position -0.670528
[ INFO] [1539957257.407326223]: Setting joint joint_a3 to position 2.09478
[ INFO] [1539957257.407344712]: Setting joint joint_a4 to position -9.14823e-14
[ INFO] [1539957257.407366592]: Setting joint joint_a5 to position -0.271432
[ INFO] [1539957257.407384098]: Setting joint joint_a6 to position -6.95887e-13
[ INFO] [1539957257.407597318]: CHOMP trajectory initialized using method: fillTrajectory 
[ INFO] [1539957257.407726206]: The following collision detectors are active in the planning scene.
[ INFO] [1539957257.407750127]: HYBRID
[ INFO] [1539957257.407773164]: Active collision detector is: HYBRID
[ INFO] [1539957257.926845148]: First coll check took 0.519028888
[ INFO] [1539957257.968133172]: Forward kinematics took 0.030162728
[ INFO] [1539957257.968504038]: iteration: 0
[ INFO] [1539957257.991888202]: Chomp Got mesh to mesh safety at iter 0. Breaking out early.
[ INFO] [1539957258.018371138]: Forward kinematics took 0.026439103
[ INFO] [1539957258.045701039]: Forward kinematics took 0.026936546
[ INFO] [1539957258.072316638]: Forward kinematics took 0.026181787
[ INFO] [1539957258.099451473]: Forward kinematics took 0.026799219
[ INFO] [1539957258.125387927]: Forward kinematics took 0.025335508
[ERROR] [1539957258.125679102]: Chomp path is not collision free!
[ INFO] [1539957258.125714749]: Terminated after 6 iterations, using path from iteration 2
[ INFO] [1539957258.125728253]: Optimization core finished in 0,187774 sec
[ INFO] [1539957258.125761852]: Time per iteration 0.0313013
result: val: -2
liuxin00738 commented 5 years ago

I got the same issue. Could anyone pointing to a direction to solve it?

simonschmeisser commented 5 years ago

Hi @liuxin00738 I haven't committed anything of the above yet. You might want to have a look at the code and start fixing some small and obvious bugs and see if that leads you anywhere. There are many small and obvious bugs! Maybe it then actually starts working already. Maybe you fix all of the obvious bugs and it still does not work. My trust in this algorithm working out in the end is limited. But it will be easier to evaluate if someone invests a day or two in fixing small issues. Please open a PR with those then

Hugal31 commented 4 years ago

I think I have the same problem:

I spent some time debugging why the CHOMP optimizer (configured as shown in this tutorial) was showing the error "Goal constraints are violated: ur5/shoulder_pan_joint" (error -14).

After some debugging with GDB, it appears the path "optimized" by Chomp is not within the goal tolerance_above / tolerance_below values, whereas the input of the algorithm, computed with OMPL, is within the constraint.

For instance, when I aks for a joint to be at 1.3380 rad, with 0.0001 tolerance above or below, OMPL gives 1.33799 whereas CHOMP gives 1.3353.

Here is my CHOMP configuration file:

planning_time_limit: 10.0
max_iterations: 200
max_iterations_after_collision_free: 5
smoothness_cost_weight: 0.1
obstacle_cost_weight: 1.0
learning_rate: 0.01
smoothness_cost_velocity: 0.0
smoothness_cost_acceleration: 1.0
smoothness_cost_jerk: 0.0
ridge_factor: 0.01
use_pseudo_inverse: false
pseudo_inverse_ridge_factor: 1e-4
joint_update_limit: 0.5
collision_clearence: 0.2
collision_threshold: 0.07
use_stochastic_descent: true
enable_failure_recovery: true
max_recovery_attempts: 5
trajectory_initialization_method: "fillTrajectory"

Is there something I don't understand about CHOMP? Is it one of its limits, and should I lower my tolerance? If yes, how can I do it with the RVIZ MoveIt! plugin?

akhilkp777 commented 1 year ago

does chomp supports x y z roll pitch yaw as input for a goal pose