linear-cone.hpp
Go to the documentation of this file.
1 // Copyright (c) 2015-2018, CNRS
2 // Authors: Justin Carpentier <jcarpent@laas.fr>
3 
4 #ifndef __multicontact_api_geometry_linear_cone_hpp__
5 #define __multicontact_api_geometry_linear_cone_hpp__
6 
7 #include <Eigen/Dense>
8 #include <Eigen/Geometry>
9 #include <pinocchio/spatial/se3.hpp>
10 
14 
15 #define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(TYPE, ROWS) \
16  EIGEN_STATIC_ASSERT(TYPE::RowsAtCompileTime == ROWS, \
17  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
18 
19 namespace multicontact_api {
20 namespace geometry {
21 
22 template <typename _Scalar, int _dim, int _Options>
23 struct LinearCone
24  : public serialization::Serializable<LinearCone<_Scalar, _dim, _Options> > {
25  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26  typedef _Scalar Scalar;
27  enum { dim = _dim, Options = _Options };
28  typedef Eigen::Matrix<Scalar, dim, -1, Options> MatrixDx;
29  typedef Eigen::Matrix<Scalar, dim, dim, Options> MatrixD;
30  typedef Eigen::Matrix<Scalar, dim, 1, Options> VectorD;
31  typedef Eigen::DenseIndex Index;
32 
34  LinearCone() : m_rays() {}
35 
37  template <typename EigenDerived>
38  explicit LinearCone(const Eigen::MatrixBase<EigenDerived>& rays)
39  // : m_rays(_dim,rays.cols())
40  : m_rays(rays) {
41  // EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(EigenDerived,MatrixDx)
42  // for(int k=0; k<rays.cols(); ++k)
43  // m_rays.col(k) = rays.col(k).normalized();
44  }
45 
47  explicit LinearCone(const Index size) : m_rays(_dim, size) {}
48 
50  template <typename S2, int O2>
51  LinearCone(const LinearCone<S2, dim, O2>& other) : m_rays(other.m_rays) {}
52 
53  void addRay(const VectorD& ray) {
54  m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + 1);
55  m_rays.template rightCols<1>() = ray.normalized();
56  }
57 
58  template <typename EigenDerived>
59  void stack(const Eigen::MatrixBase<EigenDerived>& rays) {
61  m_rays.conservativeResize(Eigen::NoChange_t(), m_rays.cols() + rays.cols());
62  m_rays.rightCols(rays.cols()) = rays;
63  }
64 
65  template <typename S2, int O2>
66  void stack(const LinearCone<S2, dim, O2>& other) {
67  stack(other.rays());
68  }
69 
71  const MatrixDx& rays() const { return m_rays; }
72  MatrixDx& rays() { return m_rays; }
73 
75  Index size() const { return m_rays.cols(); }
76 
77  template <typename S2, int O2>
78  bool operator==(const LinearCone<S2, dim, O2>& other) const {
79  return m_rays == other.m_rays;
80  }
81 
82  template <typename S2, int O2>
83  bool operator!=(const LinearCone<S2, dim, O2>& other) const {
84  return !(*this == other);
85  }
86 
87  template <typename S2, int O2>
88  bool isApprox(
89  const LinearCone<S2, dim, O2>& other,
90  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
91  return m_rays.isApprox(other.m_rays, prec);
92  }
93 
94  void disp(std::ostream& os) const { os << "Rays:\n" << m_rays << std::endl; }
95 
96  friend std::ostream& operator<<(std::ostream& os, const LinearCone& C) {
97  C.disp(os);
98  return os;
99  }
100 
101  protected:
104 
105  private:
106  // Serialization of the class
108 
109  template <class Archive>
110  void save(Archive& ar, const unsigned int /*version*/) const {
111  ar& boost::serialization::make_nvp("rays", m_rays);
112  }
113 
114  template <class Archive>
115  void load(Archive& ar, const unsigned int /*version*/) {
116  ar >> boost::serialization::make_nvp("rays", m_rays);
117  }
118 
119  BOOST_SERIALIZATION_SPLIT_MEMBER()
120 };
121 
122 template <typename _Scalar, int _Options>
123 struct ForceConeTpl : public LinearCone<_Scalar, 3, _Options> {
124  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
125 
128  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
129  using typename Base::Scalar;
130  enum { dim = Base::dim };
131  using Base::rays;
132  using Base::size;
133  using typename Base::Index;
134  using typename Base::MatrixD;
135  using typename Base::MatrixDx;
136  using typename Base::VectorD;
137  using Base::operator==;
138  using Base::operator!=;
139  // using Base::isApprox; // Leads to a bug with clang
140 
142  typedef VectorD Vector3;
143  typedef Eigen::AngleAxis<Scalar> AngleAxis;
144 
147 
148  template <typename EigenDerived>
149  explicit ForceConeTpl(const Eigen::MatrixBase<EigenDerived>& rays)
150  : Base(rays) {}
151 
152  explicit ForceConeTpl(const Index size) : Base(size) {}
153 
156  static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction,
157  const int num_rays) {
158  return RegularCone(mu, direction, num_rays, 0.);
159  }
160 
161  static ForceConeTpl RegularCone(const Scalar mu, const VectorD& direction,
162  const int num_rays,
163  const Scalar theta_offset) {
164  assert(mu >= 0. && "mu must be positive");
165  assert(num_rays >= 1 && "The number of rays must be at least one");
166 
167  const VectorD normalized_direction(direction.normalized());
168  ForceConeTpl cone(num_rays);
169 
170  const Scalar angle = (2. * M_PI) / num_rays;
171 
172  const MatrixD Po(MatrixD::Identity() -
173  normalized_direction * normalized_direction.transpose());
174 
175  const MatrixD rot_offset(
176  AngleAxis(theta_offset, normalized_direction).toRotationMatrix());
177  const VectorD init_direction(rot_offset *
178  (Po * VectorD::Ones()).normalized());
179  const MatrixD rot(
180  AngleAxis(angle, normalized_direction).toRotationMatrix());
181 
182  VectorD ray((direction + mu * init_direction).normalized());
183 
184  for (int k = 0; k < num_rays; ++k) {
185  cone.rays().col(k) = ray;
186  if (k != num_rays - 1) ray = rot * ray;
187  }
188 
189  return cone;
190  }
191 
192  WrenchCone SE3ActOn(const SE3& M) const {
193  WrenchCone res(size());
194  typedef typename WrenchCone::MatrixDx::ColXpr Col6Xpr;
195  typedef typename MatrixDx::ConstColXpr ConstCol3Xpr;
196 
197  const typename SE3::Matrix3& R = M.rotation();
198  const typename SE3::Vector3& t = M.translation();
199 
200  for (Index k = 0; k < size(); ++k) {
201  ConstCol3Xpr in_col = rays().col(k);
202  Col6Xpr out_col = res.rays().col(k);
203 
204  out_col.template head<3>() = R * in_col;
205  out_col.template tail<3>() = t.cross(out_col.template head<3>());
206  }
207 
208  return res;
209  }
210 
211  template <typename S2, int O2>
212  bool isApprox(
213  const ForceConeTpl<S2, O2>& other,
214  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
215  return Base::isApprox(other, prec);
216  }
217 
218  operator WrenchCone() const {
219  WrenchCone res(size());
220  res.rays().template topRows<3>() = rays();
221  res.rays().template bottomRows<3>().setZero();
222  return res;
223  }
224 };
225 
226 template <typename _Scalar, int _Options>
227 struct WrenchConeTpl : public LinearCone<_Scalar, 6, _Options> {
228  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
229 
232  typedef pinocchio::SE3Tpl<_Scalar, _Options> SE3;
233  using typename Base::Scalar;
234  enum { dim = Base::dim };
235  using Base::rays;
236  using Base::size;
237  using typename Base::Index;
238  using typename Base::MatrixDx;
239  using typename Base::VectorD;
240  using Base::operator==;
241  using Base::operator!=;
242  // using Base::isApprox; // Leads to a bug with clang
243 
245  typedef VectorD Vector6;
246 
247  typedef typename ForceCone::Matrix3x Matrix3x;
248 
249  typedef typename Matrix6x::template NRowsBlockXpr<3>::Type LinearBlock;
250  typedef
251  typename Matrix6x::template ConstNRowsBlockXpr<3>::Type ConstLinearBlock;
252 
255 
258 
260  template <typename EigenDerived>
261  explicit WrenchConeTpl(const Eigen::MatrixBase<EigenDerived>& rays)
262  : Base(rays) {}
263 
265  explicit WrenchConeTpl(const Index size) : Base(size) {}
266 
268  template <typename S2, int O2>
269  explicit WrenchConeTpl(const ForceConeTpl<S2, O2>& force_cone)
270  : Base(force_cone.size()) {
271  rays().template topRows<3>() = force_cone.rays();
272  rays().template bottomRows<3>().setZero();
273  }
274 
276  template <typename S2, int O2>
277  WrenchConeTpl(const WrenchConeTpl<S2, O2>& other) : Base(other) {}
278 
279  WrenchConeTpl SE3ActOn(const SE3& M) const {
280  WrenchConeTpl res(size());
281  typedef typename MatrixDx::ColXpr Col6Xpr;
282  typedef typename MatrixDx::ConstColXpr ConstCol6Xpr;
283 
284  const typename SE3::Matrix3& R = M.rotation();
285  const typename SE3::Vector3& t = M.translation();
286 
287  for (Index k = 0; k < size(); ++k) {
288  ConstCol6Xpr in_col = rays().col(k);
289  Col6Xpr out_col = res.rays().col(k);
290 
291  out_col.template head<3>() = R * in_col.template head<3>();
292  out_col.template tail<3>() =
293  t.cross(out_col.template head<3>()) + R * in_col.template tail<3>();
294  }
295 
296  return res;
297  }
298 
299  template <typename S2, int O2>
300  bool isApprox(
301  const WrenchConeTpl<S2, O2>& other,
302  const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) const {
303  return Base::isApprox(other, prec);
304  }
305 
306  ConstLinearBlock linear() const { return rays().template topRows<3>(); }
307  LinearBlock linear() { return rays().template topRows<3>(); }
308 
309  ConstAngularBlock angular() const { return rays().template bottomRows<3>(); }
310  AngularBlock angular() { return rays().template bottomRows<3>(); }
311 
312  ForceCone toForceCone() const { return ForceCone(linear()); }
313 };
314 } // namespace geometry
315 } // namespace multicontact_api
316 
317 #endif // ifndef __multicontact_api_geometry_linear_cone_hpp__
#define EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_ROWS_SIZE(TYPE, ROWS)
Definition: linear-cone.hpp:15
void load(Archive &ar, pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:24
void save(Archive &ar, const pinocchio::container::aligned_vector< T > &v, const unsigned int version)
Definition: aligned-vector.hpp:16
Definition: ellipsoid.hpp:12
Definition: linear-cone.hpp:123
pinocchio::SE3Tpl< _Scalar, _Options > SE3
Definition: linear-cone.hpp:128
@ dim
Definition: linear-cone.hpp:130
Eigen::AngleAxis< Scalar > AngleAxis
Definition: linear-cone.hpp:143
static ForceConeTpl RegularCone(const Scalar mu, const VectorD &direction, const int num_rays, const Scalar theta_offset)
Definition: linear-cone.hpp:161
ForceConeTpl(const Index size)
Definition: linear-cone.hpp:152
ForceConeTpl()
Default constructor.
Definition: linear-cone.hpp:146
ForceConeTpl(const Eigen::MatrixBase< EigenDerived > &rays)
Definition: linear-cone.hpp:149
WrenchCone SE3ActOn(const SE3 &M) const
Definition: linear-cone.hpp:192
bool isApprox(const ForceConeTpl< S2, O2 > &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: linear-cone.hpp:212
static ForceConeTpl RegularCone(const Scalar mu, const VectorD &direction, const int num_rays)
Definition: linear-cone.hpp:156
WrenchConeTpl< _Scalar, _Options > WrenchCone
Definition: linear-cone.hpp:127
MatrixDx Matrix3x
Definition: linear-cone.hpp:141
VectorD Vector3
Definition: linear-cone.hpp:142
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef LinearCone< _Scalar, 3, _Options > Base
Definition: linear-cone.hpp:126
Definition: linear-cone.hpp:24
void addRay(const VectorD &ray)
Definition: linear-cone.hpp:53
LinearCone()
Default constructor.
Definition: linear-cone.hpp:34
Index size() const
Definition: linear-cone.hpp:75
void disp(std::ostream &os) const
Definition: linear-cone.hpp:94
Eigen::Matrix< Scalar, dim, dim, Options > MatrixD
Definition: linear-cone.hpp:29
bool isApprox(const LinearCone< S2, dim, O2 > &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: linear-cone.hpp:88
LinearCone(const LinearCone< S2, dim, O2 > &other)
Copy constructor.
Definition: linear-cone.hpp:51
Eigen::DenseIndex Index
Definition: linear-cone.hpp:31
friend std::ostream & operator<<(std::ostream &os, const LinearCone &C)
Definition: linear-cone.hpp:96
Eigen::Matrix< Scalar, dim, -1, Options > MatrixDx
Definition: linear-cone.hpp:28
Eigen::Matrix< Scalar, dim, 1, Options > VectorD
Definition: linear-cone.hpp:30
bool operator==(const LinearCone< S2, dim, O2 > &other) const
Definition: linear-cone.hpp:78
MatrixDx & rays()
Definition: linear-cone.hpp:72
LinearCone(const Eigen::MatrixBase< EigenDerived > &rays)
Constructor from a set of rays.
Definition: linear-cone.hpp:38
@ Options
Definition: linear-cone.hpp:27
@ dim
Definition: linear-cone.hpp:27
LinearCone(const Index size)
Contrustor from a given size.
Definition: linear-cone.hpp:47
MatrixDx m_rays
Rays of the linear cone.
Definition: linear-cone.hpp:103
void stack(const Eigen::MatrixBase< EigenDerived > &rays)
Definition: linear-cone.hpp:59
bool operator!=(const LinearCone< S2, dim, O2 > &other) const
Definition: linear-cone.hpp:83
void stack(const LinearCone< S2, dim, O2 > &other)
Definition: linear-cone.hpp:66
friend class boost::serialization::access
Definition: linear-cone.hpp:107
const MatrixDx & rays() const
Definition: linear-cone.hpp:71
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Definition: linear-cone.hpp:26
Definition: linear-cone.hpp:227
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef LinearCone< _Scalar, 6, _Options > Base
Definition: linear-cone.hpp:230
VectorD Vector6
Definition: linear-cone.hpp:245
LinearBlock AngularBlock
Definition: linear-cone.hpp:253
pinocchio::SE3Tpl< _Scalar, _Options > SE3
Definition: linear-cone.hpp:232
ForceCone::Matrix3x Matrix3x
Definition: linear-cone.hpp:247
WrenchConeTpl SE3ActOn(const SE3 &M) const
Definition: linear-cone.hpp:279
ConstLinearBlock ConstAngularBlock
Definition: linear-cone.hpp:254
ForceCone toForceCone() const
Definition: linear-cone.hpp:312
WrenchConeTpl(const WrenchConeTpl< S2, O2 > &other)
Copy constructor.
Definition: linear-cone.hpp:277
@ dim
Definition: linear-cone.hpp:234
Matrix6x::template ConstNRowsBlockXpr< 3 >::Type ConstLinearBlock
Definition: linear-cone.hpp:251
WrenchConeTpl()
Default constructor.
Definition: linear-cone.hpp:257
WrenchConeTpl(const Eigen::MatrixBase< EigenDerived > &rays)
Constructor from a set of rays.
Definition: linear-cone.hpp:261
AngularBlock angular()
Definition: linear-cone.hpp:310
WrenchConeTpl(const Index size)
Constructs a WrenchCone of a given size.
Definition: linear-cone.hpp:265
ConstLinearBlock linear() const
Definition: linear-cone.hpp:306
MatrixDx Matrix6x
Definition: linear-cone.hpp:244
ConstAngularBlock angular() const
Definition: linear-cone.hpp:309
Matrix6x::template NRowsBlockXpr< 3 >::Type LinearBlock
Definition: linear-cone.hpp:249
ForceConeTpl< _Scalar, _Options > ForceCone
Definition: linear-cone.hpp:231
bool isApprox(const WrenchConeTpl< S2, O2 > &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: linear-cone.hpp:300
WrenchConeTpl(const ForceConeTpl< S2, O2 > &force_cone)
Constructs a WrenchCone of a given size.
Definition: linear-cone.hpp:269
LinearBlock linear()
Definition: linear-cone.hpp:307