Wrench.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_LGSM_LIE_WRENCH_H
00011 #define EIGEN_LGSM_LIE_WRENCH_H
00012 
00013 /*******************************************************************************
00014 * Definition/implementation of WrenchBase
00015 ********************************************************************************/
00016 
00028 namespace internal {
00029   template<class Derived>
00030     struct traits<WrenchBase<Derived> > : traits<LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> > {
00031     };
00032 }
00033 
00034 template<class Derived> 
00035 class WrenchBase : public LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>{
00036 protected:
00038   typedef LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> Base;
00039 public:
00040   // inherit MatrixBase interface
00041   EIGEN_DENSE_PUBLIC_INTERFACE(WrenchBase)
00042   // inherit assignement operator
00043   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(WrenchBase)
00044 
00046   typedef typename Base::BaseType BaseType;
00048   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00049 
00051   typedef Matrix<Scalar, 3, 1> Force;
00053   typedef LieAlgebraDual<Matrix<Scalar, 3, 1> > Torque;
00054 
00056   inline Scalar tx() const { return this->getTorque().x(); }
00058   inline Scalar ty() const { return this->getTorque().y(); }
00060   inline Scalar tz() const { return this->getTorque().z(); }
00062   inline Scalar fx() const { return this->getForce().x(); }
00064   inline Scalar fy() const { return this->getForce().y(); }
00066   inline Scalar fz() const { return this->getForce().z(); }
00067 
00069   inline Scalar& tx() { return this->getTorque().x(); }
00071   inline Scalar& ty() { return this->getTorque().y(); }
00073   inline Scalar& tz() { return this->getTorque().z(); }
00075   inline Scalar& fx() { return this->getForce().x(); }
00077   inline Scalar& fy() { return this->getForce().y(); }
00079   inline Scalar& fz() { return this->getForce().z(); }
00080 
00081   
00083   inline Map<Torque> getTorque(){ return Map<Torque>(this->data()); }
00085   inline Map<const Torque> getTorque() const {return  Map<const Torque>(this->data()); }
00087   inline Map<Force> getForce() { return Map<Force>(this->derived().get().template tail<3>().data()); }
00089   inline Map<const Force> getForce() const { return Map<const Force>(this->derived().get().template tail<3>().data()); }
00090 
00091   template<class RotationDerived> inline Wrench<Scalar> changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>&) const;
00092 
00093   template<class OtherDerived> inline Wrench<Scalar> changePoint(const MatrixBase<OtherDerived>& point) const;
00094 };
00095 
00096 
00097 template<class Derived>
00098 template<class RotationDerived> 
00099 inline Wrench<typename internal::traits<WrenchBase<Derived> >::Scalar> WrenchBase<Derived>::changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>& rot) const {
00100   return Wrench<Scalar>(rot*this->getTorque(),
00101                         rot*this->getForce());
00102 }
00103 
00104 
00105 template<class Derived>
00106 template<class OtherDerived> 
00107 inline Wrench<typename internal::traits<WrenchBase<Derived> >::Scalar> WrenchBase<Derived>::changePoint(const MatrixBase<OtherDerived>& point) const {
00108   return Wrench<Scalar>(this->getTorque() + this->getForce.cross(point),
00109                         this->getForce() );
00110 }
00111 
00112 /**************************
00113  * Implementation of Wrench
00114  **************************/
00115 
00116 namespace internal {
00117   template<typename _Scalar>
00118     struct traits<Wrench<_Scalar> > : traits<LieAlgebraDual<Matrix<_Scalar, 6, 1> > >
00119     {
00120       typedef _Scalar Scalar;
00121     };
00122 }
00123 
00134 template<typename _Scalar>
00135 class Wrench : public WrenchBase<Wrench<_Scalar> >{
00136 protected:
00137   typedef WrenchBase<Wrench<_Scalar> > Base;
00138 public:
00139   // inherit MatrixBase interface
00140   EIGEN_DENSE_PUBLIC_INTERFACE(Wrench)
00141   // inherit assignement operator
00142   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Wrench)
00143 
00145   typedef typename Base::BaseType BaseType;
00147   typedef typename internal::traits<Wrench>::Coefficients Coefficients;
00148 
00149 
00151   inline Wrench() : m_coeffs() {}
00153   inline Wrench(const Wrench& g) : m_coeffs(g.get() ) {}
00155   inline Wrench(const BaseType& g) : m_coeffs(g) {}
00157   inline Wrench(const typename Base::PlainObject& g) : m_coeffs(g.get() ) {}
00159   inline Wrench(Scalar tx, Scalar ty, Scalar tz, Scalar fx, Scalar fy, Scalar fz) {
00160     m_coeffs << tx, ty, tz, fx, fy, fz;
00161   }
00163   template<typename OtherDerived>
00164   EIGEN_STRONG_INLINE Wrench(const MatrixBase<OtherDerived>& other)
00165     : m_coeffs(other)
00166   {
00167   }
00169   template<typename OtherDerived>
00170   EIGEN_STRONG_INLINE Wrench& operator=(const MatrixBase<OtherDerived>& other) 
00171   {
00172     m_coeffs = other;
00173     return *this;
00174   }
00176   inline Wrench(const typename Base::Torque& t, const typename Base::Force& f) {
00177     this->getForce() = f;
00178     this->getTorque() = t;
00179   }
00181   inline Coefficients& get() { return m_coeffs; }
00183   inline const Coefficients& get() const { return m_coeffs; }
00184 
00185 protected:
00187   Coefficients m_coeffs;
00188 };
00189 
00191 typedef Wrench<double> Wrenchd;
00193 typedef Wrench<float> Wrenchf;
00194 
00195 /**************************************
00196  * Implementation of Map<Wrench>
00197  **************************************/
00198 
00199 namespace internal {
00200   template<typename _Scalar, int MapOptions, typename StrideType>
00201     struct traits<Map<Wrench<_Scalar>, MapOptions, StrideType> > : traits<Map<LieAlgebraDual<Matrix<_Scalar, 6, 1> >, MapOptions, StrideType> >
00202     {
00203       typedef _Scalar Scalar;
00204     };
00205 }
00216 template<typename _Scalar, int MapOptions, typename StrideType>
00217 class Map<Wrench<_Scalar>, MapOptions, StrideType> : public WrenchBase<Map<Wrench<_Scalar>, MapOptions, StrideType> >{
00218   protected:
00219     typedef WrenchBase<Map<Wrench<_Scalar> > > Base;
00220   public:
00221     EIGEN_DENSE_PUBLIC_INTERFACE(Map)
00222       EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00223 
00224       typedef typename internal::traits<Map>::Coefficients Coefficients;
00225 
00226     inline Map(const Wrench<Scalar>& w) : m_coeffs(w.get()) {};
00227     template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> 
00228       inline Map(const Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};
00229 
00230     inline Map(Scalar* data) : m_coeffs(data) {};
00231     inline Map(const Map& m) : m_coeffs(m.get()) {};
00232 
00233     inline Coefficients& get() { return m_coeffs; }
00234     inline const Coefficients& get() const { return m_coeffs; }
00235 
00236   protected:
00237     Coefficients m_coeffs;
00238 };
00239 
00240 #endif
00241 


lgsm
Author(s): Roberto Martín-Martín
autogenerated on Sat Jun 8 2019 18:26:30