New "parameterization" field for OrientationConstraint message

[JeroenDM]

I think it could be useful to add an extra field to the message for orientation constraints that specifies how the orientation should be parameterized when testing the constraints. Currently, the tolerances are applied to the XYZ Euler angles (intrinsic rotations), but they could be just as well be applied to any three number parameterization.

While working on #2092, I seem to notice that exponential coordinates (rotation angle * rotation axis) performed better for projection-based sampling.

My proposal would be to add something like this:

# How should the orientation be parameterized with three numbers
# the absolute axis tolerances are applied to these numbers
uint8 parameterization

# The different options for the orientation constraint parameterization
uint8 XYZ_EULER_ANGLES=0
uint8 EXPONENTIAL_COORDINATES=1

Edit An alternative proposed by @felixvd is to use separate fields for the two different tolerances.

# (optional) Angle tolerance. Only one of the below representation may be used. Not all solvers may support all of them.
# in Euler angles
float64 absolute_x_axis_tolerance
float64 absolute_y_axis_tolerance
float64 absolute_z_axis_tolerance
# in exponential coordinates [add link or explanation here]
float64 value_1
float64 value_2
float64 value_3

The implementation in kinematic_constraints.cpp could look like this: (we would also need to update the constraint samplers)

ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::RobotState& state, bool verbose) const
{
    /* ... some other code ... */
    Eigen::Isometry3d diff(desired_rotation_matrix_inv_ * state.getGlobalLinkTransform(link_model_).linear());
    if (parameterization_ == Parameterization::EXPONENTIAL_COORDINATES)
    {
      angle_axis = Eigen::AngleAxisd(diff.linear());
      xyz = angle_axis.axis() * angle_axis.angle();
      xyz(0) = fabs(xyz(0));
      xyz(1) = fabs(xyz(1));
      xyz(2) = fabs(xyz(2));
    }
    else if (parameterization_ == Parameterization::XYZ_EULER_ANGLES)
    {
      // use Euler angles by default
      // 0,1,2 corresponds to XYZ, the convention used in sampling constraints
      xyz = diff.linear().eulerAngles(0, 1, 2);
      xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0)));
      xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1)));
      xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2)));
    }
    else 
    { /*default */ }
  }

  bool result = xyz(2) < absolute_z_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
                xyz(1) < absolute_y_axis_tolerance_ + std::numeric_limits<double>::epsilon() &&
                xyz(0) < absolute_x_axis_tolerance_ + std::numeric_limits<double>::epsilon();

   /* ... some other code ... */
}

A typical planning problem where we want to keep a gripper level to the ground, that can also be solved with the new constraints.

This problem could also be specified using tolerances on Euler angles, but for planners that don't like Euler angles, it is useful to have this alternative. For example, TrajOpt does not use Euler angles.

[JeroenDM]

@felixvd @ommmid any thought on this? It would bring me a step closer to adding Orientation constraints to the Constrained planning part of the OMPL interface. (moveit/moveit#2273). Or do you see alternative solutions?

[ommmid]

In the current orientation constraint message, the tolerances are referred to as optional axis-angle error tolerances specified. Does this axis-angle already means exponential coordinates? Why does it not say Euler-agnels?
I would definitely go for two different representations (as you suggest) for orientation constraints as far as they are not confused by the one that is there in the message already.

[felixvd]

I don't use the OrientationConstraint message a lot, so I also had to look it up and got confused by the current description.

I agree that clarifying and separating the two representations is good, but I wonder about the parametrization switch that encapsulates the logic in kinematic_constraints.cpp. Should the representation of the orientation constraint really affect the constraint sampling? Also, from a UI perspective, separate named fields might be better, e.g.:

[...]
# (optional) Angle tolerance. Only one of the below representation may be used. Not all solvers may support all of them.
# in Euler angles
float64 absolute_x_axis_tolerance
float64 absolute_y_axis_tolerance
float64 absolute_z_axis_tolerance
# in exponential coordinates [add link or explanation here]
float64 value_1
float64 value_2
float64 value_3

[...]

Unless OrientationConstraint messages are passed around frequently enough that the serialization is a bottleneck.

Either way is fine with me though, as long as the message fields and the documentation text in the message are clear.

[JeroenDM]

In the current orientation constraint message, the tolerances are referred to as optional axis-angle error tolerances specified. Does this axis-angle already mean exponential coordinates? Why does it not say Euler-angels?

@ommmid #82 :) I probably should just make a pull request for that issue.

[JeroenDM]

Should the representation of the orientation constraint really affect the constraint sampling?

I don't see how we could separate the two. The "shape" of the constraint region that needs to be sampled is determined by the parameterization.

I like the idea of separate field names. I think the messages are only used on planner initialization, so I don't think serialization is a bottleneck (for motion planning at least, I don't know about the other uses of the message).

[JeroenDM]

I created a pull request that shows how this could be used by the new constrained planner in the ompl interface: JeroenDM/moveit#6

[JeroenDM]

