Skip to content

GitLab

Commit c93253cc authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

[Minor][C++] Clean code of LieGroupOperationBase

parent 2b3b7a8c
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ c93253cc
......@@ -192,6 +192,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
multibody/liegroup/liegroup.hpp
multibody/liegroup/operation-base.hpp
multibody/liegroup/operation-base.hxx
multibody/liegroup/vector-space.hpp
multibody/liegroup/cartesian-product.hpp
multibody/liegroup/special-orthogonal.hpp
......
src/multibody/liegroup/operation-base.hpp
View file @ c93253cc
......@@ -87,14 +87,8 @@ namespace se3
typedef Derived LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
///
/// \brief Return the resolution of the finite differerence increment according to the Scalar type
/// \remark Ideally, this function must depend on the value of q
///
/// \returns The finite difference increment.
///
// typename ConfigVector_t::Scalar finiteDifferenceIncrement() const
// { return derived().finiteDifferenceIncrement(); }
/// \name API with return value as argument
/// \{
/**
* @brief Integrate joint's configuration for a tangent vector during one unit time
......@@ -104,26 +98,10 @@ namespace se3
*
* @return The configuration integrated
*/
template <class Config_t, class Tangent_t>
static ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v)
{
ConfigVector_t qout;
integrate(q, v, qout);
return qout;
}
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
static void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
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);
}
const Eigen::MatrixBase<ConfigOut_t>& qout);
/**
* @brief Interpolation between two joint's configurations
......@@ -134,40 +112,11 @@ namespace se3
*
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
// ConfigVector_t interpolate(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, double u) const
// { return derived().interpolate_impl(q0, q1, u); }
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u)
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
return qout;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
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);
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
else integrate(q0, u * difference(q0, q1), qout);
}
const Eigen::MatrixBase<ConfigOut_t>& qout);
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
......@@ -177,16 +126,8 @@ namespace se3
*
* @return The joint configuration
*/
static ConfigVector_t random()
{
ConfigVector_t qout;
random(qout);
return qout;
}
template <class Config_t>
static void random (const Eigen::MatrixBase<Config_t>& qout)
{ return Derived::random_impl (qout); }
static void random (const Eigen::MatrixBase<Config_t>& qout);
/**
* @brief Generate a configuration vector uniformly sampled among
......@@ -197,25 +138,10 @@ namespace se3
*
* @return The joint configuration
*/
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
{
ConfigVector_t qout;
randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
return qout;
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
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);
}
const Eigen::MatrixBase<ConfigOut_t> & qout);
/**
* @brief the tangent vector that must be integrated during one unit time to go from q0 to q1
......@@ -225,25 +151,10 @@ namespace se3
*
* @return The corresponding velocity
*/
template <class ConfigL_t, class ConfigR_t>
static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t diff;
difference(q0, q1, diff);
return diff;
}
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d)
{
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);
}
const Eigen::MatrixBase<Tangent_t>& d);
/**
* @brief Squared distance between two configurations of the joint
......@@ -255,21 +166,7 @@ namespace se3
*/
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
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);
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference(q0, q1, t);
return t.squaredNorm();
}
const Eigen::MatrixBase<ConfigR_t> & q1);
/**
* @brief Distance between two configurations of the joint
......@@ -281,29 +178,7 @@ namespace se3
*/
template <class ConfigL_t, class ConfigR_t>
static double distance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{ return sqrt(squaredDistance(q0, q1)); }
/**
* @brief Get neutral configuration of joint
*
* @return The joint's neutral configuration
*/
// ConfigVector_t neutralConfiguration() const
// { return derived().neutralConfiguration_impl(); }
/**
* @brief Normalize a configuration
*
* @param[in,out] q Configuration to normalize (size full model.nq)
*/
// void normalize(Eigen::VectorXd & q) const
// { return derived().normalize_impl(q); }
/**
* @brief Default implementation of normalize
*/
// void normalize_impl(Eigen::VectorXd &) const { }
const Eigen::MatrixBase<ConfigR_t> & q1);
/**
* @brief Check if two configurations are equivalent within the given precision
......@@ -314,18 +189,52 @@ namespace se3
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
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);
}
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision());
/// \}
/// \name API that allocates memory
/// \{
template <class Config_t, class Tangent_t>
static ConfigVector_t integrate(const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v);
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u);
static ConfigVector_t random();
template <class ConfigL_t, class ConfigR_t>
static ConfigVector_t randomConfiguration(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit);
template <class ConfigL_t, class ConfigR_t>
static TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
/// \}
/// \name Default implementations
/// \{
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout);
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
template <class ConfigL_t, class ConfigR_t>
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{ return q0.isApprox(q1, prec); }
const Scalar & prec);
/// \}
protected:
/// Default constructor.
......@@ -341,4 +250,6 @@ namespace se3
} // namespace se3
#include "pinocchio/multibody/liegroup/operation-base.hxx"
#endif // ifndef __se3_lie_group_operation_base_hpp__
src/multibody/liegroup/operation-base.hxx 0 → 100644
View file @ c93253cc
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_lie_group_operation_base_hxx__
#define __se3_lie_group_operation_base_hxx__
namespace se3 {
// --------------- API with return value as argument ---------------------- //
template <class Derived>
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
void LieGroupOperationBase<Derived>::integrate(
const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
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);
}
/**
* @brief Interpolation between two joint's configurations
*
* @param[in] q0 Initial configuration to interpolate
* @param[in] q1 Final configuration to interpolate
* @param[in] u u in [0;1] position along the interpolation.
*
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void LieGroupOperationBase<Derived>::interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
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);
}
/**
* @brief Generate a random joint configuration, normalizing quaternions when necessary.
*
* \warning Do not take into account the joint limits. To shoot a configuration uniformingly
* depending on joint limits, see uniformySample
*
* @return The joint configuration
*/
template <class Derived>
template <class Config_t>
void LieGroupOperationBase<Derived>::random(
const Eigen::MatrixBase<Config_t>& qout)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
return Derived::random_impl (qout);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void LieGroupOperationBase<Derived>::randomConfiguration(
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
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);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
void LieGroupOperationBase<Derived>::difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t>& d)
{
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);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
double LieGroupOperationBase<Derived>::squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
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);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
double LieGroupOperationBase<Derived>::distance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return sqrt(squaredDistance(q0, q1));
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
bool LieGroupOperationBase<Derived>::isSameConfiguration(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
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);
}
// ----------------- API that allocates memory ---------------------------- //
template <class Derived>
template <class Config_t, class Tangent_t>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::integrate(
const Eigen::MatrixBase<Config_t> & q,
const Eigen::MatrixBase<Tangent_t> & v)
{
ConfigVector_t qout;
integrate(q, v, qout);
return qout;
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::interpolate(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u)
{
ConfigVector_t qout;
interpolate(q0, q1, u, qout);
return qout;
}
template <class Derived>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::random()
{
ConfigVector_t qout;
random(qout);
return qout;
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
typename LieGroupOperationBase<Derived>::ConfigVector_t LieGroupOperationBase<Derived>::randomConfiguration(
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit)
{
ConfigVector_t qout;
randomConfiguration(lower_pos_limit, upper_pos_limit, qout);
return qout;
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
typename LieGroupOperationBase<Derived>::TangentVector_t LieGroupOperationBase<Derived>::difference(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t diff;
difference(q0, q1, diff);
return diff;
}
// ----------------- Default implementations ------------------------------ //
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void LieGroupOperationBase<Derived>::interpolate_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar& u,
const Eigen::MatrixBase<ConfigOut_t>& qout)
{
if (u == 0) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q0;
else if(u == 1) const_cast<Eigen::MatrixBase<ConfigOut_t>&>(qout) = q1;
else integrate(q0, u * difference(q0, q1), qout);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
double LieGroupOperationBase<Derived>::squaredDistance_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
TangentVector_t t;
difference(q0, q1, t);
return t.squaredNorm();
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
bool LieGroupOperationBase<Derived>::isSameConfiguration_impl(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Scalar & prec)
{
return q0.isApprox(q1, prec);
}
} // namespace se3
#endif // __se3_lie_group_operation_base_hxx__
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