Commit 83d50c0e authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

ligroup/SE3: dIntegrateTransport_dq_impl

parent 8da6cc78
......@@ -351,8 +351,19 @@ namespace pinocchio
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
Matrix2 R;
Vector2 t;
exp(v, R, t);
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
tinv[0] *= Scalar(-1.);
Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
Jout.template topRows<2>().noalias() += tinv * Jin.template bottomRows<1>();
Jout.template bottomRows<1>() = Jin.template bottomRows<1>();
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -649,8 +660,15 @@ namespace pinocchio
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
Jout.template topRows<3>().noalias = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -659,12 +677,13 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
Jout.template topRows<3>().noalias = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias += Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
}
......
Markdown is supported
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