@v4hn @rhaschke @AndyZe
In response to the discussion yesterday: I don't see a way to directly convert bounds on Euler angles into 4 numbers that express bounds on a quaternion or something... below I explain an approach I could come up with to keep using the default orientation tolerance representation in MoveIt. Unfortunately, it is quite a long explanation... but at least there is a picture at the end!

How to use orientation constraints for constrained planning with OMPL?

In MoveIt, the bounds on the orientation constraints are evaluated as bounds on intrinsic XYZ Euler angles. The bounds are relative to a given target orientation (the "center" of the constraint region). The simple way to turn this into an equality constraints could be the following. (I will use (pseudo) code instead of formulas, where I assume using namespace Eigen for brevity. )

Given

• a target orientationR_target as a 3 by 3 rotation matrix,
• orientation bounds, expressed as Euler angles given as vectors Vector3d lower; Vector3d upper;,

we want to create a function that takes the robot's joint values as input and returns an error vector. This error vector can be anything, but it will be used to project joint values on task space constraints, so it should express some form of distance towards the constrained region. The projection code inside OMPL looks like this (slightly changed to better blend in here):

Vector3d error = function(joint_values);
MatrixXd J; // emtpy jacobian
while ((norm = error.squaredNorm()) > squaredTolerance && iter++ < maxIterations_)
{
    J = jacobian(joint_values);
    joint_values = joint_values - some_inverse(J) * error;
    error = function(joint_values);
}
// some_inverse is implemented using Eigen3 as:
// J.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(error);

To use this projection method, we need to implement the (conveniently named... ) functions "function" and "jacobian". Let's do function, using Euler angles to calculate the error vector.

Vector3d function(Eigen::VectorXd joint_values)
{
    Matrix3d R_ee = robot.forwardKinematics(joint_values).rotation();
    // calculate the deviation from the target (bounds are relative to the target)
    R_diff = R_ee.transpose() * R_target;
    Vector3d euler_angles = R_diff.eulerAngles(0, 1, 2);
	
    // check the bounds and create the error vector
    Vector3d error;
    for (int i=0; i<3; ++i)
    {
        if (euler_angle[i] < lower[i])
            error[i] = lower[i] - euler_angle[i];
        else if (euler_angle[i] > upper[i])
            error[i] =  euler_angle[i] - upper[i];
        else
            error[i] = 0.0; 
    }
    return error;
}

The Jacobian can also be implemented. It turns out that this approach has bad convergence (experimentally observed, not theoretically proven). Using another three-element representation for orientation can improve this. It turns out if we replace the euler_angles above with anlge_axis like this:

AngleAxisd aa(R_diff);
Vector3d angle_axis = aa.angle() * aa.axis();

it does work reasonably. (Again, experimental observation.) But this requires us to express orientation bounds for this angle-axis vector, instead of on the XYZ Euler angles.

After @AndyZe's comment in the recent meeting, we can add another approach to this list. Make the error vector independent of how the bounds are specified. This leads us to completely separate the bounds check and the error vector into two parts:

Vector3d function(Eigen::VectorXd joint_values)
{
    Matrix3d R_ee = robot.forwardKinematics(joint_values).rotation();
    // calculate the deviation from the target
    R_diff = R_ee.transpose() * R_target;

    // PART 1
    // check if the robot configuration satisfies the constraints
    Vector3d euler_angles = R_diff.eulerAngles(0, 1, 2);
    bool is_in_bounds = true;
    for (int i=0; i<3; ++i)
    {
        if (euler_angle[i] < lower[i] || euler_angle[i] > upper[i])
        { 
            is_in_bounds = false;
            break;
        }
    }
    
    // PART 2
    // return a non-zero error vector if it did not satisfy the constraints.
    Quateriond error;
    error.setZero(); // I'm not sure this works.
    if (!is_in_bounds)
        error = R_diff;  // implicit conversion from matrix to quaternion
    
    return error;
}

It could be a disadvantage that the information on which specific bounds are violated is not used to project the joint values. I don't know. I imagine if we try to visualize this difference in a simplified way, it looks like this:

[mamoll]

I like the parametrization flag +3 doubles better than having 6 doubles, of which only 3 are meaningful. In the latter case you probably have to use 3 of the doubles as a boolean switch: if Euler orientation thresholds are all zero/nan/inf/whatever use exponential constraints and vice versa. That doesn't seem like a clearer design to me.

Re. the pictures at the end, the projection should continue until the orientation is valid. That means projecting onto a point near the boundary of the valid orientations is the right approach. Projecting onto the centroid will be more expensive computationally. This could be done in a post processing stage for the final solution path rather than for all states being sampled.

[JeroenDM]

In the latter case you probably have to use 3 of the doubles as a boolean switch: if Euler orientation thresholds are all zero/nan/inf/whatever use exponential constraints and vice versa. That doesn't seem like a clearer design to me.

I experienced these issues when trying to implement this. I also think the flags are easier to use and turn out to be easier to implement.

[gautz]

Is this issue entirely solved by #96 ?

[v4hn]

I believe so.