Planning an arm-motion using ompl_planning and trajectory_filter [closed]

asked 2011-06-15 23:46:21 -0500

Witalij Siebert gravatar image

updated 2011-06-15 23:49:47 -0500

Dear community,

I am working on the cob3 robot arm using motion-planning from the cob_arm_navigation stack.

It is modified successfully so far to use it on the real robot arm. To detect collision objects the arm uses a TOF camera publishing point clouds.

After self filtering the point cloud with the robot model the arm is able to detect obstacles and avoid collisions.

But there are sometimes problems with the trajectory_filter if i want to move the arm from one collision-free position to another with an obstacle in between using OMPL planner.

In a few cases it is possible for the arm to find a path and move it collision-free.
But mostly the OMPL planner finds a solution and the trajectory_filter declare the trajectory invalid (later on some example output).
Then the OMPL planner starts over to look for an other simple solution and so on until there is maybe a valid trajectory that the arm will move, if not there is an output with the Warning Planner trajectory collides and the planning ends.

Thats the output to screen if there is a problem with the trajectory_filter:

( INFO) : Motion planning succeeded
( INFO) : Done planning. Transitioning to control
( INFO) : Got trajectory with 108 points
( INFO) : Trajectory filter took 4.006965 seconds
( INFO) : Collision between body in namespace points and link arm_5_link
(ERROR) : Trajectory invalid
( WARN) : Filtered trajectory collides
(ERROR) : Move arm will abort this goal. Will replan
( INFO) : Displaying move arm joint goal.
( INFO) : Constraint violated:: Joint name:arm_1_joint, value: 3.004306, Constraint: 0.210000, tolerance_above: 0.400000, tolerance_below: 0.400000
( WARN) : State violates goal constraints.

After several outputs like that it is possible that there is a valid trajectory and the arm moves collision-free to its goal. That looks like this:

( INFO): Motion planning succeeded
( INFO): Done planning. Transitioning to control
( INFO): Got trajectory with 29 points
( INFO): Trajectory filter took 4.005894 seconds
( INFO): Trajectory validity check was successful
( INFO): Sending trajectory with 28 points and timestamp: 1308219869.357828
( INFO): Joint: 0 name: arm_1_joint
( INFO): Joint: 1 name: arm_2_joint
( INFO): Joint: 2 name: arm_3_joint
( INFO): Joint: 3 name: arm_4_joint
( INFO): Joint: 4 name: arm_5_joint
( INFO): Joint: 5 name: arm_6_joint
( INFO): Joint: 6 name: arm_7_joint
( INFO): Reached goal

But the OMPL planner is always successful.

My Configs:

  • OMPL Planner: SBLkConfig1
  • Filters:

    service_type: FilterJointTrajectoryWithConstraints

    • name: unnormalize_trajectory
      type: UnNormalizeFilterJointTrajectoryWithConstraints
    • name: cubic_spline_short_cutter_smoother
      type: CubicSplineShortCutterFilterJointTrajectoryWithConstraints
      params: {discretization: 0.01}


  • What can be the problem that the ompl planner find a solution and the trajectory_filter declares it to invalid?

  • Is it possible to change some parameters of the trajectory_filter or other config files that will help to find valid trajectories more often?

  • Is there a list of parameters the trajecotry_filter is using?

If you have some question or the problem is not described well enough let me know.
I'd be pleased to hear some answers, if someone have a possible solution or just a suggestion.

Regards Witalij

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2012-02-24 19:20:47


Are you using cturtle or diamondback? The planning components in cturtle have been much improved with the new ompl library and the ompl_ros_interface in diamondback.
Sachin Chitta gravatar image Sachin Chitta  ( 2011-06-28 12:47:53 -0500 )edit
Hello, Sachin. I'm using ROS diamondback.
Witalij Siebert gravatar image Witalij Siebert  ( 2011-06-29 21:23:06 -0500 )edit
Its often the resolution of the collision checking that is the problem. I would suggest waiting for e-turtle which will bring out a set of tools that makes debugging this kind of thing easier. More information is here:
Sachin Chitta gravatar image Sachin Chitta  ( 2011-07-14 05:24:24 -0500 )edit