force-set.hpp

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

#ifndef __pinocchio_spatial_force_set_hpp__
#define __pinocchio_spatial_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<context::Scalar, context::Options> ForceSet;

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

} // namespace pinocchio

#endif // ifndef __pinocchio_spatial_force_set_hpp__