Trajectory filter failing for coordinated arms movement

edit

Hello,

I am working on coordinated motions with two 7 dof arms. I'm using diamondback. Currently, I am running only simulations via gazebo. I have been able to expose some basic move_arm functionality to two arms by creating an appropriate planning group and by using one joint_trajectory_action controller for handling control of both arms. I am using a JointPlanner via the RRT planner from ompl:

arms:
  planner_type: JointPlanner
  planner_configs:
    - RRTkConfig2 
  projection_evaluator: joint_state

  RRTkConfig2:
    type: kinematic::RRT  
    range: 0.75

Filter setup:

service_type: FilterJointTrajectoryWithConstraints
filter_chain:
  -
    name: unnormalize_trajectory
    type: UnNormalizeFilterJointTrajectoryWithConstraints
  -
    name: cubic_spline_short_cutter_smoother
    type: CubicSplineShortCutterFilterJointTrajectoryWithConstraints
    params: {discretization: 0.01}

The problems is on some coordinated movements, which bring the two arms close together, the trajectory filter doesn't seem to be constraint aware and it is removing parts of the plan which allow for collision free movement. So, in the end the arms are "colliding" with each other. I say "colliding" because they actual just pass through each other. Using rviz I can see the ompl RRT planner (prior to the filter) plans a collision free trajectory. But, after the trajectory filter snips alot of extra movement out the arms "collide". The "collisions" that happen are usually relatively minimal, i.e. just the tips of the end effectors pass through each other. I have tried different discretization values, but this doesn't help. I can increases the link padding, and this fixes the problem, but too much padding is unacceptable for our applications.

1. Why is the trajectory filter eliminating parts of the plan that allow for collision free movement?

2. What changes can I make to the filter or planner to stop this from happening? Are their other filters that are more appropriate for my application?

3. Why is gazebo allowing "collisions", i.e. letting the arms pass through each other, as opposed to having them stop each other?

Any advice is much appreciated.

Cheers,
Jonathan