Biorbd Release_1.13.0
Loading...
Searching...
No Matches
RotoTrans.h
1#ifndef BIORBD_UTILS_ROTO_TRANS_H
2#define BIORBD_UTILS_ROTO_TRANS_H
3
4#include "biorbdConfig.h"
5
6#include <memory>
7#include <vector>
8
9#include "Utils/Scalar.h"
10#include "rbdl/rbdl_math.h"
11
12namespace BIORBD_NAMESPACE {
13namespace rigidbody {
14class NodeSegment;
15}
16
17namespace utils {
18class String;
19class Vector;
20class Rotation;
21class Vector3d;
23
28#ifdef SWIG
29class BIORBD_API RotoTrans
30#else
31class BIORBD_API RotoTrans : public RigidBodyDynamics::Math::Matrix4d
32#endif
33{
34 public:
40 const RigidBodyDynamics::Math::Matrix4d& matrix =
41 RigidBodyDynamics::Math::Matrix4d::Identity());
42
63 const Scalar& v00,
64 const Scalar& v01,
65 const Scalar& v02,
66 const Scalar& v03,
67 const Scalar& v10,
68 const Scalar& v11,
69 const Scalar& v12,
70 const Scalar& v13,
71 const Scalar& v20,
72 const Scalar& v21,
73 const Scalar& v22,
74 const Scalar& v23,
75 const Scalar& v30,
76 const Scalar& v31,
77 const Scalar& v32,
78 const Scalar& v33);
79
84 RotoTrans(const Rotation& rot);
85
91 RotoTrans(const Rotation& rot, const Vector3d& trans);
92
102 RotoTrans(
103 const Vector& rotation,
104 const Vector3d& translation,
105 const String& rotationSequence);
106
112
113#ifdef BIORBD_USE_EIGEN3_MATH
114
119 template <typename OtherDerived>
120 RotoTrans(const Eigen::MatrixBase<OtherDerived>& other)
121 : RigidBodyDynamics::Math::Matrix4d(other) {
122 checkUnitary();
123 }
124
125#endif
126
140 static RotoTrans fromMarkers(
141 const rigidbody::NodeSegment& origin,
142 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
143 axis1markers,
144 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
145 axis2markers,
146 const std::pair<String, String>& axesNames,
147 const String& axisToRecalculate);
148
154 Vector3d axe(size_t idx) const;
155
160 RotoTrans transpose() const;
161
166 Vector3d trans() const;
167
172 Rotation rot() const;
173
181 const Rotation& rot,
182 const Vector3d& trans);
183
190
201 static RotoTrans
202 fromEulerAngles(const Vector& rot, const Vector3d& trans, const String& seq);
203
214 static Vector toEulerAngles(const RotoTrans& rt, const String& seq);
215
216#ifndef BIORBD_USE_CASADI_MATH
222 static utils::RotoTrans mean(const std::vector<utils::RotoTrans>& rt);
223#endif
224
225#ifndef SWIG
226#ifdef BIORBD_USE_EIGEN3_MATH
231 template <typename OtherDerived>
232 RotoTrans& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
233 Eigen::Matrix4d::operator=(other);
234 return *this;
235 }
236#endif
237#endif
238
239 protected:
245 RigidBodyDynamics::Math::Vector4d expand3dTo4d(const Vector3d& v1);
246
247 public:
254 void checkUnitary();
255};
256
257} // namespace utils
258} // namespace BIORBD_NAMESPACE
259
260#ifndef SWIG
266std::ostream& operator<<(
267 std::ostream& os,
269#endif
270
271#endif // BIORBD_UTILS_ROTO_TRANS_H
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Rotation rot() const
Return the rotation matrix.
Vector3d trans() const
Return the translation vector.
static RotoTrans combineRotAndTrans(const Rotation &rot, const Vector3d &trans)
Set the RotoTrans from a rotation and a translation.
RotoTrans(const RigidBodyDynamics::Math::Matrix4d &matrix=RigidBodyDynamics::Math::Matrix4d::Identity())
Construct RotoTrans matrix.
Definition RotoTrans.cpp:14
RigidBodyDynamics::Math::Vector4d expand3dTo4d(const Vector3d &v1)
Expand 3D vector to 4D (padding with an extra 1).
static RotoTrans fromSpatialTransform(const utils::SpatialTransform &st)
set the RotoTrans from a spatial transform
static Vector toEulerAngles(const RotoTrans &rt, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
static RotoTrans fromEulerAngles(const Vector &rot, const Vector3d &trans, const String &seq)
Create a RotoTrans from Euler angles.
RotoTrans transpose() const
Return the tranposed matrix.
Definition RotoTrans.cpp:95
void checkUnitary()
Check if the RotoTrans has a unitary matrix of rotation and the last row is (0, 0,...
static RotoTrans fromMarkers(const rigidbody::NodeSegment &origin, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes and an origin defined by markers
Definition RotoTrans.cpp:75
Vector3d axe(size_t idx) const
Get a particular axis of the rotation matrix.
Definition RotoTrans.cpp:90
static utils::RotoTrans mean(const std::vector< utils::RotoTrans > &rt)
Get the mean of the 4x4 matrices.
Wrapper of the SpatialTransform (rotation and translation).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21