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

edit

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
  filter_chain:
  

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

Questions:

• 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