Description

The description here will focus on the OrientationConstraint, but this problem should also affect PositionConstraint. The issue was first flagged in MoveitTaskConstructor issue #191.

The orientation constraint given here is to make sure that the water in the attached object cuvette does not spill on the workspace. However this does not work if the link_name is an attached object or a sub-frame.

moveit_msgs::Constraints constraint;
constraint.name = "test";

Eigen::Quaterniond quat = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
                          Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());

moveit_msgs::OrientationConstraint oc;
oc.header.frame_id = "optical_table"; // See below left image
oc.link_name = "cuvette";  // Object that is attached to the eef, see below right image
oc.orientation.w = quat.w();
oc.orientation.x = quat.x();
oc.orientation.y = quat.y();
oc.orientation.z = quat.z();

oc.absolute_x_axis_tolerance = 0.4; // ~23 degrees
oc.absolute_y_axis_tolerance = 0.4; // ~23 degrees
oc.absolute_z_axis_tolerance = 2 * M_PI; // Represents the gravity axis (Pointing UP)
oc.weight = 1;
constraint.orientation_constraints.push_back(oc);

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Source or Binary build? source
• If binary, which release version?
• If source, which branch? master

Steps to reproduce

Add an object and attach it to you eef. Create an orientation constraint from point A to point B. You could use MTC to plan and visualize the results to see multiple solutions because there are some valid solutions returned. (See expected and actual behavior)

Expected behaviour

Open the file fixed_orientation_constraint.mp4 from this link. You will see that the orientation constraint is respected for multiple cases and that there are no false negative/positive. Compare with the actual behavior below.

Actual behaviour

Open the file default_orientation_constraint.mp4 from this link. Some orientation constraints are not respected.

Backtrace or Console output

I have added backtraces for different path that triggers OrientationConstraint::configure and OrientationConstraint::decide:
Bactrace#1
Bactrace#2
Bactrace#3

Analysis

The generatePlan will trigger the isStateValid which will trigger the resolveConstraintFrames. If successfull, it will then trigger the isPathValid which will NOT trigger the resolveConstraintFrames. So when the Orientation::configure tries to get the link model, it fails to retrieve the link because it is an attached object.

I think there is another issue at hand. There is clearly something wrong with the isStateValid and/or the IKConstraintSampler::validate. Because when something fails there, the isPathValid is not triggered. And when I force these 2 validation to always pass, there are a lot of new good solutions found! See videos above.

When all the above is fixed, I think there is another problem, because the isPathValid through OrientationConstraint::decide gives bad solutions. Here is the fix the problem:

// When the ResolvedConstraintFrames changes the oc.orientation...

// Actual
// desired_rotation_matrix_ = world_R_desired * object_R_link
// desired_rotation_matrix_inv_ = link_R_object * desired_R_world
// diff rotation =  link_R_object * desired_R_world * world_R_link
Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());

// Fix Option1
// desired_rotation_matrix_ = world_R_desired
// desired_rotation_matrix_inv_ = desired_R_world
// diff rotation =  desired_R_world * world_R_link * link_R_object
Eigen::Isometry3d diff(desired_rotation_matrix_inv() * state.getGlobalLinkTransform(link_model_).linear() * link_to_object_rotation_matrix_);

// Fix Option2
// desired_rotation_matrix_ = world_R_desired
// desired_rotation_matrix_inv_ = desired_R_world
// diff rotation =  desired_R_world * world_R_object
Eigen::Isometry3d diff(desired_rotation_matrix_inv() * state.getFrameTransform(link_name_).linear());

I think we need to keep an extra rotation in the OrientationConstraintMsgs for the original desired_rotation. Or we could store the real_link_name and the link_name(can be an attached body or a sub-frame).

I have this branch that fixes this issue. Note that this is a LAZY UGLY HACK, it only fixes the issue for the OrientationConstraint when the link_name is fixed. Also, read the Readme for this hack to work.

Discussion

As discussed with @v4hn, we should probably add the ResolvedConstraints before the planning pipeline. This would reduce calls to ResolvedConstraints that is triggered 2 times at the moment (3 times if you fix the isPathValid). Also there would be no need to add the ResolvedConstraints adapter for each planning pipeline.

When the isStateValid and the IKConstraintSampler::validate are forced to PASS, I don't understand why it fixes some issues. I think that these validations, for path_constraints, is not necessary because it is rechecked in isPlanValid for the trajectory. This clearly speedup quite a bit the process (See videos and compare the task time). This is based on a limited knowledge of this library though...