# Trajectory constraints lead to discontinuous joint planning

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.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:
-
seq: 0
stamp:
secs: 349
nsecs: 157000000
frame_id: "world"
orientation:
x: -0.70761630062
y: 0.000350377689552
z: -0.706596714945
w: 0.000361608916337
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 ...
edit retag close merge delete