Trajectory constraints lead to discontinuous joint planning

edit

A follow up from a question I asked on discourse

I am trying to get a robot arm (7 DoF Panda from Franka Emika) to move an object from position A to position B while keeping that object upright during the movement. I set the following constraints:

upright_constraints = Constraints()
upright_constraints.name = "upright"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = self.arm.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.5
orientation_constraint.absolute_y_axis_tolerance = 0.5
orientation_constraint.absolute_z_axis_tolerance = 3.14 # allow free rotation around this axis
orientation_constraint.weight = 1

First question: how are these x, y and z tolerances used? What do they mean exactly? tolerance relative to what? and on which angle system (quaternion, euler, axis angle?)

Also when I use these constraints combined with OMPL (RTTConnect) and IKFast, no plan is found most of the time. And when a plan is found, the trajectory of the object and the end effector is smooth, but the underlying joint configuration jumps from one step to the next. Here is a GIF showing how it looks like. What else can I try to fix that problem?

The configurations used to generate that path are the following:

Path contraints:

name: "upright"
joint_constraints: []
position_constraints: []
orientation_constraints: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 349
        nsecs: 157000000
      frame_id: "world"
    orientation: 
      x: -0.70761630062
      y: 0.000350377689552
      z: -0.706596714945
      w: 0.000361608916337
    link_name: "panda_link8"
    absolute_x_axis_tolerance: 3.14
    absolute_y_axis_tolerance: 0.1
    absolute_z_axis_tolerance: 0.1
    weight: 1.0
visibility_constraints: []

i don’t know why for those value (x 3.14, y 0.1, z 0.1 it managed to find a path, totally not intuitive) Here is the log from RTTConnect

[/move_group INFO 1563826183.386238577, 352.138000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826183.386365074, 352.138000000]: RRTConnect: Starting planning with 1 states already in datastructure
[/move_group INFO 1563826191.847614911, 360.447000000]: RRTConnect: Created 8 states (2 start + 6 goal)
[/move_group INFO 1563826191.847754873, 360.447000000]: Solution found in 8.462048 seconds
[/move_group INFO 1563826192.703497689, 361.262000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826193.788332731, 362.328000000]: panda_arm: Allocating specialized state sampler for state space
[/move_group INFO 1563826198.994204393, 367.406000000]: SimpleSetup: Path simplification took 7.146335 seconds and changed from 4 to 4 states

And the parameters of move_group

allow_trajectory_execution: true
controller_list:
- action_ns: follow_joint_trajectory
  default: true
  joints: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6,
    panda_joint7]
  name: panda_arm_controller
  type: FollowJointTrajectory
hand:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault, ProjESTkConfigDefault,
    LazyPRMkConfigDefault, LazyPRMstarkConfigDefault, SPARSkConfigDefault, SPARStwokConfigDefault,
    TrajOptDefault]
jiggle_fraction: 0.05
max_range: 5.0
max_safe_path_cost: 1
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_manage_controllers: true
octomap_frame: world
octomap_resolution: 0.05
ompl: {display_random_valid_states: false, link_for_exploration_tree: '', maximum_waypoint_distance: 0.0,
  minimum_waypoint_count: 10, simplify_solutions: true}
panda_arm:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault, ProjESTkConfigDefault,
    LazyPRMkConfigDefault, LazyPRMstarkConfigDefault, SPARSkConfigDefault, SPARStwokConfigDefault,
    TrajOptDefault]
panda_arm_hand:
  planner_configs: [SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault,
    BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault, RRTConnectkConfigDefault,
    RRTstarkConfigDefault, TRRTkConfigDefault, PRMkConfigDefault, PRMstarkConfigDefault,
    FMTkConfigDefault, BFMTkConfigDefault, PDSTkConfigDefault, STRIDEkConfigDefault,
    BiTRRTkConfigDefault, LBTRRTkConfigDefault, BiESTkConfigDefault ...