DART  6.7.3
Geometry.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_MATH_GEOMETRY_HPP_
34 #define DART_MATH_GEOMETRY_HPP_
35 
36 #include <Eigen/Dense>
37 
39 #include "dart/math/Constants.hpp"
40 #include "dart/math/MathTypes.hpp"
41 
42 namespace dart {
43 namespace math {
44 
46 Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v);
47 
49 Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m);
50 
51 //------------------------------------------------------------------------------
53 Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v);
54 
56 Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q);
57 
59 Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
60  const Eigen::Vector3d& _pt);
61 
63 Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
64  double _x, double _y, double _z);
65 
67 Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el);
68 
70 Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond& _q,
71  int _el1, int _el2);
72 
73 //------------------------------------------------------------------------------
76 Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle);
77 
80 Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle);
81 
84 Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle);
85 
88 Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle);
89 
92 Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle);
93 
96 Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle);
97 
100 Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle);
101 
104 Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle);
105 
108 Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle);
109 
113 Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle);
114 
117 Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle);
118 
122 Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle);
123 
124 //------------------------------------------------------------------------------
126 Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R);
127 
129 Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R);
130 
132 // Eigen::Vector3d matrixToEulerXZX(const Eigen::Matrix3d& R);
133 
135 Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R);
136 
138 // Eigen::Vector3d matrixToEulerYXY(const Eigen::Matrix3d& R);
139 
141 Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R);
142 
144 Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R);
145 
147 // Eigen::Vector3d matrixToEulerYZY(const Eigen::Matrix3d& R);
148 
150 Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R);
151 
153 Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d& _R);
154 
156 // Eigen::Vector3d matrixToEulerZXZ(const Eigen::Matrix3d& R);
157 
159 // Eigen::Vector3d matrixToEulerZYZ(const Eigen::Matrix3d& R);
160 
161 //------------------------------------------------------------------------------
163 Eigen::Isometry3d expMap(const Eigen::Vector6d& _S);
164 
169 Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s);
170 
172 Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _expmap);
173 
175 Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _expmap);
176 
178 Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d& _expmap,
179  const Eigen::Vector3d& _qdot);
180 
183 Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _expmap, int _qi);
184 
188 Eigen::Vector3d logMap(const Eigen::Matrix3d& _R);
189 
191 Eigen::Vector6d logMap(const Eigen::Isometry3d& _T);
192 
193 //------------------------------------------------------------------------------
200 // SE3 Normalize(const SE3& T);
201 
203 // Axis Reparameterize(const Axis& s);
204 
205 //------------------------------------------------------------------------------
209 Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
210 
212 Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T);
213 
215 template<typename Derived>
216 typename Derived::PlainObject AdTJac(const Eigen::Isometry3d& _T,
217  const Eigen::MatrixBase<Derived>& _J)
218 {
219  // Check the number of rows is 6 at compile time
220  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
221  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
222 
223  typename Derived::PlainObject ret(_J.rows(), _J.cols());
224 
225  // Compute AdT column by column
226  for (int i = 0; i < _J.cols(); ++i)
227  ret.col(i) = AdT(_T, _J.col(i));
228 
229  return ret;
230 }
231 
233 template<typename Derived>
234 typename Derived::PlainObject AdTJacFixed(const Eigen::Isometry3d& _T,
235  const Eigen::MatrixBase<Derived>& _J)
236 {
237  // Check if _J is fixed size Jacobian
238  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
239 
240  // Check the number of rows is 6 at compile time
241  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
242  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
243 
244  typename Derived::PlainObject ret(_J.rows(), _J.cols());
245 
246  // Compute AdT
247  ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
248  ret.template bottomRows<3>().noalias()
249  = -ret.template topRows<3>().colwise().cross(_T.translation())
250  + _T.linear() * _J.template bottomRows<3>();
251 
252  return ret;
253 }
254 
256 Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
257 
259 Eigen::Vector6d AdTAngular(const Eigen::Isometry3d& _T,
260  const Eigen::Vector3d& _w);
261 
263 Eigen::Vector6d AdTLinear(const Eigen::Isometry3d& _T,
264  const Eigen::Vector3d& _v);
265 
267 // se3 AdP(const Vec3& p, const se3& s);
268 
270 template<typename Derived>
271 typename Derived::PlainObject AdRJac(const Eigen::Isometry3d& _T,
272  const Eigen::MatrixBase<Derived>& _J)
273 {
274  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
275  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
276 
277  typename Derived::PlainObject ret(_J.rows(), _J.cols());
278 
279  ret.template topRows<3>().noalias() =
280  _T.linear() * _J.template topRows<3>();
281 
282  ret.template bottomRows<3>().noalias() =
283  _T.linear() * _J.template bottomRows<3>();
284 
285  return ret;
286 }
287 
288 template<typename Derived>
289 typename Derived::PlainObject AdRInvJac(const Eigen::Isometry3d& _T,
290  const Eigen::MatrixBase<Derived>& _J)
291 {
292  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
293  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
294 
295  typename Derived::PlainObject ret(_J.rows(), _J.cols());
296 
297  ret.template topRows<3>().noalias() =
298  _T.linear().transpose() * _J.template topRows<3>();
299 
300  ret.template bottomRows<3>().noalias() =
301  _T.linear().transpose() * _J.template bottomRows<3>();
302 
303  return ret;
304 }
305 
306 template<typename Derived>
307 typename Derived::PlainObject adJac(const Eigen::Vector6d& _V,
308  const Eigen::MatrixBase<Derived>& _J)
309 {
310  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
311  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
312 
313  typename Derived::PlainObject ret(_J.rows(), _J.cols());
314 
315  ret.template topRows<3>().noalias() =
316  - _J.template topRows<3>().colwise().cross(_V.head<3>());
317 
318  ret.template bottomRows<3>().noalias() =
319  - _J.template bottomRows<3>().colwise().cross(_V.head<3>())
320  - _J.template topRows<3>().colwise().cross(_V.tail<3>());
321 
322  return ret;
323 }
324 
326 Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
327 
329 template<typename Derived>
330 typename Derived::PlainObject AdInvTJac(const Eigen::Isometry3d& _T,
331  const Eigen::MatrixBase<Derived>& _J)
332 {
333  // Check the number of rows is 6 at compile time
334  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
335  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
336 
337  typename Derived::PlainObject ret(_J.rows(), _J.cols());
338 
339  // Compute AdInvT column by column
340  for (int i = 0; i < _J.cols(); ++i)
341  ret.col(i) = AdInvT(_T, _J.col(i));
342 
343  return ret;
344 }
345 
347 template<typename Derived>
348 typename Derived::PlainObject AdInvTJacFixed(
349  const Eigen::Isometry3d& _T,
350  const Eigen::MatrixBase<Derived>& _J)
351 {
352  // Check if _J is fixed size Jacobian
353  EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
354 
355  // Check the number of rows is 6 at compile time
356  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 6,
357  THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
358 
359  typename Derived::PlainObject ret(_J.rows(), _J.cols());
360 
361  // Compute AdInvT
362  ret.template topRows<3>().noalias()
363  = _T.linear().transpose() * _J.template topRows<3>();
364  ret.template bottomRows<3>().noalias()
365  = _T.linear().transpose()
366  * (_J.template bottomRows<3>()
367  + _J.template topRows<3>().colwise().cross(_T.translation()));
368 
369  return ret;
370 }
371 
373 // Eigen::Vector3d AdInvTLinear(const Eigen::Isometry3d& T,
374 // const Eigen::Vector3d& v);
375 
377 // Axis AdInvTAngular(const SE3& T, const Axis& w);
378 
380 // se3 AdInvR(const SE3& T, const se3& V);
381 
383 Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d& _T,
384  const Eigen::Vector3d& _v);
385 
389 Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
390 
392 // dse3 dAdTLinear(const SE3& T, const Vec3& F);
393 
395 Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
396 
398 Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
399 
401 // dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);
402 
406 Eigen::Vector6d ad(const Eigen::Vector6d& _X, const Eigen::Vector6d& _Y);
407 
409 // Vec3 ad_Vec3_se3(const Vec3& v, const se3& S);
410 
412 // Axis ad_Axis_Axis(const Axis& w, const Axis& v);
413 
417 Eigen::Vector6d dad(const Eigen::Vector6d& _s, const Eigen::Vector6d& _t);
418 
420 Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _AI);
421 
424 Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original,
425  const Eigen::Vector3d& _comShift,
426  double _mass);
427 
429 {
430  AXIS_X = 0,
431  AXIS_Y = 1,
432  AXIS_Z = 2
433 };
434 
438 Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis,
439  AxisType axisType = AxisType::AXIS_X);
440 
444 Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis,
445  const Eigen::Vector3d& translation,
446  AxisType axisType = AxisType::AXIS_X);
447 
449 DART_DEPRECATED(6.0)
450 Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin,
451  const Eigen::Vector3d& _axisZ);
452 
455 bool verifyRotation(const Eigen::Matrix3d& _R);
456 
459 bool verifyTransform(const Eigen::Isometry3d& _T);
460 
463 inline double wrapToPi(double angle)
464 {
465  constexpr auto pi = constantsd::pi();
466 
467  return std::fmod(angle+pi, 2*pi) - pi;
468 }
469 
470 template <typename MatrixType, typename ReturnType>
471 void extractNullSpace(const Eigen::JacobiSVD<MatrixType>& _SVD, ReturnType& _NS)
472 {
473  int rank = 0;
474  // TODO(MXG): Replace this with _SVD.rank() once the latest Eigen is released
475  if(_SVD.nonzeroSingularValues() > 0)
476  {
477  double thresh = std::max(_SVD.singularValues().coeff(0)*1e-10,
478  std::numeric_limits<double>::min());
479  int i = _SVD.nonzeroSingularValues()-1;
480  while( i >= 0 && _SVD.singularValues().coeff(i) < thresh )
481  --i;
482  rank = i+1;
483  }
484 
485  int cols = _SVD.matrixV().cols(), rows = _SVD.matrixV().rows();
486  _NS = _SVD.matrixV().block(0, rank, rows, cols-rank);
487 }
488 
489 template <typename MatrixType, typename ReturnType>
490 void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
491 {
492  Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
493  extractNullSpace(svd, _NS);
494 }
495 
496 typedef std::vector<Eigen::Vector3d> SupportGeometry;
497 
499 
505  const SupportGeometry& _geometry,
506  const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
507  const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
508 
513  std::vector<std::size_t>& _originalIndices,
514  const SupportGeometry& _geometry,
515  const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
516  const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
517 
520 
523 SupportPolygon computeConvexHull(std::vector<std::size_t>& _originalIndices,
524  const SupportPolygon& _points);
525 
527 Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull);
528 
532 
536 
537 };
538 
542  Eigen::Vector2d& _intersectionPoint,
543  const Eigen::Vector2d& a1,
544  const Eigen::Vector2d& a2,
545  const Eigen::Vector2d& b1,
546  const Eigen::Vector2d& b2);
547 
549 double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2);
550 
552 bool isInsideSupportPolygon(const Eigen::Vector2d& _p,
553  const SupportPolygon& _support,
554  bool _includeEdge = true);
555 
558 Eigen::Vector2d computeClosestPointOnLineSegment(const Eigen::Vector2d& _p,
559  const Eigen::Vector2d& _s1,
560  const Eigen::Vector2d& _s2);
561 
565  const Eigen::Vector2d& _p,
566  const SupportPolygon& _support);
567 
571  std::size_t& _index1,
572  std::size_t& _index2,
573  const Eigen::Vector2d& _p,
574  const SupportPolygon& _support);
575 
576 
577 // Represents a bounding box with minimum and maximum coordinates.
578 class BoundingBox {
579  public:
580  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
581 
582  BoundingBox();
583  BoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max);
584 
585  inline const Eigen::Vector3d& getMin() const { return mMin; }
586  inline const Eigen::Vector3d& getMax() const { return mMax; }
587 
588  inline void setMin(const Eigen::Vector3d& min) { mMin = min; }
589  inline void setMax(const Eigen::Vector3d& max) { mMax = max; }
590 
591  // \brief Centroid of the bounding box (i.e average of min and max)
592  inline Eigen::Vector3d computeCenter() const { return (mMax + mMin) * 0.5; }
593  // \brief Coordinates of the maximum corner with respect to the centroid.
594  inline Eigen::Vector3d computeHalfExtents() const { return (mMax - mMin) * 0.5; }
595  // \brief Length of each of the sides of the bounding box.
596  inline Eigen::Vector3d computeFullExtents() const { return (mMax - mMin); }
597 
598  protected:
599  // \brief minimum coordinates of the bounding box
600  Eigen::Vector3d mMin;
601  // \brief maximum coordinates of the bounding box
602  Eigen::Vector3d mMax;
603 };
604 
605 } // namespace math
606 } // namespace dart
607 
608 #endif // DART_MATH_GEOMETRY_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
Definition: Geometry.hpp:578
const Eigen::Vector3d & getMax() const
Definition: Geometry.hpp:586
void setMin(const Eigen::Vector3d &min)
Definition: Geometry.hpp:588
Eigen::Vector3d mMax
Definition: Geometry.hpp:602
Eigen::Vector3d computeHalfExtents() const
Definition: Geometry.hpp:594
Eigen::Vector3d computeCenter() const
Definition: Geometry.hpp:592
Eigen::Vector3d computeFullExtents() const
Definition: Geometry.hpp:596
Eigen::Vector3d mMin
Definition: Geometry.hpp:600
void setMax(const Eigen::Vector3d &max)
Definition: Geometry.hpp:589
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoundingBox()
Definition: Geometry.cpp:1976
const Eigen::Vector3d & getMin() const
Definition: Geometry.hpp:585
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
std::vector< _Tp, Eigen::aligned_allocator< _Tp > > aligned_vector
Definition: Memory.hpp:66
Eigen::Vector6d dad(const Eigen::Vector6d &_s, const Eigen::Vector6d &_t)
fast version of ad(se3(Eigen_Vec3(0), v), S)
Definition: Geometry.cpp:1303
IntersectionResult computeIntersection(Eigen::Vector2d &_intersectionPoint, const Eigen::Vector2d &a1, const Eigen::Vector2d &a2, const Eigen::Vector2d &b1, const Eigen::Vector2d &b2)
Compute the intersection between a line segment that goes from a1 -> a2 and a line segment that goes ...
Definition: Geometry.cpp:1761
Derived::PlainObject AdInvTJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for dynamic size Jacobian.
Definition: Geometry.hpp:330
Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d &_origin, const Eigen::Vector3d &_axisZ)
Generate frame given origin and z-axis.
Definition: Geometry.cpp:1492
Derived::PlainObject AdInvTJacFixed(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for fixed size Jacobian.
Definition: Geometry.hpp:348
Eigen::Vector3d quatToExp(const Eigen::Quaterniond &_q)
Definition: Geometry.cpp:62
Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d &T)
Get linear transformation matrix of Adjoint mapping.
Definition: Geometry.cpp:637
Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d &_angle)
Given EulerXYZ angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(ang...
Definition: Geometry.cpp:848
Derived::PlainObject AdRJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Change coordinate Frame of a Jacobian.
Definition: Geometry.hpp:271
Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d &_angle)
Given Euler XYX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotY(an...
Definition: Geometry.cpp:817
bool verifyRotation(const Eigen::Matrix3d &_T)
Check if determinant of _R is equat to 1 and all the elements are not NaN values.
Definition: Geometry.cpp:1407
AxisType
Definition: Geometry.hpp:429
@ AXIS_Y
Definition: Geometry.hpp:431
@ AXIS_X
Definition: Geometry.hpp:430
@ AXIS_Z
Definition: Geometry.hpp:432
Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d &_angle)
Given EulerZXZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1111
SupportPolygon computeSupportPolgyon(const SupportGeometry &_geometry, const Eigen::Vector3d &_axis1, const Eigen::Vector3d &_axis2)
Project the support geometry points onto a plane with the given axes and then compute their convex hu...
Definition: Geometry.cpp:1499
Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d &_R)
get the Euler XYX angle from R
Definition: Geometry.cpp:70
Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d &_R)
get the Euler ZXY angle from R
Definition: Geometry.cpp:211
Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d &_angle)
Given EulerZYZ angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(ang...
Definition: Geometry.cpp:1142
Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d &_q, int _qi)
computes the derivative of the Jacobian of the expmap wrt to _qi indexed dof; _qi \in {0,...
Definition: Geometry.cpp:489
Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d &_angle)
Given EulerZXY angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotX(ang...
Definition: Geometry.cpp:1049
Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d &_angle)
Given EulerYXY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(ang...
Definition: Geometry.cpp:941
Eigen::Matrix3d expMapJac(const Eigen::Vector3d &_q)
Computes the Jacobian of the expmap.
Definition: Geometry.cpp:444
Eigen::Matrix3d computeRotation(const Eigen::Vector3d &axis, const AxisType axisType)
Compute a rotation matrix from a vector.
Definition: Geometry.cpp:1446
double wrapToPi(double angle)
Compute the angle (in the range of -pi to +pi) which ignores any full rotations.
Definition: Geometry.hpp:463
Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d &_q, const Eigen::Vector3d &_qdot)
Computes the time derivative of the expmap Jacobian.
Definition: Geometry.cpp:461
Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d &_R)
get the Euler XYZ angle from R
Definition: Geometry.cpp:105
Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d &_m)
Definition: Geometry.cpp:1417
Eigen::Vector2d computeClosestPointOnLineSegment(const Eigen::Vector2d &_p, const Eigen::Vector2d &_s1, const Eigen::Vector2d &_s2)
Returns the point which is closest to _p that also lays on the line segment that goes from _s1 -> _s2...
Definition: Geometry.cpp:1880
Eigen::Vector2d computeCentroidOfHull(const SupportPolygon &_convexHull)
Compute the centroid of a polygon, assuming the polygon is a convex hull.
Definition: Geometry.cpp:1556
Eigen::Vector6d AdTLinear(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
fast version of Ad(T, se3(0, v))
Definition: Geometry.cpp:673
Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d &_angle)
Given EulerZYX angles, return a 3x3 rotation matrix, which is equivalent to RotZ(angle(0)) * RotY(ang...
Definition: Geometry.cpp:1080
bool isInsideSupportPolygon(const Eigen::Vector2d &_p, const SupportPolygon &_support, bool _includeEdge)
Returns true if the point _p is inside the support polygon.
Definition: Geometry.cpp:1815
Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_v)
Fast version of Ad(Inv([R 0; 0 1]), se3(0, v))
Definition: Geometry.cpp:737
Eigen::Vector3d rotatePoint(const Eigen::Quaterniond &_q, const Eigen::Vector3d &_pt)
Definition: Geometry.cpp:395
Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d &_angle)
Given EulerXZX angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:879
Eigen::Isometry3d computeTransform(const Eigen::Vector3d &axis, const Eigen::Vector3d &translation, AxisType axisType)
Compute a transform from a vector and a position.
Definition: Geometry.cpp:1476
Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d &_original, const Eigen::Vector3d &_comShift, double _mass)
Use the Parallel Axis Theorem to compute the moment of inertia of a body whose center of mass has bee...
Definition: Geometry.cpp:1394
Eigen::Isometry3d expAngular(const Eigen::Vector3d &_s)
fast version of Exp(se3(s, 0))
Definition: Geometry.cpp:1215
bool verifyTransform(const Eigen::Isometry3d &_T)
Check if determinant of the rotational part of _T is equat to 1 and all the elements are not NaN valu...
Definition: Geometry.cpp:1412
Derived::PlainObject AdRInvJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Definition: Geometry.hpp:289
Eigen::Vector6d AdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
fast version of Ad(Inv(T), V)
Definition: Geometry.cpp:714
Eigen::Vector3d logMap(const Eigen::Matrix3d &_R)
Log mapping.
Definition: Geometry.cpp:515
Eigen::Isometry3d expMap(const Eigen::Vector6d &_S)
Exponential mapping.
Definition: Geometry.cpp:1176
Eigen::Vector6d AdR(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
Fast version of Ad([R 0; 0 1], V)
Definition: Geometry.cpp:650
double cross(const Eigen::Vector2d &_v1, const Eigen::Vector2d &_v2)
Compute a 2D cross product.
Definition: Geometry.cpp:1809
Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d &_angle)
Given EulerXZY angles, return a 3x3 rotation matrix, which is equivalent to RotX(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:910
Eigen::Vector6d dAdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv(T), F)
Definition: Geometry.cpp:784
Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d &_angle)
Given EulerYZX angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:995
SupportPolygon computeConvexHull(const SupportPolygon &_points)
Computes the convex hull of a set of 2D points.
Definition: Geometry.cpp:1548
IntersectionResult
Intersection_t is returned by the computeIntersection() function to indicate whether there was a vali...
Definition: Geometry.hpp:531
@ INTERSECTING
An intersection was found.
Definition: Geometry.hpp:533
@ BEYOND_ENDPOINTS
There is no intersection because the end points do not expand far enough.
Definition: Geometry.hpp:535
@ PARALLEL
The line segments are parallel.
Definition: Geometry.hpp:534
Derived::PlainObject AdTJacFixed(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for fixed size Jacobian.
Definition: Geometry.hpp:234
void extractNullSpace(const Eigen::JacobiSVD< MatrixType > &_SVD, ReturnType &_NS)
Definition: Geometry.hpp:471
Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d &_R)
get the Euler YXZ angle from R
Definition: Geometry.cpp:236
Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d &_R)
get the Euler XZY angle from R
Definition: Geometry.cpp:161
Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond &_q, int _el)
Definition: Geometry.cpp:262
Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d &_v)
Definition: Geometry.cpp:1432
void computeNullSpace(const MatrixType &_M, ReturnType &_NS)
Definition: Geometry.hpp:490
Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond &, int _el1, int _el2)
Definition: Geometry.cpp:317
Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d &_R)
get the Euler YZX angle from R
Definition: Geometry.cpp:186
Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d &_angle)
Given EulerYXZ angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotX(ang...
Definition: Geometry.cpp:972
Eigen::Vector3d matrixToEulerZYX(const Eigen::Matrix3d &_R)
get the Euler ZYX angle from R
Definition: Geometry.cpp:136
Derived::PlainObject adJac(const Eigen::Vector6d &_V, const Eigen::MatrixBase< Derived > &_J)
Definition: Geometry.hpp:307
Eigen::Vector6d AdTAngular(const Eigen::Isometry3d &_T, const Eigen::Vector3d &_w)
fast version of Ad(T, se3(w, 0))
Definition: Geometry.cpp:661
Derived::PlainObject AdTJac(const Eigen::Isometry3d &_T, const Eigen::MatrixBase< Derived > &_J)
Adjoint mapping for dynamic size Jacobian.
Definition: Geometry.hpp:216
Eigen::Vector6d dAdT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
dual adjoint mapping
Definition: Geometry.cpp:759
Eigen::Vector6d ad(const Eigen::Vector6d &_X, const Eigen::Vector6d &_Y)
adjoint mapping
Definition: Geometry.cpp:744
Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d &_angle)
Given EulerYZY angles, return a 3x3 rotation matrix, which is equivalent to RotY(angle(0)) * RotZ(ang...
Definition: Geometry.cpp:1018
Eigen::Vector2d computeClosestPointOnSupportPolygon(const Eigen::Vector2d &_p, const SupportPolygon &_support)
Returns the point which is closest to _p that also lays on the edge of the support polygon.
Definition: Geometry.cpp:1921
std::vector< Eigen::Vector3d > SupportGeometry
Definition: Geometry.hpp:496
Eigen::Vector6d AdT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
Rectify the rotation part so as that it satifies the orthogonality condition.
Definition: Geometry.cpp:624
Inertia transformInertia(const Eigen::Isometry3d &_T, const Inertia &_I)
Definition: Geometry.cpp:1311
Eigen::Matrix3d expMapRot(const Eigen::Vector3d &_q)
Computes the Rotation matrix from a given expmap vector.
Definition: Geometry.cpp:427
Eigen::Vector6d dAdInvR(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv([R 0; 0 1]), F)
Definition: Geometry.cpp:793
Eigen::Quaterniond expToQuat(const Eigen::Vector3d &_v)
Definition: Geometry.cpp:50
Eigen::Matrix6d Inertia
Definition: MathTypes.hpp:105
common::aligned_vector< Eigen::Vector2d > SupportPolygon
Definition: Geometry.hpp:498
Definition: BulletCollisionDetector.cpp:63
static constexpr T pi()
Definition: Constants.hpp:44