Skip to content

Commit

Permalink
Correctly handle angle axis singularity when calculating numerical ja…
Browse files Browse the repository at this point in the history 
…cobian
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Feb 10, 2024
1 parent f0e5732 commit f78e2c9
Show file tree
Hide file tree
Showing 3 changed files with 385 additions and 156 deletions.
53 changes: 24 additions & 29 deletions tesseract_common/include/tesseract_common/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,45 +114,40 @@ Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b);
*/
Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R);

/**
* @brief Calculate the rotation error vector given a rotation error matrix where the angle is between [0, 2 * pi]
* @details This function does not break down when the angle is near zero or 2pi when calculating the numerical
* jacobian. This is because when using Eigen's angle axis it converts the angle to be between [0, PI] where internally
* if the angle is between [-PI, 0] it flips the sign of the axis. Both this function and calcRotationalError both check
* for this flip and reverts it. Since the angle is always between [-PI, PI], switching the range to [0, PI] will
* never be close to 2PI. In the case of zero, it also does not break down because we are making sure that the angle
* axis aligns with the quaternion axis eliminating this issue. As you can see the quaternion keeps the angle small but
* flips the axis so the correct delta rotation is calculated.
*
* Angle: 0.001 results in an axis: [0, 0, 1]
* Angle: -0.001 results in and axis: [0, 0, -1]
* e1 = angle * axis = [0, 0, 0.001]
* e2 = angle * axis = [0, 0, -0.001]
* delta = e2 - e1 = [0, 0, 0.002]
*
* @details This should be used when numerically calculating rotation jacobians
* @param R rotation error matrix
* @return Rotation error vector = Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()
*/
Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R);

/**
* @brief Calculate error between two transforms expressed in t1 coordinate system
* @warning This should not be used to calculate numerical jacobian
* @warning This should not be used to calculate numerical jacobian, see calcJacobianTransformErrorDiff
* @param t1 Target Transform
* @param t2 Current Transform
* @return error [Position, calcRotationalError(Angle Axis)]
*/
Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);

/**
* @brief Calculate error between two transforms expressed in t1 coordinate system
* @warning This should only be used to calculate numerical jacobian
* @param t1 Target Transform
* @param t2 Current Transform
* @return error [Position, calcRotationalError2(Angle Axis)]
* @brief Calculate jacobian transform error difference expressed in the target frame coordinate system
* @details This is used when the target is a fixed frame in the environment
* @param target Target The desired transform to express the transform error difference in
* @param source The current location of the source transform
* @param source_perturbed The perturbed location of the source transform
* @return The change in error represented in the target frame
*/
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
const Eigen::Isometry3d& source,
const Eigen::Isometry3d& source_perturbed);

/**
* @brief Calculate jacobian transform error difference expressed in the target frame coordinate system
* @details This is used when the target and source are both dynamic links
* @param target Target The desired transform to express the transform error difference in
* @param target_perturbed The perturbed location of the target transform
* @param source The current location of the source transform
* @param source_perturbed The perturbed location of the source transform
* @return The change in error represented in the target frame
*/
Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
const Eigen::Isometry3d& target_perturbed,
const Eigen::Isometry3d& source,
const Eigen::Isometry3d& source_perturbed);

