Wrench.h

Go to the documentation of this file.

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_LGSM_LIE_WRENCH_H
#define EIGEN_LGSM_LIE_WRENCH_H

/*******************************************************************************
* Definition/implementation of WrenchBase
********************************************************************************/

namespace internal {
  template<class Derived>
    struct traits<WrenchBase<Derived> > : traits<LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> > {
    };
}

template<class Derived> 
class WrenchBase : public LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived>{
protected:
  typedef LieAlgebraDualBase<Matrix<typename internal::traits<Derived>::Scalar, 6, 1>, Derived> Base;
public:
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(WrenchBase)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(WrenchBase)

  typedef typename Base::BaseType BaseType;
  typedef typename internal::traits<Derived>::Coefficients Coefficients;

  typedef Matrix<Scalar, 3, 1> Force;
  typedef LieAlgebraDual<Matrix<Scalar, 3, 1> > Torque;

  inline Scalar tx() const { return this->getTorque().x(); }
  inline Scalar ty() const { return this->getTorque().y(); }
  inline Scalar tz() const { return this->getTorque().z(); }
  inline Scalar fx() const { return this->getForce().x(); }
  inline Scalar fy() const { return this->getForce().y(); }
  inline Scalar fz() const { return this->getForce().z(); }

  inline Scalar& tx() { return this->getTorque().x(); }
  inline Scalar& ty() { return this->getTorque().y(); }
  inline Scalar& tz() { return this->getTorque().z(); }
  inline Scalar& fx() { return this->getForce().x(); }
  inline Scalar& fy() { return this->getForce().y(); }
  inline Scalar& fz() { return this->getForce().z(); }

  
  inline Map<Torque> getTorque(){ return Map<Torque>(this->data()); }
  inline Map<const Torque> getTorque() const {return  Map<const Torque>(this->data()); }
  inline Map<Force> getForce() { return Map<Force>(this->derived().get().template tail<3>().data()); }
  inline Map<const Force> getForce() const { return Map<const Force>(this->derived().get().template tail<3>().data()); }

  template<class RotationDerived> inline Wrench<Scalar> changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>&) const;

  template<class OtherDerived> inline Wrench<Scalar> changePoint(const MatrixBase<OtherDerived>& point) const;
};


template<class Derived>
template<class RotationDerived> 
inline Wrench<typename internal::traits<WrenchBase<Derived> >::Scalar> WrenchBase<Derived>::changeFrame(const LieGroupBase<Quaternion<Scalar>, RotationDerived>& rot) const {
  return Wrench<Scalar>(rot*this->getTorque(),
                        rot*this->getForce());
}


template<class Derived>
template<class OtherDerived> 
inline Wrench<typename internal::traits<WrenchBase<Derived> >::Scalar> WrenchBase<Derived>::changePoint(const MatrixBase<OtherDerived>& point) const {
  return Wrench<Scalar>(this->getTorque() + this->getForce.cross(point),
                        this->getForce() );
}

/**************************
 * Implementation of Wrench
 **************************/

namespace internal {
  template<typename _Scalar>
    struct traits<Wrench<_Scalar> > : traits<LieAlgebraDual<Matrix<_Scalar, 6, 1> > >
    {
      typedef _Scalar Scalar;
    };
}

template<typename _Scalar>
class Wrench : public WrenchBase<Wrench<_Scalar> >{
protected:
  typedef WrenchBase<Wrench<_Scalar> > Base;
public:
  // inherit MatrixBase interface
  EIGEN_DENSE_PUBLIC_INTERFACE(Wrench)
  // inherit assignement operator
  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Wrench)

  typedef typename Base::BaseType BaseType;
  typedef typename internal::traits<Wrench>::Coefficients Coefficients;


  inline Wrench() : m_coeffs() {}
  inline Wrench(const Wrench& g) : m_coeffs(g.get() ) {}
  inline Wrench(const BaseType& g) : m_coeffs(g) {}
  inline Wrench(const typename Base::PlainObject& g) : m_coeffs(g.get() ) {}
  inline Wrench(Scalar tx, Scalar ty, Scalar tz, Scalar fx, Scalar fy, Scalar fz) {
    m_coeffs << tx, ty, tz, fx, fy, fz;
  }
  template<typename OtherDerived>
  EIGEN_STRONG_INLINE Wrench(const MatrixBase<OtherDerived>& other)
    : m_coeffs(other)
  {
  }
  template<typename OtherDerived>
  EIGEN_STRONG_INLINE Wrench& operator=(const MatrixBase<OtherDerived>& other) 
  {
    m_coeffs = other;
    return *this;
  }
  inline Wrench(const typename Base::Torque& t, const typename Base::Force& f) {
    this->getForce() = f;
    this->getTorque() = t;
  }
  inline Coefficients& get() { return m_coeffs; }
  inline const Coefficients& get() const { return m_coeffs; }

protected:
  Coefficients m_coeffs;
};

typedef Wrench<double> Wrenchd;
typedef Wrench<float> Wrenchf;

/**************************************
 * Implementation of Map<Wrench>
 **************************************/

namespace internal {
  template<typename _Scalar, int MapOptions, typename StrideType>
    struct traits<Map<Wrench<_Scalar>, MapOptions, StrideType> > : traits<Map<LieAlgebraDual<Matrix<_Scalar, 6, 1> >, MapOptions, StrideType> >
    {
      typedef _Scalar Scalar;
    };
}
template<typename _Scalar, int MapOptions, typename StrideType>
class Map<Wrench<_Scalar>, MapOptions, StrideType> : public WrenchBase<Map<Wrench<_Scalar>, MapOptions, StrideType> >{
  protected:
    typedef WrenchBase<Map<Wrench<_Scalar> > > Base;
  public:
    EIGEN_DENSE_PUBLIC_INTERFACE(Map)
      EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)

      typedef typename internal::traits<Map>::Coefficients Coefficients;

    inline Map(const Wrench<Scalar>& w) : m_coeffs(w.get()) {};
    template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> 
      inline Map(const Array<Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& g) : m_coeffs(g.data()) {};

    inline Map(Scalar* data) : m_coeffs(data) {};
    inline Map(const Map& m) : m_coeffs(m.get()) {};

    inline Coefficients& get() { return m_coeffs; }
    inline const Coefficients& get() const { return m_coeffs; }

  protected:
    Coefficients m_coeffs;
};

#endif