force-set.hpp

//
// Copyright (c) 2015 CNRS
//

#ifndef __pinocchio_force_set_hpp__
#define __pinocchio_force_set_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include <Eigen/Geometry>

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  class ForceSetTpl
  {
  public:
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef SE3Tpl<Scalar,Options> SE3;

    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;

  public:
    // Constructors
    ForceSetTpl(const int & ncols ) : size(ncols),m_f(3,ncols), m_n(3,ncols) 
    { m_f.fill(NAN); m_n.fill(NAN); }
    ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
      : size((int)linear.cols()),m_f(linear), m_n(angular)
    {  assert( linear.cols() == angular.cols() ); }

    Matrix6x matrix() const
    {
      Matrix6x F(6,size); F << m_f, m_n;
      // F.template topRows<3>() = m_f;
      // F.template bottomRows<3>()  = m_n;
      return F;
    }
    operator Matrix6x () const { return matrix(); }

    // Getters
    const Matrix3x & linear() const { return m_f; }
    const Matrix3x & angular() const { return m_n; }

    ForceSetTpl se3Action(const SE3 & m) const
    {
      Matrix3x Rf (m.rotation()*linear());
      return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
      // TODO check if nothing better than explicitely calling skew
    }
    ForceSetTpl se3ActionInverse(const SE3 & m) const
    {
      return ForceSetTpl(m.rotation().transpose()*linear(),
              m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
      // TODO check if nothing better than explicitely calling skew
    }

    friend std::ostream & operator << (std::ostream & os, const ForceSetTpl & phi)
    {
      os
    << "F =\n" << phi.linear() << std::endl
    << "Tau =\n" << phi.angular() << std::endl;
      return os;
    }

    /* --- BLOCK ------------------------------------------------------------ */
    struct Block
    {
      ForceSetTpl & ref;
      int idx,len;
      Block( ForceSetTpl & ref, const int & idx, const int & len )
    : ref(ref), idx(idx), len(len) {}
      
      Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
      Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> linear()  const 
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
      Eigen::Block<const ForceSetTpl::Matrix3x> angular() const 
      { return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }

      ForceSetTpl::Matrix6x matrix() const
      {
    ForceSetTpl::Matrix6x res(6,len); res << linear(),angular(); 
    return res;
      }

      Block& operator= (const ForceSetTpl & copy)
      {
    assert(copy.size == len);
    linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
    angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
    return *this;
      }

      Block& operator= (const ForceSetTpl::Block & copy)
      {
    assert(copy.len == len);
    linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
    angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
    return *this;
      }

      template <typename D>
      Block& operator= (const Eigen::MatrixBase<D> & m)
      {
        eigen_assert(D::RowsAtCompileTime == 6);
        assert(m.cols() == len);
        linear() = m.template topRows<3>();
        angular() = m.template bottomRows<3>();
        return *this;
      }

      ForceSetTpl se3Action(const SE3 & m) const
      {
    // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
    // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
    Matrix3x Rf ((m.rotation()*linear()));
    return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
    // TODO check if nothing better than explicitely calling skew
      }
      ForceSetTpl se3ActionInverse(const SE3 & m) const
      {
    // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
    // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
    return ForceSetTpl(m.rotation().transpose()*linear(),
               m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
    // TODO check if nothing better than explicitely calling skew
      }

    };

    Block block(const int & idx, const int & len) { return Block(*this,idx,len); }

    /* CRBA joint operators
     *   - ForceSet::Block = ForceSet
     *   - ForceSet operator* (Inertia Y,Constraint S)
     *   - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
     *   - SE3::act(ForceSet::Block)
     */


  public: //
  private:
    int size;
    Matrix3x m_f,m_n;
  };

  typedef ForceSetTpl<double,0> ForceSet;

  template<>
  struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };


} // namespace pinocchio

#endif // ifndef __pinocchio_force_set_hpp__