pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
force-set.hpp
1 //
2 // Copyright (c) 2015 CNRS
3 //
4 
5 #ifndef __pinocchio_force_set_hpp__
6 #define __pinocchio_force_set_hpp__
7 
8 #include "pinocchio/spatial/fwd.hpp"
9 #include <Eigen/Geometry>
10 
11 namespace pinocchio
12 {
13  template<typename _Scalar, int _Options>
15  {
16  public:
17  typedef _Scalar Scalar;
18  enum { Options = _Options };
19  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
20  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
21  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
22  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
24 
25  typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
26  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
27 
28  public:
29  // Constructors
30  ForceSetTpl(const int & ncols ) : size(ncols),m_f(3,ncols), m_n(3,ncols)
31  { m_f.fill(NAN); m_n.fill(NAN); }
32  ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
33  : size((int)linear.cols()),m_f(linear), m_n(angular)
34  { assert( linear.cols() == angular.cols() ); }
35 
36  Matrix6x matrix() const
37  {
38  Matrix6x F(6,size); F << m_f, m_n;
39  // F.template topRows<3>() = m_f;
40  // F.template bottomRows<3>() = m_n;
41  return F;
42  }
43  operator Matrix6x () const { return matrix(); }
44 
45  // Getters
46  const Matrix3x & linear() const { return m_f; }
47  const Matrix3x & angular() const { return m_n; }
48 
50  ForceSetTpl se3Action(const SE3 & m) const
51  {
52  Matrix3x Rf (m.rotation()*linear());
53  return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
54  // TODO check if nothing better than explicitely calling skew
55  }
57  ForceSetTpl se3ActionInverse(const SE3 & m) const
58  {
59  return ForceSetTpl(m.rotation().transpose()*linear(),
60  m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
61  // TODO check if nothing better than explicitely calling skew
62  }
63 
64  friend std::ostream & operator << (std::ostream & os, const ForceSetTpl & phi)
65  {
66  os
67  << "F =\n" << phi.linear() << std::endl
68  << "Tau =\n" << phi.angular() << std::endl;
69  return os;
70  }
71 
72  /* --- BLOCK ------------------------------------------------------------ */
73  struct Block
74  {
75  ForceSetTpl & ref;
76  int idx,len;
77  Block( ForceSetTpl & ref, const int & idx, const int & len )
78  : ref(ref), idx(idx), len(len) {}
79 
80  Eigen::Block<ForceSetTpl::Matrix3x> linear() { return ref.m_f.block(0,idx,3,len); }
81  Eigen::Block<ForceSetTpl::Matrix3x> angular() { return ref.m_n.block(0,idx,3,len); }
82  Eigen::Block<const ForceSetTpl::Matrix3x> linear() const
83  { return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0,idx,3,len); }
84  Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
85  { return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0,idx,3,len); }
86 
87  ForceSetTpl::Matrix6x matrix() const
88  {
89  ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
90  return res;
91  }
92 
93  Block& operator= (const ForceSetTpl & copy)
94  {
95  assert(copy.size == len);
96  linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.m_f;
97  angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.m_n;
98  return *this;
99  }
100 
101  Block& operator= (const ForceSetTpl::Block & copy)
102  {
103  assert(copy.len == len);
104  linear() = copy.linear(); //ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
105  angular() = copy.angular(); //ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
106  return *this;
107  }
108 
109  template <typename D>
110  Block& operator= (const Eigen::MatrixBase<D> & m)
111  {
112  eigen_assert(D::RowsAtCompileTime == 6);
113  assert(m.cols() == len);
114  linear() = m.template topRows<3>();
115  angular() = m.template bottomRows<3>();
116  return *this;
117  }
118 
120  ForceSetTpl se3Action(const SE3 & m) const
121  {
122  // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
123  // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
124  Matrix3x Rf ((m.rotation()*linear()));
125  return ForceSetTpl(Rf,skew(m.translation())*Rf+m.rotation()*angular());
126  // TODO check if nothing better than explicitely calling skew
127  }
129  ForceSetTpl se3ActionInverse(const SE3 & m) const
130  {
131  // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
132  // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
133  return ForceSetTpl(m.rotation().transpose()*linear(),
134  m.rotation().transpose()*(angular() - skew(m.translation())*linear()) );
135  // TODO check if nothing better than explicitely calling skew
136  }
137 
138  };
139 
140  Block block(const int & idx, const int & len) { return Block(*this,idx,len); }
141 
142  /* CRBA joint operators
143  * - ForceSet::Block = ForceSet
144  * - ForceSet operator* (Inertia Y,Constraint S)
145  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
146  * - SE3::act(ForceSet::Block)
147  */
148 
149 
150  public: //
151  private:
152  int size;
153  Matrix3x m_f,m_n;
154  };
155 
157 
158  template<>
159  struct SE3GroupAction<ForceSet::Block> { typedef ForceSet ReturnType; };
160 
161 
162 } // namespace pinocchio
163 
164 #endif // ifndef __pinocchio_force_set_hpp__
165 
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
Definition: force-set.hpp:129
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, int LEVEL)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition: copy.hpp:42
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Definition: force-set.hpp:120
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Definition: force-set.hpp:50
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
Definition: force-set.hpp:57
Main pinocchio namespace.
Definition: treeview.dox:24
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21