Skip to content

GitLab

Commit 9669a0c6 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

multibody/liegroup: fix dintegrateTransport

parent 9d121484
Hide whitespace changes
Inline Side-by-side
src/multibody/liegroup/liegroup-algo.hxx
View file @ 9669a0c6
......@@ -224,8 +224,8 @@ namespace pinocchio
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointBlock(mat_in.derived()),
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)),
jmodel.jointRows(mat_in.derived()),
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType,mat_out)),
arg);
}
};
......
src/multibody/liegroup/liegroup-base.hxx
View file @ 9669a0c6
......@@ -136,7 +136,7 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
derived().dIntegrateTransport_dq_impl(q.derived(),
derived().dIntegrateTransport_dv_impl(q.derived(),
v.derived(),
Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
......
src/multibody/liegroup/special-euclidean.hpp
View file @ 9669a0c6
......@@ -358,7 +358,7 @@ namespace pinocchio
Vector2 t;
exp(v, R, t);
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
Vector2 tinv = (R.transpose() * t).reverse();
tinv[0] *= Scalar(-1.);
Jout.template topRows<2>().noalias() = R.transpose() * Jin.template topRows<2>();
......@@ -381,7 +381,8 @@ namespace pinocchio
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>();
Jout.template bottomRows<1>().noalias() += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
}
// interpolate_impl use default implementation.
......@@ -666,24 +667,24 @@ namespace pinocchio
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>();
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>
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);
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 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>();
}
......
src/multibody/liegroup/special-orthogonal.hpp
View file @ 9669a0c6
......@@ -225,7 +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;
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -234,7 +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;
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
......@@ -468,6 +468,7 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
typedef typename SE3::Matrix3 Matrix3;
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
const Matrix3 Jtmp3 = exp3(-v);
Jout.noalias() = Jtmp3 * Jin;
......@@ -479,8 +480,9 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & J_out) const
{
typedef typename SE3::Matrix3 Matrix3;
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out);
const Matrix3 Jtmp3 = exp3(-v);
Matrix3 Jtmp3;
Jexp3<SETTO>(v, Jtmp3);
Jout.noalias() = Jtmp3 * Jout;
}
......
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