/**
* @brief This computes a random color RGBA [0, 1] with alpha set to 1
Expand Down
100 changes: 73 additions & 27 deletions tesseract_common/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ Eigen::VectorXd concat(const Eigen::VectorXd& a, const Eigen::VectorXd& b)
return out;
}

Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
std::pair<Eigen::Vector3d, double> calcRotationalErrorDecomposed(const Eigen::Ref<const Eigen::Matrix3d>& R)
{
Eigen::Quaterniond q(R);
Eigen::AngleAxisd r12(q);
Expand All @@ -144,31 +144,13 @@ Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)

assert(std::abs(angle) <= M_PI);

return axis * angle;
return { axis, angle };
}

Eigen::Vector3d calcRotationalError2(const Eigen::Ref<const Eigen::Matrix3d>& R)
Eigen::Vector3d calcRotationalError(const Eigen::Ref<const Eigen::Matrix3d>& R)
{
Eigen::Quaterniond q(R);
Eigen::AngleAxisd r12(q);

// Eigen angle axis flips the sign of axis so rotation is always positive which is
// not ideal for numerical differentiation.
int s = (q.vec().dot(r12.axis()) < 0) ? -1 : 1;

// Make sure that the angle is on [0, 2 * pi]
const static double two_pi = 2.0 * M_PI;
double angle = s * r12.angle();
Eigen::Vector3d axis{ s * r12.axis() };
angle = copysign(fmod(fabs(angle), two_pi), angle);
if (angle < 0)
angle += two_pi;
else if (angle > two_pi)
angle -= two_pi;

assert(angle <= two_pi && angle >= 0);

return axis * angle;
std::pair<Eigen::Vector3d, double> data = calcRotationalErrorDecomposed(R);
return data.first * data.second;
}

Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2)
Expand All @@ -177,10 +159,74 @@ Eigen::VectorXd calcTransformError(const Eigen::Isometry3d& t1, const Eigen::Iso
return concat(pose_err.translation(), calcRotationalError(pose_err.rotation()));
}

Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2)
{
Eigen::Isometry3d pose_err = t1.inverse() * t2;
return concat(pose_err.translation(), calcRotationalError2(pose_err.rotation()));
Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
const Eigen::Isometry3d& source,
const Eigen::Isometry3d& source_perturbed)
{
Eigen::Isometry3d pose_err = target.inverse() * source;
std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);

Eigen::Isometry3d perturbed_pose_err = target.inverse() * source_perturbed;
std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
calcRotationalErrorDecomposed(perturbed_pose_err.rotation());

// They should always pointing in the same direction
#ifndef NDEBUG
if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
#endif

// Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian difference
Eigen::VectorXd perturbed_err;
if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2)
perturbed_err = concat(perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - 2 * M_PI));
else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2)
perturbed_err = concat(perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI));
else
perturbed_err = concat(perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);

return (perturbed_err - err);
}

Eigen::VectorXd calcJacobianTransformErrorDiff(const Eigen::Isometry3d& target,
const Eigen::Isometry3d& target_perturbed,
const Eigen::Isometry3d& source,
const Eigen::Isometry3d& source_perturbed)
{
Eigen::Isometry3d pose_err = target.inverse() * source;
std::pair<Eigen::Vector3d, double> pose_rotation_err = calcRotationalErrorDecomposed(pose_err.rotation());
Eigen::VectorXd err = concat(pose_err.translation(), pose_rotation_err.first * pose_rotation_err.second);

Eigen::Isometry3d perturbed_pose_err = target_perturbed.inverse() * source_perturbed;
std::pair<Eigen::Vector3d, double> perturbed_pose_rotation_err =
calcRotationalErrorDecomposed(perturbed_pose_err.rotation());

// They should always pointing in the same direction
#ifndef NDEBUG
if (std::abs(pose_rotation_err.second) > 0.01 && perturbed_pose_rotation_err.first.dot(pose_rotation_err.first) < 0)
throw std::runtime_error("calcJacobianTransformErrorDiff, angle axes are pointing in oposite directions!");
#endif

Eigen::Isometry3d ttp = target.inverse() * target_perturbed;
// The reason for premultiplying translation with ttp.rotation() is because the translation error needs to be in the
// target frame coordinates Angle axis has a discontinity at PI so need to correctly handle this calculating jacobian
// difference
Eigen::VectorXd perturbed_err;
if (perturbed_pose_rotation_err.second > M_PI_2 && pose_rotation_err.second < -M_PI_2)
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second - 2 * M_PI));
else if (perturbed_pose_rotation_err.second < -M_PI_2 && pose_rotation_err.second > M_PI_2)
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * (perturbed_pose_rotation_err.second + 2 * M_PI));
else
perturbed_err = concat(ttp.rotation() * perturbed_pose_err.translation(),
perturbed_pose_rotation_err.first * perturbed_pose_rotation_err.second);

return (perturbed_err - err);
}

Eigen::Vector4d computeRandomColor()
Expand Down
Loading

0 comments on commit f78e2c9

Please sign in to comment.