Commit 8d85fd10 authored by jcarpent's avatar jcarpent
Browse files

[Spatial] Introduce ForceDense and ForceRef

parent b4391843
......@@ -183,6 +183,10 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
spatial/motion-ref.hpp
spatial/motion-zero.hpp
spatial/force.hpp
spatial/force-base.hpp
spatial/force-dense.hpp
spatial/force-tpl.hpp
spatial/force-ref.hpp
spatial/inertia.hpp
spatial/fwd.hpp
spatial/skew.hpp
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// 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_force_base_hpp__
#define __se3_force_base_hpp__
namespace se3
{
/**
* @brief Base interface for forces representation.
* @details The Class implements all
* \ingroup Force_group
*
* @tparam Derived { description }
*/
template< class Derived>
class ForceBase
{
public:
FORCE_TYPEDEF_TPL(Derived);
Derived & derived() { return *static_cast<Derived*>(this); }
const Derived& derived() const { return *static_cast<const Derived*>(this); }
/**
* @brief Return the angular part of the force vector
*
* @return The 3D vector associated to the angular part of the 6D force vector
*/
ConstAngularType angular() const { return derived().angular_impl(); }
/**
* @brief Return the linear part of the force vector
*
* @return The 3D vector associated to the linear part of the 6D force vector
*/
ConstLinearType linear() const { return derived().linear_impl(); }
/// \copydoc ForceBase::angular
AngularType angular() { return derived().angular_impl(); }
/// \copydoc ForceBase::linear
LinearType linear() { return derived().linear_impl(); }
/**
* @brief Set the angular part of the force vector
*
* @tparam V3 A vector 3 like type.
*
* @param[in] n
*/
template<typename V3>
void angular(const Eigen::MatrixBase<V3> & n) { derived().angular_impl(n.derived()); }
/**
* @brief Set the linear part of the force vector
*
* @tparam V3 A vector 3 like type.
*
* @param[in] f
*/
template<typename V3>
void linear(const Eigen::MatrixBase<V3> & f) { derived().linear_impl(f.derived()); }
/**
* @brief Return the force as an Eigen vector.
*
* @return The 6D vector \f$ \phi \f$ such that
* \f{equation*}
* \leftidx{^A}\phi = \begin{bmatrix} \leftidx{^A}f \\ \leftidx{^A}\tau \end{bmatrix}
* \f}
*/
ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
/// \copydoc ForceBase::toVector
ToVectorReturnType toVector() { return derived().toVector_impl(); }
/*
* @brief C-style cast operator
* \copydoc ForceBase::toVector
*/
operator Vector6 () const { return toVector(); }
/** \returns true if each coefficients of \c *this and \a other are all exactly equal.
* \warning When using floating point scalar values you probably should rather use a
* fuzzy comparison such as isApprox()
*/
template<typename F2>
bool operator== (const ForceBase<F2> & other) const {return derived().isEqual_impl(other.derived());}
/** \returns true if at least one coefficient of \c *this and \a other does not match.
*/
template<typename F2>
bool operator!=(const ForceBase<F2> & other) const { return !(derived() == other.derived()); }
/** \returns true if *this is approximately equal to other, within the precision given by prec.
*/
bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{ return derived().isApprox_impl(other, prec); }
/** \brief Copies the Derived Force into *this
* \return a reference to *this
*/
Derived & operator= (const Derived & other) { return derived().__equl__(other); }
/**
* \brief Replaces *this by *this + other.
* \return a reference to *this
*/
Derived & operator+= (const Derived & phi) { return derived().__pequ__(phi); }
/**
* \brief Replaces *this by *this - other.
* \return a reference to *this
*/
Derived & operator-= (const Derived & phi) { return derived().__mequ__(phi); }
/** \return an expression of the sum of *this and other
*/
Derived operator+(const Derived & phi) const { return derived().__plus__(phi); }
/** \return an expression of *this scaled by the double factor a
*/
Derived operator*(const Scalar alpha) const { return derived().__mult__(alpha); }
/** \return an expression of the opposite of *this
*/
Derived operator-() const { return derived().__minus__(); }
/** \return an expression of the difference of *this and phi
*/
Derived operator-(const Derived & phi) const { return derived().__minus__(phi); }
/** \return the dot product of *this with m *
*/
Scalar dot(const Motion & m) const { return static_cast<Derived*>(this)->dot(m); }
/**
* @brief Transform from A to B coordinates the Force represented by *this such that
* \f{equation*}
* \leftidx{^B}f = \leftidx{^B}X_A^* * \leftidx{^A}f
* \f}
*
* @param[in] m The rigid transformation \f$ \leftidx{^B}m_A \f$ whose coordinates transform for forces is
* \leftidx{^B}X_A^*
*
* @return an expression of the force expressed in the new coordinates
*/
typename internal::SE3GroupAction<Derived>::ReturnType
se3Action(const SE3 & m) const
{ return derived().se3Action_impl(m); }
/**
* @brief Transform from B to A coordinates the Force represented by *this such that
* \f{equation*}
* \leftidx{^A}f = \leftidx{^A}X_B^* * \leftidx{^A}f
* \f}
*
* @param[in] m The rigid transformation \f$ \leftidx{^B}m_A \f$ whose coordinates transform for forces is
* \leftidx{^B}X_A^*
*
* @return an expression of the force expressed in the new coordinates
*/
typename internal::SE3GroupAction<Derived>::ReturnType
se3ActionInverse(const SE3 & m) const
{ return derived().se3ActionInverse_impl(m); }
template<typename M1>
typename internal::MotionAlgebraAction<Derived>::ReturnType motionAction(const MotionDense<M1> & v) const
{
return derived().motionAction(v.derived());
}
void disp(std::ostream & os) const { derived().disp_impl(os); }
friend std::ostream & operator << (std::ostream & os, const ForceBase<Derived> & X)
{
X.disp(os);
return os;
}
}; // class ForceBase
} // namespace se3
#endif // ifndef __se3_force_base_hpp__
//
// Copyright (c) 2017-2018 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_force_dense_hpp__
#define __se3_force_dense_hpp__
namespace se3
{
template<typename Derived>
class ForceDense : public ForceBase<Derived>
{
public:
typedef ForceBase<Derived> Base;
FORCE_TYPEDEF_TPL(Derived);
using Base::linear;
using Base::angular;
using Base::derived;
using Base::isApprox;
Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
Derived & setRandom() { linear().setZero(); angular().setZero(); return derived(); }
template<typename D2>
bool isEqual_impl(const ForceDense<D2> & other) const
{ return linear() == other.linear() && angular() == other.angular(); }
template<typename D2>
bool isEqual_impl(const ForceBase<D2> & other) const
{ return other.derived() == derived(); }
// Arithmetic operators
template<typename D2>
Derived & operator=(const ForceDense<D2> & other)
{
linear() = other.linear();
angular() = other.angular();
return derived();
}
template<typename V6>
Derived & operator=(const Eigen::MatrixBase<V6> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
linear() = v.template segment<3>(LINEAR);
angular() = v.template segment<3>(ANGULAR);
return derived();
}
ForcePlain operator-() const { return derived().__minus__(); }
template<typename M1>
ForcePlain operator+(const ForceDense<M1> & v) const { return derived().__plus__(v.derived()); }
template<typename M1>
ForcePlain operator-(const ForceDense<M1> & v) const { return derived().__minus__(v.derived()); }
template<typename M1>
Derived & operator+=(const ForceDense<M1> & v) { return derived().__pequ__(v.derived()); }
template<typename M1>
Derived & operator+=(const ForceBase<M1> & v)
{ v.derived().addTo(derived()); return derived(); }
template<typename M1>
Derived & operator-=(const ForceDense<M1> & v) { return derived().__mequ__(v.derived()); }
ForcePlain operator*(const Scalar alpha) const { return derived().__mult__(alpha); }
ForcePlain __minus__() const { return ForcePlain(-linear(),-angular()); }
template<typename M1>
ForcePlain __plus__(const ForceDense<M1> & v) const
{ return ForcePlain(linear()+v.linear(), angular()+v.angular()); }
template<typename M1>
ForcePlain __minus__(const ForceDense<M1> & v) const
{ return ForcePlain(linear()-v.linear(), angular()-v.angular()); }
template<typename M1>
Derived & __pequ__(const ForceDense<M1> & v)
{ linear() += v.linear(); angular() += v.angular(); return derived(); }
template<typename M1>
Derived & __mequ__(const ForceDense<M1> & v)
{ linear() -= v.linear(); angular() -= v.angular(); return derived(); }
template<typename S1>
ForcePlain __mult__(const S1 alpha) const
{ return ForcePlain(alpha*linear(),alpha*angular()); }
template<typename F1>
Scalar dot(const MotionDense<F1> & phi) const
{ return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
{
fout.linear() = v.angular().cross(linear());
fout.angular() = v.angular().cross(angular())+v.linear().cross(linear());
}
template<typename M1>
ForcePlain motionAction(const MotionDense<M1> & v) const
{
ForcePlain res;
motionAction(v,res);
return res;
}
template<typename M2>
bool isApprox(const ForceDense<M2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{ return derived().isApprox_impl(f, prec);}
template<typename D2>
bool isApprox_impl(const ForceDense<D2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
{
f.linear() = m.rotation()*linear();
f.angular() = m.rotation()*angular() + m.translation().cross(f.linear());
}
template<typename S2, int O2>
ForcePlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
{
ForcePlain res;
se3Action_impl(m,res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
{
f.linear() = angular()-m.translation().cross(linear());
f.angular() = m.rotation().transpose()*(f.linear());
f.linear() = m.rotation().transpose()*linear();
}
template<typename S2, int O2>
ForcePlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
{
ForcePlain res;
se3ActionInverse_impl(m,res);
return res;
}
void disp_impl(std::ostream & os) const
{
os
<< " f = " << linear().transpose () << std::endl
<< "tau = " << angular().transpose () << std::endl;
}
}; // class ForceDense
/// Basic operations specialization
template<typename F1>
typename traits<F1>::ForcePlain operator*(const typename traits<F1>::Scalar alpha,
const ForceDense<F1> & f)
{ return f.derived()*alpha; }
} // namespace se3
#endif // ifndef __se3_force_dense_hpp__
//
// Copyright (c) 2017-2018 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_force_ref_hpp__
#define __se3_force_ref_hpp__
namespace se3
{
template<typename Vector6ArgType>
struct traits< ForceRef<Vector6ArgType> >
{
typedef typename Vector6ArgType::Scalar Scalar;
typedef typename EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
enum {
LINEAR = 0,
ANGULAR = 3,
Options = Vector6::Options
};
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Matrix6 ActionMatrixType;
typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef ForceTpl<Scalar,Options> ForcePlain;
typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
typedef DataRefType ToVectorReturnType;
typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
typedef ConstDataRefType ToVectorConstReturnType;
}; // traits ForceRef
namespace internal
{
template<typename Vector6ArgType>
struct SE3GroupAction< ForceRef<Vector6ArgType> >
{
typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
};
template<typename Vector6ArgType>
struct MotionAlgebraAction< ForceRef<Vector6ArgType> >
{
typedef typename traits< ForceRef<Vector6ArgType> >::ForcePlain ReturnType;
};
}
template<typename Vector6ArgType>
class ForceRef : public ForceDense< ForceRef<Vector6ArgType> >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef ForceDense<ForceRef> Base;
typedef typename traits<ForceRef>::DataRefType DataRefType;
FORCE_TYPEDEF_TPL(ForceRef);
using Base::operator=;
ForceRef(Vector6ArgType & f_like)
: ref(f_like)
{
EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
assert(f_like.size() == 6);
}
ToVectorConstReturnType toVector_impl() const { return ref; }
ToVectorReturnType toVector_impl() { return ref; }
// Getters
ConstAngularType angular_impl() const { return ConstAngularType(ref.derived(),ANGULAR); }
ConstLinearType linear_impl() const { return ConstLinearType(ref.derived(),LINEAR); }
AngularType angular_impl() { return ref.template segment<3> (ANGULAR); }
LinearType linear_impl() { return ref.template segment<3> (LINEAR); }
template<typename V3>
void angular_impl(const Eigen::MatrixBase<V3> & w)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
angular_impl()=w;
}
template<typename V3>
void linear_impl(const Eigen::MatrixBase<V3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
linear_impl()=v;
}
protected:
DataRefType ref;
}; // class MotionTpl
} // namespace se3
#endif // ifndef __se3_force_ref_hpp__
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// 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_force_tpl_hpp__
#define __se3_force_tpl_hpp__
namespace se3
{
template<typename _Scalar, int _Options>
struct traits< ForceTpl<_Scalar, _Options> >
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef ForceTpl<Scalar,_Options> ForcePlain;
enum {
LINEAR = 0,
ANGULAR = 3,
Options = _Options
};
}; // traits ForceTpl
template<typename _Scalar, int _Options>
class ForceTpl : public ForceDense< ForceTpl<_Scalar, _Options> >
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef ForceDense<ForceTpl> Base;
FORCE_TYPEDEF_TPL(ForceTpl);
using Base::operator=;
using Base::linear;
using Base::angular;
// Constructors
ForceTpl() : data() {}
template<typename V1,typename V2>
ForceTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
{
assert(v.size() == 3);
assert(w.size() == 3);
data << v, w;
}
template<typename V6>
explicit ForceTpl(const Eigen::MatrixBase<V6> & v)
: data(v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
assert(v.size() == 6);
}
template<typename S2,int O2>
explicit ForceTpl(const ForceTpl<S2,O2> & clone)
: data(clone.toVector())
{}
template<typename M2>
explicit ForceTpl(const ForceDense<M2> & clone)
{ linear() = clone.linear(); angular() = clone.angular(); }
// initializers
static ForceTpl Zero() { return ForceTpl(Vector6::Zero()); }
static ForceTpl Random() { return ForceTpl(Vector6::Random()); }
ToVectorConstReturnType toVector_impl() const { return data; }
ToVectorReturnType toVector_impl() { return data; }
// Getters
ConstAngularType angular_impl() const { return data.template segment<3> (ANGULAR); }
ConstLinearType linear_impl() const { return data.template segment<3> (LINEAR); }
AngularType angular_impl() { return data.template segment<3> (ANGULAR); }
LinearType linear_impl() { return data.template segment<3> (LINEAR); }