Commit bdac70c5 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

add signatures for Jexp3 and Jexp6 with assignmentop

parent 736301ab
......@@ -107,7 +107,7 @@ namespace pinocchio
/// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
/// \f]
///
template<typename Vector3Like, typename Matrix3Like>
template<AssignmentOperatorType op, typename Vector3Like, typename Matrix3Like>
void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
const Eigen::MatrixBase<Matrix3Like> & Jexp)
{
......@@ -141,6 +141,22 @@ namespace pinocchio
Jout.noalias() += c * r * r.transpose();
}
///
/// \brief Derivative of \f$ \exp{r} \f$
/// \f[
/// \frac{\sin{||r||}}{||r||} I_3
/// - \frac{1-\cos{||r||}}{||r||^2} \left[ r \right]_x
/// + \frac{1}{||n||^2} (1-\frac{\sin{||r||}}{||r||}) r r^T
/// \f]
///
template<typename Vector3Like, typename Matrix3Like>
void Jexp3(const Eigen::MatrixBase<Vector3Like> & r,
const Eigen::MatrixBase<Matrix3Like> & Jexp)
{
Jexp3<SETTO>(r, Jexp);
}
/** \brief Derivative of log3
*
......@@ -310,7 +326,7 @@ namespace pinocchio
/// \brief Derivative of exp6
/// Computed as the inverse of Jlog6
template<typename MotionDerived, typename Matrix6Like>
template<AssignmentOperatorType op, typename MotionDerived, typename Matrix6Like>
void Jexp6(const MotionDense<MotionDerived> & nu,
const Eigen::MatrixBase<Matrix6Like> & Jexp)
{
......@@ -326,11 +342,6 @@ namespace pinocchio
const Scalar t2 = w.squaredNorm();
const Scalar t = math::sqrt(t2);
// Matrix3 J3;
// Jexp3(w, J3);
Jexp3(w, Jout.template bottomRightCorner<3,3>());
Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
const Scalar tinv = Scalar(1)/t,
t2inv = tinv*tinv;
Scalar st,ct; SINCOS (t, &st, &ct);
......@@ -345,7 +356,13 @@ namespace pinocchio
Scalar(1)/Scalar(360),
-Scalar(2)*t2inv*t2inv + (Scalar(1) + st*tinv) * t2inv * inv_2_2ct);
Vector3 p (Jout.template topLeftCorner<3,3>().transpose() * v);
// Matrix3 J3;
// Jexp3(w, J3);
Jexp3(w, Jout.template bottomRightCorner<3,3>());
Jout.template topLeftCorner<3,3>() = Jout.template bottomRightCorner<3,3>();
const Vector3 p = Jout.template topLeftCorner<3,3>().transpose() * v;
Scalar wTp (w.dot (p));
Matrix3 J (alphaSkew(.5, p) +
(beta_dot_over_theta*wTp) *w*w.transpose()
......@@ -358,6 +375,16 @@ namespace pinocchio
Jout.template bottomLeftCorner<3,3>().setZero();
}
/// \brief Derivative of exp6
/// Computed as the inverse of Jlog6
template<typename MotionDerived, typename Matrix6Like>
void Jexp6(const MotionDense<MotionDerived> & nu,
const Eigen::MatrixBase<Matrix6Like> & Jexp)
{
Jexp6<SETTO>(nu, Jexp);
}
/** \brief Derivative of log6
* \f[
* \left(\begin{array}{cc}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment