Skip to content

Commit

Permalink
Add jacobian transform error calculation function
Browse files Browse the repository at this point in the history
  • Loading branch information
Levi-Armstrong committed Dec 8, 2023
1 parent 985e4c0 commit f1e79d8
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 1 deletion.
12 changes: 11 additions & 1 deletion tesseract_common/include/tesseract_common/utils.h
Original file line number Diff line number Diff line change
Expand Up @@ -138,12 +138,22 @@ 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
* @param t1 Target Transform
* @param t2 Current Transform
* @return error [Position, Rotational(Angle Axis)]
* @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)]
*/
Eigen::VectorXd calcTransformErrorJac(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2);

/**
* @brief This computes a random color RGBA [0, 1] with alpha set to 1
* @return A random RGBA color
Expand Down
6 changes: 6 additions & 0 deletions tesseract_common/src/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -177,6 +177,12 @@ 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::Vector4d computeRandomColor()
{
Eigen::Vector4d c;
Expand Down

0 comments on commit f1e79d8

Please sign in to comment.