Commit e6c054f0 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #365 from jmirabel/devel

[C++] Move space operation to new class LieGroup and inherited classes.
parents 358d9364 6e78fa7c
......@@ -189,6 +189,16 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-composite.hpp
)
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
multibody/liegroup/special-euclidean.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
multibody/fwd.hpp
multibody/constraint.hpp
......@@ -281,6 +291,7 @@ SET(HEADERS
${${PROJECT_NAME}_TOOLS_HEADERS}
${${PROJECT_NAME}_SPATIAL_HEADERS}
${${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS}
${${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS}
${${PROJECT_NAME}_MULTIBODY_HEADERS}
${${PROJECT_NAME}_PARSERS_HEADERS}
${${PROJECT_NAME}_ALGORITHM_HEADERS}
......@@ -295,6 +306,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/math")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/lua")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
......
......@@ -78,10 +78,11 @@ namespace se3
"return the configuration normalized ");
bp::def("isSameConfiguration",
(bool (*)(const Model &, const VectorXd &, const VectorXd &))&isSameConfiguration,
(bool (*)(const Model &, const VectorXd &, const VectorXd &, const double&))&isSameConfiguration,
bp::args("Model",
"Configuration q1 (size Model::nq)",
"Configuration q2 (size Model::nq)"),
"Configuration q2 (size Model::nq)",
"Precision"),
"Return true if two configurations are equivalent");
}
......
This diff is collapsed.
......@@ -18,7 +18,7 @@
#ifndef __math_quaternion_hpp__
#define __math_quaternion_hpp__
#include <boost/math/constants/constants.hpp>
#include <pinocchio/math/fwd.hpp>
namespace se3
{
......@@ -56,9 +56,10 @@ namespace se3
///
template <typename Derived, typename otherDerived>
bool defineSameRotation(const Eigen::QuaternionBase<Derived> & q1,
const Eigen::QuaternionBase<otherDerived> & q2)
const Eigen::QuaternionBase<otherDerived> & q2,
const typename Derived::RealScalar & prec = Eigen::NumTraits<typename Derived::Scalar>::dummy_precision())
{
return (q1.coeffs().isApprox(q2.coeffs()) || q1.coeffs().isApprox(-q2.coeffs()) );
return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
}
/// Approximately normalize by applying the first order limited development
......
......@@ -21,7 +21,37 @@
namespace se3
{
enum { MAX_JOINT_NV = 6 };
template<int axis> struct JointModelRevolute;
template<int axis> struct JointDataRevolute;
struct JointModelRevoluteUnaligned;
struct JointDataRevoluteUnaligned;
template<int axis> struct JointModelRevoluteUnbounded;
template<int axis> struct JointDataRevoluteUnbounded;
struct JointModelSpherical;
struct JointDataSpherical;
struct JointModelSphericalZYX;
struct JointDataSphericalZYX;
template<int axis> struct JointModelPrismatic;
template<int axis> struct JointDataPrismatic;
struct JointModelPrismaticUnaligned;
struct JointDataPrismaticUnaligned;
struct JointModelFreeFlyer;
struct JointDataFreeFlyer;
struct JointModelPlanar;
struct JointDataPlanar;
struct JointModelTranslation;
struct JointDataTranslation;
struct JointModelComposite;
struct JointDataComposite;
......
......@@ -20,6 +20,7 @@
#define __se3_joint_base_hpp__
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/joint/fwd.hpp"
#include <Eigen/Core>
#include <limits>
......
......@@ -31,9 +31,6 @@
namespace se3
{
struct JointDataFreeFlyer;
struct JointModelFreeFlyer;
struct ConstraintIdentity;
template <>
......
......@@ -30,9 +30,6 @@
namespace se3
{
struct JointDataPlanar;
struct JointModelPlanar;
struct MotionPlanar;
template <>
struct traits< MotionPlanar >
......@@ -468,7 +465,7 @@ namespace se3
{
ConfigVector_t result;
result.head<2>().setRandom();
const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
const Scalar angle = PI * (-1 + 2 * ((Scalar)rand()) / RAND_MAX);
SINCOS(angle, &result[3], &result[2]);
return result;
}
......@@ -488,7 +485,7 @@ namespace se3
}
result[i] = lower_pos_limit[i] + ( upper_pos_limit[i] - lower_pos_limit[i]) * rand()/RAND_MAX;
}
const Scalar angle = PI * ((Scalar)(-1 + 2 * rand()) / (Scalar) RAND_MAX);
const Scalar angle = PI * (-1 + 2 * ((Scalar)rand()) / RAND_MAX);
SINCOS(angle, &result[3], &result[2]);
return result;
}
......
......@@ -28,9 +28,6 @@
namespace se3
{
struct JointDataPrismaticUnaligned;
struct JointModelPrismaticUnaligned;
struct MotionPrismaticUnaligned;
template <>
......
......@@ -27,9 +27,6 @@
namespace se3
{
template<int axis> struct JointDataPrismatic;
template<int axis> struct JointModelPrismatic;
namespace prismatic
{
......
......@@ -28,9 +28,6 @@
namespace se3
{
struct JointDataRevoluteUnaligned;
struct JointModelRevoluteUnaligned;
struct MotionRevoluteUnaligned;
template <>
struct traits < MotionRevoluteUnaligned >
......
......@@ -31,10 +31,6 @@ namespace se3
template<int axis> struct JointRevoluteUnbounded;
template<int axis> struct JointDataRevoluteUnbounded;
template<int axis> struct JointModelRevoluteUnbounded;
template<int axis>
struct traits< JointRevoluteUnbounded<axis> >
{
......
......@@ -29,9 +29,6 @@
namespace se3
{
template<int axis> struct JointDataRevolute;
template<int axis> struct JointModelRevolute;
template<int axis> struct SE3Revolute;
template<int axis> struct MotionRevolute;
......
......@@ -29,9 +29,6 @@
namespace se3
{
struct JointDataSphericalZYX;
struct JointModelSphericalZYX;
template <typename _Scalar, int _Options>
struct JointSphericalZYXTpl
......
......@@ -31,9 +31,6 @@
namespace se3
{
struct JointDataSpherical;
struct JointModelSpherical;
struct MotionSpherical;
template <>
struct traits< MotionSpherical >
......
......@@ -29,9 +29,6 @@
namespace se3
{
struct JointDataTranslation;
struct JointModelTranslation;
struct MotionTranslation;
template <>
struct traits < MotionTranslation >
......
//
// 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_cartesian_product_operation_hpp__
#define __se3_cartesian_product_operation_hpp__
#include <pinocchio/multibody/liegroup/operation-base.hpp>
namespace se3
{
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation;
template<typename LieGroup1, typename LieGroup2>
struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > {
typedef double Scalar;
enum {
NQ = LieGroup1::NQ + LieGroup2::NQ,
NV = LieGroup1::NV + LieGroup2::NV
};
typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
};
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation : public LieGroupOperationBase <CartesianProductOperation<LieGroup1, LieGroup2> >
{
typedef CartesianProductOperation<LieGroup1, LieGroup2> LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF;
template <class ConfigL_t, class ConfigR_t, class Tangent_t>
static void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<Tangent_t> & d)
{
Tangent_t& out = const_cast< Eigen::MatrixBase<Tangent_t>& > (d).derived();
LieGroup1::difference(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NV>());
LieGroup2::difference(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NV>());
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::integrate(q.template head<LieGroup1::NQ>(), v.template head<LieGroup1::NV>(), out.template head<LieGroup1::NQ>());
LieGroup2::integrate(q.template tail<LieGroup2::NQ>(), v.template tail<LieGroup2::NV>(), out.template tail<LieGroup2::NQ>());
}
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>())
+ LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>());
}
template <class Config_t>
static void random_impl (const Eigen::MatrixBase<Config_t>& qout)
{
Config_t& out = const_cast< Eigen::MatrixBase<Config_t>& > (qout).derived();
LieGroup1::random(out.template head<LieGroup1::NQ>());
LieGroup2::random(out.template tail<LieGroup2::NQ>());
}
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
static void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
const Eigen::MatrixBase<ConfigR_t> & upper,
const Eigen::MatrixBase<ConfigOut_t> & qout)
{
ConfigOut_t& out = const_cast< Eigen::MatrixBase<ConfigOut_t>& > (qout).derived();
LieGroup1::randomConfiguration(lower.template head<LieGroup1::NQ>(), upper.template head<LieGroup1::NQ>(), out.template head<LieGroup1::NQ>());
LieGroup2::randomConfiguration(lower.template tail<LieGroup2::NQ>(), upper.template tail<LieGroup2::NQ>(), out.template tail<LieGroup2::NQ>());
}
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 LieGroup1::isSameConfiguration(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), prec)
+ LieGroup2::isSameConfiguration(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), prec);
}
}; // struct CartesianProductOperation
} // namespace se3
#endif // ifndef __se3_cartesian_product_operation_hpp__
//
// 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_hpp__
#define __se3_lie_group_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/liegroup/vector-space.hpp"
#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
#include "pinocchio/multibody/liegroup/special-euclidean.hpp"
#include "pinocchio/multibody/joint/fwd.hpp"
namespace se3 {
struct LieGroupTpl {
template<typename JointModel> struct operation {
typedef VectorSpaceOperation<JointModel::NQ> type;
};
};
template<typename JointModel>
struct LieGroup {
typedef typename LieGroupTpl::operation<JointModel>::type type;
};
template<> struct LieGroupTpl::operation <JointModelComposite> {};
template<> struct LieGroupTpl::operation <JointModelSpherical> {
typedef SpecialOrthogonalOperation<3> type;
};
template<> struct LieGroupTpl::operation <JointModelFreeFlyer> {
typedef SpecialEuclideanOperation<3> type;
};
template<> struct LieGroupTpl::operation <JointModelPlanar> {
typedef SpecialEuclideanOperation<2> type;
};
template<int Axis> struct LieGroupTpl::operation <JointModelRevoluteUnbounded<Axis> > {
typedef SpecialOrthogonalOperation<2> type;
};
// TODO REMOVE: For testing purposes only
// template<>
// struct LieGroupTpl::operation <JointModelTranslation> {
// typedef CartesianProductOperation<
// CartesianProductOperation<typename LieGroup<JointModelPX>::type, typename LieGroup<JointModelPY>::type>,
// typename LieGroup<JointModelPZ>::type
// > type;
// };
}
#endif // ifndef __se3_lie_group_hpp__
//
// 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_hpp__
#define __se3_lie_group_operation_base_hpp__
#include "pinocchio/spatial/fwd.hpp" // struct traits
#include <Eigen/Core>
#include <limits>
namespace se3
{
#ifdef __clang__
#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix) \
typedef prefix traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
#define SE3_LIE_GROUP_TYPEDEF_NOARG() \
typedef int Index; \
typedef traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF_ARG(prefix) \
typedef int Index; \
typedef prefix traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef prefix traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef prefix traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_NOARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG(typename)
#else
#define SE3_LIE_GROUP_TYPEDEF_ARG() \
typedef int Index; \
typedef typename traits<LieGroupDerived>::Scalar Scalar; \
enum { \
NQ = traits<LieGroupDerived>::NQ, \
NV = traits<LieGroupDerived>::NV \
}; \
typedef typename traits<LieGroupDerived>::ConfigVector_t ConfigVector_t; \
typedef typename traits<LieGroupDerived>::TangentVector_t TangentVector_t
#define SE3_LIE_GROUP_TYPEDEF SE3_LIE_GROUP_TYPEDEF_ARG()
#define SE3_LIE_GROUP_TYPEDEF_TEMPLATE SE3_LIE_GROUP_TYPEDEF_ARG()
#endif
template<typename Derived>
struct LieGroupOperationBase
{
typedef Derived LieGroupDerived;
SE3_LIE_GROUP_TYPEDEF_TEMPLATE;
/// \name API with return value as argument
/// \{
/**
* @brief Integrate joint's configuration for a tangent vector during one unit time
*
* @param[in] q initatial configuration (size full model.nq)
* @param[in] v joint velocity (size full model.nv)
*
* @return The configuration integrated
*/
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);
/**
* @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 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);
/**
* @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 Config_t>
static void random (const Eigen::MatrixBase<Config_t>& qout);
/**
* @brief Generate a configuration vector uniformly sampled among
* provided limits.
*
* @param[in] lower_pos_limit lower joint limit
* @param[in] upper_pos_limit upper joint limit
*
* @return The joint configuration
*/
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);
/**
* @brief the tangent vector that must be integrated during one unit time to go from q0 to q1
*
* @param[in] q0 Initial configuration
* @param[in] q1 Wished configuration
*
* @return The corresponding velocity
*/
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);
/**
* @brief Squared distance between two configurations of the joint
*
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
/**
* @brief Distance between two configurations of the joint
*
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
template <class ConfigL_t, class ConfigR_t>
static double distance(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1);
/**
* @brief Check if two configurations are equivalent within the given precision