Commit 94f38811 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

liegroup/special-euclidean: transport in place

parent f55a5e11
......@@ -385,6 +385,44 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
Matrix2 R;
Vector2 t;
exp(v, R, t);
Vector2 tinv = (R.transpose() * t).reverse();
tinv[0] *= Scalar(-1.);
//TODO: Aliasing
Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
//No Aliasing
Jout.template topRows<2>().noalias() += tinv * Jout.template bottomRows<1>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J);
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6(nu, Jtmp6);
//TODO: Remove aliasing
Jout.template topRows<2>() = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
Jout.template topRows<2>().noalias() += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
Jout.template bottomRows<1>().noalias() = Jtmp6.template bottomLeftCorner<1,2>()* Jout.template topRows<2>();
Jout.template bottomRows<1>() += Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
......@@ -687,6 +725,37 @@ namespace pinocchio
Jout.template bottomRows<3>().noalias() = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J_out) const
{
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
//Aliasing
Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J_out) const
{
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out);
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......
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