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 ...@@ -225,6 +225,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin, const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const 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> template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
...@@ -233,6 +234,7 @@ namespace pinocchio ...@@ -233,6 +234,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin, const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const 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> template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
...@@ -443,7 +445,6 @@ namespace pinocchio ...@@ -443,7 +445,6 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t> & J, const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const AssignmentOperatorType op)
{ {
switch(op) switch(op)
{ {
case SETTO: case SETTO:
...@@ -465,16 +466,23 @@ namespace pinocchio ...@@ -465,16 +466,23 @@ namespace pinocchio
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q, void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v, const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin, 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> 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<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin, 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> 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