Commit 9d121484 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

ligroup/SO3, SO2: dintegrateTransport_dv_impl and dIntegrateTransport_dq_impl

parent 83d50c0e
......@@ -225,6 +225,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out) = Jin;
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -233,6 +234,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out) = Jin;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......@@ -443,7 +445,6 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
switch(op)
{
case SETTO:
......@@ -465,16 +466,23 @@ 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);
const Matrix3 Jtmp3 = exp3(-v);
Jout.noalias() = Jtmp3 * Jin;
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
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);
const Matrix3 Jtmp3 = exp3(-v);
Jexp3<SETTO>(v, Jtmp3);
Jout.noalias() = Jtmp3 * Jout;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......
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