...
 
Commits (84)
......@@ -3,4 +3,5 @@ Xcode*
*~
*.pyc
coverage*
.travis
.vscode*
......@@ -61,7 +61,9 @@ OPTION (BUILD_WITH_COMMIT_VERSION "Build libraries by setting specific commit ve
OPTION(BUILD_WITH_URDF_SUPPORT "Build the library with the URDF format support" ON)
OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the Collision support (required HPP-FCL)" OFF)
OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the Automatic Differentiation support" OFF)
OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the CASADI support" OFF)
OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the Code Generation support" OFF)
OPTION(BUILD_WITH_CATKIN_SUPPORT "Build the library with Catkin support" OFF)
OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
......@@ -90,7 +92,9 @@ IF(BUILD_WITH_URDF_SUPPORT)
ENDIF(BUILD_WITH_URDF_SUPPORT)
IF(BUILD_WITH_COLLISION_SUPPORT)
find_package(catkin QUIET COMPONENTS hpp-fcl) # compatability with catkin-packaged hpp-fcl
IF(BUILD_WITH_CATKIN_SUPPORT)
find_package(catkin QUIET COMPONENTS hpp-fcl) # compatability with catkin-packaged hpp-fcl
ENDIF(BUILD_WITH_CATKIN_SUPPORT)
ADD_REQUIRED_DEPENDENCY("hpp-fcl >= 1.0.0")
ENDIF(BUILD_WITH_COLLISION_SUPPORT)
......@@ -98,7 +102,7 @@ IF(BUILD_WITH_AUTODIFF_SUPPORT)
ADD_REQUIRED_DEPENDENCY("cppad >= 20180000.0")
ENDIF(BUILD_WITH_AUTODIFF_SUPPORT)
IF(CPPAD_FOUND)
IF(CPPAD_FOUND AND BUILD_WITH_AUTODIFF_SUPPORT)
ADD_DEFINITIONS(-DPINOCCHIO_WITH_CPPAD_SUPPORT)
PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_WITH_CPPAD_SUPPORT")
IF(NOT ${CPPAD_VERSION} VERSION_GREATER "3.3.0")
......@@ -109,14 +113,23 @@ IF(CPPAD_FOUND)
IF(BUILD_WITH_CODEGEN_SUPPORT)
ADD_OPTIONAL_DEPENDENCY("cppadcg")
ENDIF(BUILD_WITH_CODEGEN_SUPPORT)
IF(CPPADCG_FOUND)
IF(CPPADCG_FOUND AND BUILD_WITH_CODEGEN_SUPPORT)
ADD_DEFINITIONS(-DPINOCCHIO_WITH_CPPADCG_SUPPORT)
PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_WITH_CPPADCG_SUPPORT")
ENDIF(CPPADCG_FOUND)
ENDIF(CPPAD_FOUND)
ENDIF(CPPADCG_FOUND AND BUILD_WITH_CODEGEN_SUPPORT)
ENDIF(CPPAD_FOUND AND BUILD_WITH_AUTODIFF_SUPPORT)
IF(BUILD_WITH_CASADI_SUPPORT)
ADD_REQUIRED_DEPENDENCY("casadi >= 3.4.5")
ENDIF(BUILD_WITH_CASADI_SUPPORT)
IF(CASADI_FOUND AND BUILD_WITH_CASADI_SUPPORT)
ADD_DEFINITIONS(-DPINOCCHIO_WITH_CASADI_SUPPORT)
PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_WITH_CASADI_SUPPORT")
ENDIF(CASADI_FOUND AND BUILD_WITH_CASADI_SUPPORT)
# Special care of urdfdom version
IF(URDFDOM_FOUND)
IF(URDFDOM_FOUND AND BUILD_WITH_URDF_SUPPORT)
IF(${URDFDOM_VERSION} VERSION_LESS "0.3.0")
ADD_DEFINITIONS(-DPINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME)
PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_URDFDOM_COLLISION_WITH_GROUP_NAME")
......@@ -133,7 +146,7 @@ IF(URDFDOM_FOUND)
ADD_DEFINITIONS(-DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR)
PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR")
ENDIF(${URDFDOM_VERSION} VERSION_GREATER "0.4.2")
ENDIF(URDFDOM_FOUND)
ENDIF(URDFDOM_FOUND AND BUILD_WITH_URDF_SUPPORT)
# Special care of lua which can be of versions 5.1 or 5.2
# Priority is set to lua5.2
......
......@@ -38,7 +38,10 @@ SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
# --- DEPENDENCIES --- #
# The following quiet finding of Eigen is necessary to retrive
# the include directory when packaging Pinocchio in ROS
find_package(Eigen3 QUIET)
IF(BUILD_WITH_CATKIN_SUPPORT)
find_package(Eigen3 QUIET)
ENDIF(BUILD_WITH_CATKIN_SUPPORT)
if (EIGEN3_FOUND)
include_directories(${EIGEN3_INCLUDE_DIR})
endif()
......
......@@ -52,6 +52,7 @@ namespace pinocchio
namespace details {
// auxiliary function for bodyRegressor: bigL(omega)*I.toDynamicParameters().tail<6>() = I.inertia() * omega
/*
template<typename Vector3Like>
inline Eigen::Matrix<typename Vector3Like::Scalar,3,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
bigL(const Eigen::MatrixBase<Vector3Like> & omega)
......@@ -66,6 +67,31 @@ namespace pinocchio
Scalar(0), Scalar(0), Scalar(0), omega[0], omega[1], omega[2];
return L;
}
*/
// auxiliary function for bodyRegressor: res += bigL(omega)
template<typename Vector3Like, typename OutputType>
inline void
addBigL(const Eigen::MatrixBase<Vector3Like> & omega, const Eigen::MatrixBase<OutputType> & out)
{
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out);
res(0,0)+=omega[0]; res(0,1)+=omega[1]; res(0,3)+=omega[2];
res(1,1)+=omega[0]; res(1,2)+=omega[1]; res(1,4)+=omega[2];
res(2,3)+=omega[0]; res(2,4)+=omega[1]; res(2,5)+=omega[2];
}
// auxiliary function for bodyRegressor: res = cross(omega,bigL(omega))
template<typename Vector3Like, typename OutputType>
inline void
crossBigL(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<OutputType> & out)
{
typedef typename Vector3Like::Scalar Scalar;
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out);
res << Scalar(0), -v[2]*v[0], -v[2]*v[1], v[1]*v[0], v[1]*v[1]-v[2]*v[2], v[2]*v[1],
v[2]*v[0], v[2]*v[1], Scalar(0), v[2]*v[2]-v[0]*v[0], -v[1]*v[0], -v[2]*v[0],
-v[1]*v[0], v[0]*v[0]-v[1]*v[1], v[1]*v[0], -v[2]*v[1], v[2]*v[0], Scalar(0);
}
}
template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
......@@ -79,12 +105,13 @@ namespace pinocchio
typedef Symmetric3Tpl<Scalar,Options> Symmetric3;
typedef typename Symmetric3::SkewSquare SkewSquare;
using ::pinocchio::details::bigL;
using ::pinocchio::details::crossBigL;
using ::pinocchio::details::addBigL;
Eigen::MatrixBase<OutputType> & res = const_cast< Eigen::MatrixBase<OutputType> & >(regressor);
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, regressor);
res.template block<3,1>(MotionVelocity::LINEAR,0) = a.linear() + v.angular().cross(v.linear());
const Eigen::Matrix<Scalar, 3, 1, Options> & acc = res.template block<3,1>(MotionVelocity::LINEAR,0);
const Eigen::Block<OutputType,3,1> & acc = res.template block<3,1>(MotionVelocity::LINEAR,0);
res.template block<3,3>(MotionVelocity::LINEAR,1) = Symmetric3(SkewSquare(v.angular())).matrix();
addSkew(a.angular(), res.template block<3,3>(MotionVelocity::LINEAR,1));
......@@ -92,7 +119,9 @@ namespace pinocchio
res.template block<3,1>(MotionVelocity::ANGULAR,0).setZero();
skew(-acc, res.template block<3,3>(MotionVelocity::ANGULAR,1));
res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(), bigL(v.angular()));
// res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(), bigL(v.angular()));
crossBigL(v.angular(), res.template block<3,6>(MotionVelocity::ANGULAR,4));
addBigL(a.angular(), res.template block<3,6>(MotionVelocity::ANGULAR,4));
}
template<typename MotionVelocity, typename MotionAcceleration>
......
//
// Copyright (c) 2018 CNRS INRIA
// Copyright (c) 2018-2019 CNRS INRIA
//
#ifndef __pinocchio_fwd_hpp__
#define __pinocchio_fwd_hpp__
// Forward declaration of main pinocchio namespace
// Forward declaration of the main pinocchio namespace
namespace pinocchio {}
#include "pinocchio/macros.hpp"
......@@ -15,10 +15,10 @@ namespace pinocchio {}
#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
#include "pinocchio/math/cppad.hpp"
#else
#include <Eigen/Dense>
#endif
#include <Eigen/Dense>
#include "pinocchio/eigen-macros.hpp"
namespace pinocchio
......@@ -28,6 +28,11 @@ namespace pinocchio
///
template<class C> struct traits {};
namespace internal
{
template<typename T> struct traits {};
}
///
/// \brief Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type.
/// This class should be specialized for each types.
......
This diff is collapsed.
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_math_comparison_operators_hpp__
#define __pinocchio_math_comparison_operators_hpp__
namespace pinocchio
{
#define PINOCCHIO_DEFINE_COMPARISON_OP(name,OP) \
struct name \
{ \
template<typename T1, typename T2> \
static bool call(const T1 & a, const T2 & b) \
{ return a OP b;} \
}
PINOCCHIO_DEFINE_COMPARISON_OP(equal_to_op,==);
PINOCCHIO_DEFINE_COMPARISON_OP(not_equal_to_op,!=);
PINOCCHIO_DEFINE_COMPARISON_OP(less_than_op,<);
PINOCCHIO_DEFINE_COMPARISON_OP(greater_than_op,>);
PINOCCHIO_DEFINE_COMPARISON_OP(less_than_or_equal_to_op,<=);
PINOCCHIO_DEFINE_COMPARISON_OP(greater_than_or_equal_to_op,>=);
template<class OP, bool condition, bool default_return_value>
struct apply_op_if
{
template<typename T1, typename T2>
static bool op(const T1 & /*a*/, const T2 & /*b*/)
{
return default_return_value;
}
};
template<class OP, bool default_return_value>
struct apply_op_if<OP,true,default_return_value>
{
template<typename T1, typename T2>
static bool op(const T1 & a, const T2 & b)
{
return OP::call(a,b);
}
};
}
#endif //#ifndef __pinocchio_math_comparison_operators_hpp__
......@@ -8,9 +8,8 @@
#include "pinocchio/fwd.hpp"
#include <math.h>
#include <boost/math/constants/constants.hpp>
#include "pinocchio/math/sincos.hpp"
#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
namespace boost
{
namespace math
......@@ -19,18 +18,20 @@ namespace boost
{
namespace detail
{
#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
template<typename Scalar>
struct constant_pi< CppAD::AD<Scalar> > : constant_pi<Scalar> {};
#if defined(PINOCCHIO_WITH_CPPADCG_SUPPORT) && defined(PINOCCHIO_WITH_CXX11_SUPPORT)
template<typename Scalar>
struct constant_pi< CppAD::cg::CG<Scalar> > : constant_pi<Scalar> {};
#endif
#endif
}
}
}
}
#endif
namespace pinocchio
{
......@@ -43,30 +44,49 @@ namespace pinocchio
const Scalar PI()
{ return boost::math::constants::pi<Scalar>(); }
/// \brief Foward declaration of TaylorSeriesExpansion.
template<typename Scalar> struct TaylorSeriesExpansion;
namespace math
{
using std::fabs;
using std::sqrt;
using std::atan;
using std::acos;
using std::asin;
using std::pow;
using std::cos;
using std::sin;
#ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
using CppAD::fabs;
using CppAD::sqrt;
using CppAD::atan;
using CppAD::acos;
using CppAD::asin;
using CppAD::atan2;
using CppAD::pow;
using CppAD::cos;
using CppAD::sin;
#else
using std::atan2;
#endif
#define PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(name) \
template<typename Scalar> \
Scalar name(const Scalar & value) \
{ using std::name; return name(value); }
#define PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(name) \
namespace internal \
{ \
template<typename T1, typename T2> \
struct return_type_##name \
{ \
typedef T1 type; \
}; \
template<typename T1, typename T2> \
struct call_##name \
{ \
static inline typename return_type_##name<T1,T2>::type \
run(const T1 & a, const T2 & b) \
{ using std::name; return name(a,b); } \
}; \
} \
template<typename T1, typename T2> \
inline typename internal::return_type_##name<T1,T2>::type name(const T1 & a, const T2 & b) \
{ return internal::call_##name<T1,T2>::run(a,b); }
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(fabs)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(sqrt)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(atan)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(acos)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(asin)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(cos)
PINOCCHIO_OVERLOAD_MATH_UNARY_OPERATOR(sin)
PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(pow)
PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(min)
PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(max)
PINOCCHIO_OVERLOAD_MATH_BINARY_OPERATOR(atan2)
}
}
......
......@@ -6,6 +6,7 @@
#define __pinocchio_math_matrix_hpp__
#include <Eigen/Core>
#include <boost/type_traits.hpp>
namespace pinocchio
{
......@@ -25,6 +26,71 @@ namespace pinocchio
typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
#endif
};
namespace internal
{
template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
struct isUnitaryAlgo
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
const RealScalar & prec =
Eigen::NumTraits< Scalar >::dummy_precision())
{
return mat.isUnitary(prec);
}
};
template<typename MatrixLike>
struct isUnitaryAlgo<MatrixLike,false>
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
const RealScalar & prec =
Eigen::NumTraits< Scalar >::dummy_precision())
{
PINOCCHIO_UNUSED_VARIABLE(prec);
return true;
}
};
}
template<typename MatrixLike>
inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
const typename MatrixLike::RealScalar & prec =
Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
{
return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
}
namespace internal
{
template<typename Scalar>
struct CallCorrectMatrixInverseAccordingToScalar
{
template<typename MatrixIn, typename MatrixOut>
static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = const_cast<MatrixOut &>(dest.derived());
dest_.noalias() = m_in.inverse();
}
};
}
template<typename MatrixIn, typename MatrixOut>
inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = const_cast<MatrixOut &>(dest.derived());
internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
}
}
#endif //#ifndef __pinocchio_math_matrix_hpp__
//
// Copyright (c) 2016,2018 CNRS, INRIA
// Copyright (c) 2016-2019 CNRS, INRIA
//
#ifndef __math_quaternion_hpp__
#define __math_quaternion_hpp__
#ifndef __pinocchio_math_quaternion_hpp__
#define __pinocchio_math_quaternion_hpp__
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/math/sincos.hpp"
#include <boost/type_traits.hpp>
#include <Eigen/Geometry>
namespace pinocchio
{
......@@ -26,7 +31,7 @@ namespace pinocchio
{
typedef typename D1::Scalar Scalar;
const Scalar innerprod = q1.dot(q2);
Scalar theta = acos(innerprod);
Scalar theta = math::acos(innerprod);
static const Scalar PI_value = PI<Scalar>();
if(innerprod < 0)
......@@ -80,14 +85,15 @@ namespace pinocchio
const Scalar N2 = q.squaredNorm();
#ifndef NDEBUG
const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
assert(math::fabs(N2-1.) <= epsilon);
typedef apply_op_if<less_than_or_equal_to_op,boost::is_floating_point<Scalar>::value,true> static_leq;
assert(static_leq::op(math::fabs(N2-1.), epsilon));
#endif
const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
#ifndef NDEBUG
const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
assert(math::fabs(q.norm() - Scalar(1)) <=
std::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision()));
assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
#endif
}
......@@ -105,7 +111,7 @@ namespace pinocchio
const Scalar mult1 = sqrt(Scalar(1)-u1);
const Scalar mult2 = sqrt(u1);
const Scalar PI_value = PI<Scalar>();
static const Scalar PI_value = PI<Scalar>();
Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);
......@@ -154,5 +160,93 @@ namespace pinocchio
{
quaternion::uniformRandom(q);
}
namespace quaternion
{
namespace internal
{
template<Eigen::DenseIndex i>
struct quaternionbase_assign_impl_if_t_negative
{
template<typename Scalar, typename Matrix3, typename QuaternionDerived>
static inline void run(Scalar t,
Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 & mat)
{
using pinocchio::math::sqrt;
Eigen::DenseIndex j = (i+1)%3;
Eigen::DenseIndex k = (j+1)%3;
t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
q.coeffs().coeffRef(i) = Scalar(0.5) * t;
t = Scalar(0.5)/t;
q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
}
};
struct quaternionbase_assign_impl_if_t_positive
{
template<typename Scalar, typename Matrix3, typename QuaternionDerived>
static inline void run(Scalar t,
Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 & mat)
{
using pinocchio::math::sqrt;
t = sqrt(t + Scalar(1.0));
q.w() = Scalar(0.5)*t;
t = Scalar(0.5)/t;
q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
}
};
template<typename Scalar>
struct quaternionbase_assign_impl
{
template<typename Matrix3, typename QuaternionDerived>
static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 & mat)
{
using pinocchio::math::sqrt;
Scalar t = mat.trace();
if (t > Scalar(0))
quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
else
{
Eigen::DenseIndex i = 0;
if (mat.coeff(1,1) > mat.coeff(0,0))
i = 1;
if (mat.coeff(2,2) > mat.coeff(i,i))
i = 2;
if(i==0)
quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
else if(i==1)
quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
else
quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
}
}
};
} // namespace internal
template<typename D, typename Matrix3>
void assignQuaternion(Eigen::QuaternionBase<D> & quat,
const Eigen::MatrixBase<Matrix3> & R)
{
internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
R.derived());
}
} // namespace quaternion
}
#endif //#ifndef __math_quaternion_hpp__
#endif //#ifndef __pinocchio_math_quaternion_hpp__
......@@ -6,14 +6,13 @@
#ifndef __math_sincos_hpp__
#define __math_sincos_hpp__
#include "pinocchio/fwd.hpp"
#include <boost/type_traits.hpp>
#include <cmath>
namespace pinocchio
{
/// Forward declaration
template<typename Scalar,bool value = boost::is_floating_point<Scalar>::value> struct SINCOSAlgo;
template<typename Scalar, bool value = boost::is_floating_point<Scalar>::value> struct SINCOSAlgo;
///
/// \brief Computes sin/cos values of a given input scalar.
......
......@@ -29,7 +29,7 @@ namespace pinocchio
template<int degree>
static Scalar precision()
{
static Scalar value = std::pow(std::numeric_limits<Scalar>::epsilon(),Scalar(1)/Scalar(degree+1));
static Scalar value = math::pow(std::numeric_limits<Scalar>::epsilon(),Scalar(1)/Scalar(degree+1));
return value;
}
}; // struct TaylorSeriesExpansion
......
......@@ -21,7 +21,8 @@
typedef TYPENAME traits<DERIVED>::DenseBase DenseBase; \
typedef TYPENAME traits<DERIVED>::MatrixReturnType MatrixReturnType; \
typedef TYPENAME traits<DERIVED>::ConstMatrixReturnType ConstMatrixReturnType; \
enum { LINEAR = traits<DERIVED>::LINEAR, ANGULAR = traits<DERIVED>::ANGULAR };
enum { LINEAR = traits<DERIVED>::LINEAR, ANGULAR = traits<DERIVED>::ANGULAR }; \
enum { Options = traits<DERIVED>::Options };
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,typename)
#define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED,PINOCCHIO_EMPTY_ARG)
......@@ -62,9 +63,25 @@ namespace pinocchio
return os;
}
typename SE3GroupAction<Derived>::ReturnType
se3Action(const SE3Tpl<Scalar,Options> & m) const
{
return derived().se3Action(m);
}
typename SE3GroupAction<Derived>::ReturnType
se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
{
return derived().se3ActionInverse(m);
}
template<typename MotionDerived>
DenseBase motionAction(const MotionDense<MotionDerived> & v) const
{ return derived().motionAction(v); }
typename MotionAlgebraAction<Derived,MotionDerived>::ReturnType
motionAction(const MotionDense<MotionDerived> & v) const
{
return derived().motionAction(v);
}
}; // class ConstraintBase
......
......@@ -30,17 +30,14 @@ namespace pinocchio
}; // traits ConstraintTpl
namespace internal
{
template<int Dim, typename Scalar, int Options>
struct SE3GroupAction< ConstraintTpl<Dim,Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
template<int Dim, typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintTpl<Dim,Scalar,Options>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
}
template<int Dim, typename Scalar, int Options>
struct SE3GroupAction< ConstraintTpl<Dim,Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
template<int Dim, typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintTpl<Dim,Scalar,Options>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,Dim> ReturnType; };
template<int _Dim, typename _Scalar, int _Options>
struct ConstraintTpl
: public ConstraintBase< ConstraintTpl<_Dim,_Scalar,_Options> >
......@@ -52,7 +49,7 @@ namespace pinocchio
friend class ConstraintBase<ConstraintTpl>;
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTpl)
enum { NV = _Dim, Options = _Options };
enum { NV = _Dim };
using Base::nv;
......@@ -127,14 +124,14 @@ namespace pinocchio
}
DenseBase se3Action(const SE3 & m) const
DenseBase se3Action(const SE3Tpl<Scalar,Options> & m) const
{
DenseBase res(6,nv());
motionSet::se3Action(m,S,res);
return res;
}
DenseBase se3ActionInverse(const SE3 & m) const
DenseBase se3ActionInverse(const SE3Tpl<Scalar,Options> & m) const
{
DenseBase res(6,nv());
motionSet::se3ActionInverse(m,S,res);
......
......@@ -155,11 +155,8 @@ namespace pinocchio
typedef ForceSetTpl<double,0> ForceSet;
namespace internal
{
template<>
struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };
}
template<>
struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };
} // namespace pinocchio
......
......@@ -399,7 +399,6 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
const Eigen::MatrixBase<TangentVectorType> & vs) const
{
derived().calc(data,qs.derived(),vs.derived());
}
template<typename Matrix6Type>
......@@ -429,9 +428,11 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
int idx_v_impl() const { return i_v; }
JointIndex id_impl() const { return i_id; }
void setIndexes(JointIndex id, int q, int v) { derived().setIndexes_impl(id, q, v); }
void setIndexes(JointIndex id, int q, int v)
{ derived().setIndexes_impl(id, q, v); }
void setIndexes_impl(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setIndexes_impl(JointIndex id,int q,int v)
{ i_id = id, i_q = q; i_v = v; }
void disp(std::ostream & os) const
{
......@@ -468,7 +469,8 @@ struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
{ return !(derived() == other.derived()); }
template <class OtherDerived>
bool isEqual(const JointModelBase<OtherDerived> &) const { return false; }
bool isEqual(const JointModelBase<OtherDerived> &) const
{ return false; }
bool isEqual(const JointModelBase<Derived> & other) const
{
......
//
// Copyright (c) 2019 INRIA
//
#ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
#define __pinocchio_multibody_joint_joint_common_operations_hpp__
#include "pinocchio/macros.hpp"
#include <boost/type_traits.hpp>
namespace pinocchio
{
namespace internal
{
template<typename Scalar, bool is_floating_point = boost::is_floating_point<Scalar>::value>
struct PerformStYSInversion
{
template<typename M1, typename M2>
static EIGEN_STRONG_INLINE
void run(const Eigen::MatrixBase<M1> & StYS,
const Eigen::MatrixBase<M2> & Dinv)
{
M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2,Dinv);
Dinv_.setIdentity();
StYS.llt().solveInPlace(Dinv_);
}
};
template<typename Scalar>
struct PerformStYSInversion<Scalar, false>
{
template<typename M1, typename M2>
static EIGEN_STRONG_INLINE
void run(const Eigen::MatrixBase<M1> & StYS,
const Eigen::MatrixBase<M2> & Dinv)
{
M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2,Dinv);
inverse(StYS,Dinv_);
}
};
}
}
#endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
......@@ -268,18 +268,21 @@ namespace pinocchio
const Eigen::MatrixBase<TangentVectorType> & vs) const;
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const
{
data.U.noalias() = I * data.S.matrix();
data.StU.noalias() = data.S.matrix().transpose() * data.U;
// compute inverse
data.Dinv.setIdentity();
data.StU.llt().solveInPlace(data.Dinv);
// data.Dinv.setIdentity();
// data.StU.llt().solveInPlace(data.Dinv);
internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
}
int nv_impl() const { return m_nv; }
......
......@@ -13,6 +13,7 @@
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/quaternion.hpp"
#include "pinocchio/multibody/joint/joint-common-operations.hpp"
namespace pinocchio
{
......@@ -44,7 +45,7 @@ namespace pinocchio
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)
enum { NV = 6, Options = _Options };
enum { NV = 6 };
template<typename Vector6Like>
JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
......@@ -54,7 +55,7 @@ namespace pinocchio
}
template<typename S1, int O1>
typename SE3::ActionMatrixType se3Action(const SE3Tpl<S1,O1> & m) const
typename SE3Tpl<S1,O1>::ActionMatrixType se3Action(const SE3Tpl<S1,O1> & m) const
{ return m.toActionMatrix(); }
int nv_impl() const { return NV; }
......@@ -112,16 +113,13 @@ namespace pinocchio
return Y.derived();
}
namespace internal
{
template<typename S1, int O1>
struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
template<typename S1, int O1, typename MotionDerived>
struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
}
template<typename S1, int O1>
struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
template<typename S1, int O1, typename MotionDerived>
struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
template<typename Scalar, int Options> struct JointFreeFlyerTpl;
......@@ -269,11 +267,16 @@ namespace pinocchio
}
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const
{
data.U = I;
data.Dinv.setIdentity();
I.llt().solveInPlace(data.Dinv);
// compute inverse
// data.Dinv.setIdentity();
// I.llt().solveInPlace(data.Dinv);
internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
if (update_I)
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
......
......@@ -19,21 +19,18 @@ namespace pinocchio
template<typename Scalar, int Options = 0> struct MotionPlanarTpl;
typedef MotionPlanarTpl<double> MotionPlanar;
namespace internal
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPlanarTpl<Scalar,Options> >
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPlanarTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPlanarTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPlanarTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename _Scalar, int _Options>
struct traits< MotionPlanarTpl<_Scalar,_Options> >
{
......@@ -207,7 +204,7 @@ namespace pinocchio
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPlanarTpl)
enum { NV = 3, Options = _Options };
enum { NV = 3 };
template<typename Vector3Like>
JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & vj) const
......@@ -344,16 +341,13 @@ namespace pinocchio
return IS;
}
namespace internal
{
template<typename S1, int O1>
struct SE3GroupAction< ConstraintPlanarTpl<S1,O1> >
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
template<typename S1, int O1, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPlanarTpl<S1,O1>,MotionDerived >
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
}
template<typename S1, int O1>
struct SE3GroupAction< ConstraintPlanarTpl<S1,O1> >
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
template<typename S1, int O1, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPlanarTpl<S1,O1>,MotionDerived >
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
template<typename Scalar, int Options> struct JointPlanarTpl;
......@@ -478,7 +472,9 @@ namespace pinocchio
}
template<typename Matrix6Like>
void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
void calc_aba(JointDataDerived & data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I) const
{
data.U.template leftCols<2>() = I.template leftCols<2>();
data.U.template rightCols<1>() = I.template rightCols<1>();
......@@ -487,13 +483,14 @@ namespace pinocchio
data.StU.template rightCols<1>() = data.U.template bottomRows<1>();
// compute inverse
data.Dinv.setIdentity();
data.StU.llt().solveInPlace(data.Dinv);
// data.Dinv.setIdentity();
// data.StU.llt().solveInPlace(data.Dinv);
internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
data.UDinv.noalias() = data.U * data.Dinv;
if (update_I)
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
if(update_I)
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
}
static std::string classname() { return std::string("JointModelPlanar");}
......
......@@ -19,21 +19,18 @@ namespace pinocchio
template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
typedef MotionPrismaticUnalignedTpl<double> MotionPrismaticUnaligned;
namespace internal
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename _Scalar, int _Options>
struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
{
......@@ -176,7 +173,7 @@ namespace pinocchio
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnaligned)
enum { NV = 1, Options = _Options };
enum { NV = 1 };
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
......@@ -308,17 +305,14 @@ namespace pinocchio
}
namespace internal
{
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
}
template<typename Scalar, int Options>
struct SE3GroupAction< ConstraintPrismaticUnaligned<Scalar,Options> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismaticUnaligned<Scalar,Options>,MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
template<typename _Scalar, int _Options>
......@@ -411,11 +405,13 @@ namespace pinocchio
typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
JointModelPrismaticUnalignedTpl() {}
JointModelPrismaticUnalignedTpl(Scalar x, Scalar y, Scalar z)
JointModelPrismaticUnalignedTpl(const Scalar & x,
const Scalar & y,
const Scalar & z)
: axis(x,y,z)
{
axis.normalize();
assert(axis.isUnitary() && "Translation axis is not unitary");
assert(isUnitary(axis) && "Translation axis is not unitary");
}
template<typename Vector3Like>
......@@ -423,7 +419,7 @@ namespace pinocchio
: axis(axis)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
assert(axis.isUnitary() && "Translation axis is not unitary");
assert(isUnitary(axis) && "Translation axis is not unitary");
}
JointDataDerived createData() const { return JointDataDerived(axis); }
......
......@@ -18,21 +18,18 @@ namespace pinocchio
template<typename Scalar, int Options, int _axis> struct MotionPrismaticTpl;
namespace internal
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
{
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
}
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename _Scalar, int _Options, int _axis>
struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
{
......@@ -184,13 +181,10 @@ namespace pinocchio
typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
}; // traits TransformPrismaticTpl
namespace internal
{
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
{ typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };
}
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
{ typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };
template<typename _Scalar, int _Options, int axis>
struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
{
......@@ -218,10 +212,10 @@ namespace pinocchio
operator PlainType() const { return plain(); }
template<typename S2, int O2>
typename internal::SE3GroupAction<TransformPrismaticTpl>::ReturnType
typename SE3GroupAction<TransformPrismaticTpl>::ReturnType
se3action(const SE3Tpl<S2,O2> & m) const
{
typedef typename internal::SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
ReturnType res(m);
res.translation()[axis] += m_displacement;
......@@ -263,7 +257,7 @@ namespace pinocchio
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismatic)
enum { NV = 1, Options = _Options };
enum { NV = 1 };
typedef SpatialAxis<LINEAR+axis> Axis;
......@@ -386,17 +380,14 @@ namespace pinocchio
return Y.derived().col(Inertia::LINEAR + axis);
}
namespace internal
{
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< ConstraintPrismatic<Scalar,Options,axis> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismatic<Scalar,Options,axis>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
}
template<typename Scalar, int Options, int axis>
struct SE3GroupAction< ConstraintPrismatic<Scalar,Options,axis> >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename Scalar, int Options, int axis, typename MotionDerived>
struct MotionAlgebraAction< ConstraintPrismatic<Scalar,Options,axis>, MotionDerived >
{ typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
template<typename _Scalar, int _Options, int _axis>
struct JointPrismaticTpl
{
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -18,20 +18,17 @@ namespace pinocchio
template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl;
typedef MotionRevoluteUnalignedTpl<double> MotionRevoluteUnaligned;
namespace internal
template<typename Scalar, int Options>
struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
{
template<typename Scalar, int Options>
struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
{
typedef MotionTpl<Scalar,Options> ReturnType;
};
template<typename Scalar