MoveIt orientation constraint causes arbitrary motion

Question

I'm using the MoveIt Python interface. I've noticed that adding an orientation constraint. When I attempt to add a constraint so that the gripper maintains an upright orientation, not only does the gripper not maintain an upright orientation, it also starts moving arbitrarily.

https://gist.github.com/dqii/2c9c091413b21b782e5e7f0c0408a8c0 Here's my code

https://www.youtube.com/watch?v=MgorLi79B1s Here's a video... where you see that it first moves from a to b smoothly, and once the orientation constraints are added, its movements are jerky.

Is this typical of MoveIt? Am I missing something basic?

---

Originally posted by infantasy on ROS Answers with karma: 31 on 2020-12-06

Post score: 1

---

Original comments

Comment by madcat on 2022-02-21:
Hey, I am facing the same issue. I tried to make the end-effector level with the orientation constraint. The path is jerky and the end effector often flips in the path.

In ompl_planning.yaml:

RRTConnectkConfigDefault:
  type: geometric::RRTConnect
  range: 0.0
  enforce_joint_model_state_space: True

for constraints:

orien_const = OrientationConstraint()
orien_const.link_name = self.move_group.get_end_effector_link()
orien_const.header.frame_id = 'panda_link0'

orien_const.orientation.x = q[0] # q is the quaternion of the target pose
orien_const.orientation.y = q[1]
orien_const.orientation.z = q[2]
orien_const.orientation.w = q[3]

orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = pi # ignore yaw
orien_const.weight = 1.0

Any advice is welcome and thanks!

Answer 1

I am not 100% on this, but to me this seems like a problem i had with moveit constrained planning in the past as well. My guess: This is caused by "jumps" in the IK, where each sampled point is individually fine, but the interpolation between them leads to these "flips". This would describe why some segments of the trajectory look just fine, but the transitions between them are so messed up. Some discussion on this:

• This issue describes the problem
• This old mail thread discusses some ways to work around this, among others changing/parameterizeing the IK algorithm used.

One sure way to get rid of this should be using position-only ik as described here, but this can drastically increase your planning times. Changing the range parameter in config/ompl_planning.yaml was less drastic, but still helped in my case. One simple thing to help with the increased planning time was relaxing my orientation tolerances, e.g. in my case i decided that a tolerance of 0.3 was more then enough.

Could you try validating that this is the problem (i.e. checking if position-only ik / the other approaches listed here improve your results)? If this is the case it would also be interesting to hear what change could bring the biggest improvements for your setup.

---

Originally posted by RobertWilbrandt with karma: 176 on 2020-12-07

This answer was NOT ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by infantasy on 2020-12-07:
Thanks! I'll give those a try and document everything here. Here's what I've done so far. I was not sure which range parameter you changed since there were many, so I did not try that.

1. Changed tolerance from 0.1 => 0.3, no effect. Kept new tolerance for subsequent experiments.
2. Changed tolerance from 0.1 => 1.5, no effect... sometimes succeeds on first waypoint with constraints but infrequent.
3. KDL IK => Trac-IK, no effect.
4. KDL IK => Trac IK && position only IK: planning fails with constraints
5. Position only IK (KDL) + Timeout 0.05 => 0.5, planning fails both with and without constraints.
6. enforce_joint_model_state_space sometimes partially succeeded... I added it to hand in ompl_planning, but planning failed for the second waypoint with constraints, and sometimes succeeds for the first waypoint with constraints but infrequently.

Comment by RobertWilbrandt on 2020-12-07:
Hey, could you post the error messages for the "position only IK" and "enforce_joint_model_state_space" cases? It might be that this is just a planning timeout, in this case you should be able to use move_group.set_planning_time to increase it (mostly for getting a working solution now).

For the range parameter: You need to set this for the specific planner you use. OMPL is a library full of probabilistic planners, and if you don't specify any the planning will default to RRTConnect (if i remember correctly). You should be able to look up your default planner for your move group at the bottom of the config/ompl_planning.yaml file, you are looking for the default_planner_config section of the panda_arm section. Then you can go to the upper part and change the range parameter in the section belonging to your planner.

Comment by RobertWilbrandt on 2020-12-07:
One important point i forgot to mention: Did you actually verify that a trajectory complying with the orientation constraints exists? This is not always totally obvious, as joint limits are just unintuitive sometimes. I don't really have a good way to test this, in my case i was able to find complying trajectories "by hand" and then use these as test benchmarks for my parameter changes.

This is in some way detached from the main discussion, as the planner should be able to tell you it didn't find a trajectory instead of just ignoring the constraint. It is just a good sanity check before getting really frustrated by unsuccessfull planning attempts...

Answer 2

I am pretty sure this is precisely the problem @RobertWilbrandt explained:

This is caused by "jumps" in the IK, where each sampled point is individually fine, but the interpolation between them leads to these "flips".

Based on my limited experience with orientation constraint planning I think using enforce_joint_model_state_space and increasing the planning time (which you both already mentioned) is your best option at the moment in MoveIt.

There is also the planning with a constraint database option if your setup stays the same and you want faster planning, but I've not tested this myself yet.

The position only IK solution @RobertWilbrandt proposed is interesting, I'm not even sure it was intended to solve this problem, but it certainly could help a lot depending on the implementation details inside MoveIt.

In a couple of months, you will hopefully have another option when this pull request and it's little brother for orientation constraints gets merged.

Edit: and if your final applications involved real-time planning moveit_servo might also be interesting to look at :)

---

Originally posted by jeroendm with karma: 168 on 2020-12-08

This answer was NOT ACCEPTED on the original site

Post score: 4