force-set.hpp
Go to the documentation of this file.
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  {
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 
88  {
89  ForceSetTpl::Matrix6x res(6,len); res << linear(),angular();
90  return res;
91  }
92 
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 
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 
Eigen::Block< const ForceSetTpl::Matrix3x > angular() const
Definition: force-set.hpp:84
Eigen::Block< const ForceSetTpl::Matrix3x > linear() const
Definition: force-set.hpp:82
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Definition: force-set.hpp:120
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Definition: force-set.hpp:22
ForceSetTpl::Matrix6x matrix() const
Definition: force-set.hpp:87
ForceSetTpl se3Action(const SE3 &m) const
af = aXb.act(bf)
Definition: force-set.hpp:50
Block block(const int &idx, const int &len)
Definition: force-set.hpp:140
ForceSetTpl< double, 0 > ForceSet
Definition: force-set.hpp:156
Block & operator=(const ForceSetTpl &copy)
Definition: force-set.hpp:93
Eigen::Block< ForceSetTpl::Matrix3x > linear()
Definition: force-set.hpp:80
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
Definition: force-set.hpp:26
const Matrix3x & angular() const
Definition: force-set.hpp:47
ForceSetTpl(const int &ncols)
Definition: force-set.hpp:30
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
Definition: force-set.hpp:25
Eigen::Block< ForceSetTpl::Matrix3x > angular()
Definition: force-set.hpp:81
Matrix6x matrix() const
Definition: force-set.hpp:36
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
Main pinocchio namespace.
Definition: timings.cpp:28
const Matrix3x & linear() const
Definition: force-set.hpp:46
res
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: force-set.hpp:19
int cols
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
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition: copy.hpp:52
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
Definition: force-set.hpp:129
ForceSetTpl se3ActionInverse(const SE3 &m) const
bf = aXb.actInv(af)
Definition: force-set.hpp:57
SE3Tpl< Scalar, Options > SE3
Definition: force-set.hpp:23
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
Definition: force-set.hpp:21
Block(ForceSetTpl &ref, const int &idx, const int &len)
Definition: force-set.hpp:77
Eigen::Matrix< Scalar, 3, 3, Options > Matrix3
Definition: force-set.hpp:20
friend std::ostream & operator<<(std::ostream &os, const ForceSetTpl &phi)
Definition: force-set.hpp:64
ConstLinearRef translation() const
Definition: se3-base.hpp:38
ForceSetTpl(const Matrix3x &linear, const Matrix3x &angular)
Definition: force-set.hpp:32


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30