Skip to content
GitLab
Verified Commit 3849da15 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

mutlibody/lie-group: use at most derived quantities

parent 87f83cd0
Hide whitespace changes
Inline Side-by-side
src/multibody/liegroup/liegroup-base.hxx
View file @ 3849da15
......@@ -21,7 +21,7 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
derived().integrate_impl(q, v, qout);
derived().integrate_impl(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
}
template <class Derived>
......@@ -32,7 +32,7 @@ namespace pinocchio {
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
derived().integrateCoeffWiseJacobian_impl(q,J);
derived().integrateCoeffWiseJacobian_impl(q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
}
......@@ -47,9 +47,9 @@ namespace pinocchio {
switch (arg) {
case ARG0:
dIntegrate_dq(q,v,J); return;
dIntegrate_dq(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); return;
case ARG1:
dIntegrate_dv(q,v,J); return;
dIntegrate_dv(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); return;
default: return;
}
}
......@@ -64,7 +64,9 @@ 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().dIntegrate_dq_impl(q, v, J);
derived().dIntegrate_dq_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J));
}
template <class Derived>
......@@ -77,7 +79,9 @@ 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().dIntegrate_dv_impl(q, v, J);
derived().dIntegrate_dv_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J));
}
/**
......@@ -100,7 +104,7 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
Derived().interpolate_impl(q0, q1, u, qout);
Derived().interpolate_impl(q0, q1, u, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
}
template <class Derived>
......@@ -109,7 +113,7 @@ namespace pinocchio {
(const Eigen::MatrixBase<Config_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return derived().normalize_impl (qout);
return derived().normalize_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout));
}
/**
......@@ -126,7 +130,7 @@ namespace pinocchio {
(const Eigen::MatrixBase<Config_t> & qout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return derived().random_impl (qout);
return derived().random_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout));
}
template <class Derived>
......@@ -139,7 +143,9 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
derived ().randomConfiguration_impl(lower_pos_limit, upper_pos_limit, qout);
derived ().randomConfiguration_impl(lower_pos_limit.derived(),
upper_pos_limit.derived(),
PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
}
template <class Derived>
......@@ -152,21 +158,20 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
derived().difference_impl(q0, q1, d);
derived().difference_impl(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d));
}
template <class Derived>
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void LieGroupBase<Derived>::dDifference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOut_t, JacobianMatrix_t);
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
derived().template dDifference_impl<arg> (q0, q1, J);
derived().template dDifference_impl<arg> (q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J));
}
template <class Derived>
......@@ -178,7 +183,7 @@ namespace pinocchio {
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return derived().squaredDistance_impl(q0, q1);
return derived().squaredDistance_impl(q0.derived(), q1.derived());
}
template <class Derived>
......@@ -188,7 +193,7 @@ namespace pinocchio {
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
return sqrt(squaredDistance(q0, q1));
return sqrt(squaredDistance(q0.derived(), q1.derived()));
}
template <class Derived>
......@@ -200,7 +205,7 @@ namespace pinocchio {
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
return derived().isSameConfiguration_impl(q0, q1, prec);
return derived().isSameConfiguration_impl(q0.derived(), q1.derived(), prec);
}
// ----------------- API that allocates memory ---------------------------- //
......@@ -213,7 +218,7 @@ namespace pinocchio {
const Eigen::MatrixBase<Tangent_t> & v) const
{
ConfigVector_t qout;
integrate(q, v, qout);
integrate(q.derived(), v.derived(), qout);
return qout;
}
......@@ -225,7 +230,7 @@ namespace pinocchio {
const Scalar & u) const
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
interpolate(q0.derived(), q1.derived(), u, qout);
return qout;
}
......@@ -246,7 +251,7 @@ namespace pinocchio {
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const
{
ConfigVector_t qout;
randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
randomConfiguration(lower_pos_limit.derived(), upper_pos_limit.derived(), qout);
return qout;
}
......@@ -257,7 +262,7 @@ namespace pinocchio {
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
TangentVector_t diff;
difference(q0, q1, diff);
difference(q0.derived(), q1.derived(), diff);
return diff;
}
......@@ -275,7 +280,7 @@ namespace pinocchio {
else
{
TangentVector_t vdiff(u * difference(q0, q1));
integrate(q0, vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout));
}
}
......@@ -287,7 +292,7 @@ namespace pinocchio {
const Eigen::MatrixBase<ConfigR_t> & q1) const
{
TangentVector_t t;
difference(q0, q1, t);
difference(q0.derived(), q1.derived(), t);
return t.squaredNorm();
}
......
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