Commit 8da6cc78 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

ligroup/SE3: dIntegrateTransport_dv_impl

parent 2529e15d
......@@ -356,11 +356,21 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport_dv_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);
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6(nu, Jtmp6);
Jout.template topRows<2>().noalias() = Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
Jout.template topRows<2>().noalias() += Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
Jout.template bottomRows<1>().noalias() = Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
Jout.template bottomRows<1>().noalias() += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
}
// interpolate_impl use default implementation.
......@@ -649,6 +659,12 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
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>();
}
......
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