DART 6.7.3
Loading...
Searching...
No Matches
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
41
42namespace dart {
43namespace math {
44
46Eigen::Matrix3d makeSkewSymmetric(const Eigen::Vector3d& _v);
47
49Eigen::Vector3d fromSkewSymmetric(const Eigen::Matrix3d& _m);
50
51//------------------------------------------------------------------------------
53Eigen::Quaterniond expToQuat(const Eigen::Vector3d& _v);
54
56Eigen::Vector3d quatToExp(const Eigen::Quaterniond& _q);
57
59Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
60 const Eigen::Vector3d& _pt);
61
63Eigen::Vector3d rotatePoint(const Eigen::Quaterniond& _q,
64 double _x, double _y, double _z);
65
67Eigen::Matrix3d quatDeriv(const Eigen::Quaterniond& _q, int _el);
68
70Eigen::Matrix3d quatSecondDeriv(const Eigen::Quaterniond& _q,
71 int _el1, int _el2);
72
73//------------------------------------------------------------------------------
76Eigen::Matrix3d eulerXYXToMatrix(const Eigen::Vector3d& _angle);
77
80Eigen::Matrix3d eulerXYZToMatrix(const Eigen::Vector3d& _angle);
81
84Eigen::Matrix3d eulerXZXToMatrix(const Eigen::Vector3d& _angle);
85
88Eigen::Matrix3d eulerXZYToMatrix(const Eigen::Vector3d& _angle);
89
92Eigen::Matrix3d eulerYXYToMatrix(const Eigen::Vector3d& _angle);
93
96Eigen::Matrix3d eulerYXZToMatrix(const Eigen::Vector3d& _angle);
97
100Eigen::Matrix3d eulerYZXToMatrix(const Eigen::Vector3d& _angle);
101
104Eigen::Matrix3d eulerYZYToMatrix(const Eigen::Vector3d& _angle);
105
108Eigen::Matrix3d eulerZXYToMatrix(const Eigen::Vector3d& _angle);
109
113Eigen::Matrix3d eulerZYXToMatrix(const Eigen::Vector3d& _angle);
114
117Eigen::Matrix3d eulerZXZToMatrix(const Eigen::Vector3d& _angle);
118
122Eigen::Matrix3d eulerZYZToMatrix(const Eigen::Vector3d& _angle);
123
124//------------------------------------------------------------------------------
126Eigen::Vector3d matrixToEulerXYX(const Eigen::Matrix3d& _R);
127
129Eigen::Vector3d matrixToEulerXYZ(const Eigen::Matrix3d& _R);
130
132// Eigen::Vector3d matrixToEulerXZX(const Eigen::Matrix3d& R);
133
135Eigen::Vector3d matrixToEulerXZY(const Eigen::Matrix3d& _R);
136
138// Eigen::Vector3d matrixToEulerYXY(const Eigen::Matrix3d& R);
139
141Eigen::Vector3d matrixToEulerYXZ(const Eigen::Matrix3d& _R);
142
144Eigen::Vector3d matrixToEulerYZX(const Eigen::Matrix3d& _R);
145
147// Eigen::Vector3d matrixToEulerYZY(const Eigen::Matrix3d& R);
148
150Eigen::Vector3d matrixToEulerZXY(const Eigen::Matrix3d& _R);
151
153Eigen::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//------------------------------------------------------------------------------
163Eigen::Isometry3d expMap(const Eigen::Vector6d& _S);
164
169Eigen::Isometry3d expAngular(const Eigen::Vector3d& _s);
170
172Eigen::Matrix3d expMapRot(const Eigen::Vector3d& _expmap);
173
175Eigen::Matrix3d expMapJac(const Eigen::Vector3d& _expmap);
176
178Eigen::Matrix3d expMapJacDot(const Eigen::Vector3d& _expmap,
179 const Eigen::Vector3d& _qdot);
180
183Eigen::Matrix3d expMapJacDeriv(const Eigen::Vector3d& _expmap, int _qi);
184
188Eigen::Vector3d logMap(const Eigen::Matrix3d& _R);
189
191Eigen::Vector6d logMap(const Eigen::Isometry3d& _T);
192
193//------------------------------------------------------------------------------
200// SE3 Normalize(const SE3& T);
201
203// Axis Reparameterize(const Axis& s);
204
205//------------------------------------------------------------------------------
209Eigen::Vector6d AdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
210
212Eigen::Matrix6d getAdTMatrix(const Eigen::Isometry3d& T);
213
215template<typename Derived>
216typename 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
233template<typename Derived>
234typename 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
256Eigen::Vector6d AdR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
257
259Eigen::Vector6d AdTAngular(const Eigen::Isometry3d& _T,
260 const Eigen::Vector3d& _w);
261
263Eigen::Vector6d AdTLinear(const Eigen::Isometry3d& _T,
264 const Eigen::Vector3d& _v);
265
267// se3 AdP(const Vec3& p, const se3& s);
268
270template<typename Derived>
271typename 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
288template<typename Derived>
289typename 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
306template<typename Derived>
307typename 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
326Eigen::Vector6d AdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _V);
327
329template<typename Derived>
330typename 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
347template<typename Derived>
348typename 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
383Eigen::Vector6d AdInvRLinear(const Eigen::Isometry3d& _T,
384 const Eigen::Vector3d& _v);
385
389Eigen::Vector6d dAdT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
390
392// dse3 dAdTLinear(const SE3& T, const Vec3& F);
393
395Eigen::Vector6d dAdInvT(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
396
398Eigen::Vector6d dAdInvR(const Eigen::Isometry3d& _T, const Eigen::Vector6d& _F);
399
401// dse3 dAdInvPLinear(const Vec3& p, const Vec3& F);
402
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
418
420Inertia transformInertia(const Eigen::Isometry3d& _T, const Inertia& _AI);
421
424Eigen::Matrix3d parallelAxisTheorem(const Eigen::Matrix3d& _original,
425 const Eigen::Vector3d& _comShift,
426 double _mass);
427
429{
432 AXIS_Z = 2
434
438Eigen::Matrix3d computeRotation(const Eigen::Vector3d& axis,
439 AxisType axisType = AxisType::AXIS_X);
440
444Eigen::Isometry3d computeTransform(const Eigen::Vector3d& axis,
445 const Eigen::Vector3d& translation,
446 AxisType axisType = AxisType::AXIS_X);
447
450Eigen::Isometry3d getFrameOriginAxisZ(const Eigen::Vector3d& _origin,
451 const Eigen::Vector3d& _axisZ);
452
455bool verifyRotation(const Eigen::Matrix3d& _R);
456
459bool verifyTransform(const Eigen::Isometry3d& _T);
460
463inline double wrapToPi(double angle)
464{
465 constexpr auto pi = constantsd::pi();
466
467 return std::fmod(angle+pi, 2*pi) - pi;
468}
469
470template <typename MatrixType, typename ReturnType>
471void 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
489template <typename MatrixType, typename ReturnType>
490void computeNullSpace(const MatrixType& _M, ReturnType& _NS)
491{
492 Eigen::JacobiSVD<MatrixType> svd(_M, Eigen::ComputeFullV);
493 extractNullSpace(svd, _NS);
494}
495
496typedef 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
523SupportPolygon computeConvexHull(std::vector<std::size_t>& _originalIndices,
524 const SupportPolygon& _points);
525
527Eigen::Vector2d computeCentroidOfHull(const SupportPolygon& _convexHull);
528
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
549double cross(const Eigen::Vector2d& _v1, const Eigen::Vector2d& _v2);
550
552bool isInsideSupportPolygon(const Eigen::Vector2d& _p,
553 const SupportPolygon& _support,
554 bool _includeEdge = true);
555
558Eigen::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.
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 & getMin() const
Definition Geometry.hpp:585
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
const Eigen::Vector3d & getMax() const
Definition Geometry.hpp:586
void setMax(const Eigen::Vector3d &max)
Definition Geometry.hpp:589
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoundingBox()
Definition Geometry.cpp:1976
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