Biorbd Release_1.13.0
Loading...
Searching...
No Matches
RotoTrans.cpp
1#define BIORBD_API_EXPORTS
2#include "Utils/RotoTrans.h"
3
4#include "RigidBody/NodeSegment.h"
5#include "Utils/Error.h"
6#include "Utils/Rotation.h"
7#include "Utils/SpatialTransform.h"
8#include "Utils/String.h"
9#include "Utils/Vector.h"
10#include "Utils/Vector3d.h"
11
12using namespace BIORBD_NAMESPACE;
13
14utils::RotoTrans::RotoTrans(const RigidBodyDynamics::Math::Matrix4d& matrix)
15 : RigidBodyDynamics::Math::Matrix4d(matrix) {
17}
18
20 const utils::Scalar& v00,
21 const utils::Scalar& v01,
22 const utils::Scalar& v02,
23 const utils::Scalar& v03,
24 const utils::Scalar& v10,
25 const utils::Scalar& v11,
26 const utils::Scalar& v12,
27 const utils::Scalar& v13,
28 const utils::Scalar& v20,
29 const utils::Scalar& v21,
30 const utils::Scalar& v22,
31 const utils::Scalar& v23,
32 const utils::Scalar& v30,
33 const utils::Scalar& v31,
34 const utils::Scalar& v32,
35 const utils::Scalar& v33)
36 : RigidBodyDynamics::Math::Matrix4d(
37 v00,
38 v01,
39 v02,
40 v03,
41 v10,
42 v11,
43 v12,
44 v13,
45 v20,
46 v21,
47 v22,
48 v23,
49 v30,
50 v31,
51 v32,
52 v33) {
54}
55
57 : RigidBodyDynamics::Math::Matrix4d(
58 combineRotAndTrans(rot, utils::Vector3d::Zero())) {}
59
61 const utils::Rotation& rot,
63 : RigidBodyDynamics::Math::Matrix4d(combineRotAndTrans(rot, trans)) {}
64
66 const utils::Vector& rotation,
67 const utils::Vector3d& translation,
68 const utils::String& rotationSequence)
69 : RigidBodyDynamics::Math::Matrix4d(
70 fromEulerAngles(rotation, translation, rotationSequence)) {}
71
73 : RigidBodyDynamics::Math::Matrix4d(fromSpatialTransform(st)) {}
74
76 const rigidbody::NodeSegment& origin,
77 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
78 axis1markers,
79 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
80 axis2markers,
81 const std::pair<utils::String, utils::String>& axesNames,
82 const utils::String& axisToRecalculate) {
83 RotoTrans rt_out;
84 rt_out.block(0, 0, 3, 3) = Rotation::fromMarkers(
85 axis1markers, axis2markers, axesNames, axisToRecalculate);
86 rt_out.block(0, 3, 3, 1) = origin;
87 return rt_out;
88}
89
91 utils::Error::check(idx <= 2, "Axis must be between 0 and 2 included");
92 return rot().block(0, static_cast<unsigned int>(idx), 3, 1);
93}
94
97 tp.block(0, 0, 3, 3) = block(0, 0, 3, 3).transpose();
98 tp.block(0, 3, 3, 1) = -tp.block(0, 0, 3, 3) * block(0, 3, 3, 1);
99 tp.block(3, 0, 1, 4) =
100 RigidBodyDynamics::Math::Vector4d(0, 0, 0, 1).transpose();
101 return tp;
102}
103
105 return this->block<3, 1>(0, 3);
106}
107
109 return this->block<3, 3>(0, 0);
110}
111
113 const utils::Rotation& rot,
114 const utils::Vector3d& trans) {
116 out.block(0, 0, 3, 3) = rot;
117 out.block(0, 3, 3, 1) = trans;
118 out.block(3, 0, 1, 4) =
119 RigidBodyDynamics::Math::Vector4d(0, 0, 0, 1).transpose();
120 return out;
121}
122
127
129 const utils::Vector& rot,
130 const utils::Vector3d& trans,
131 const utils::String& seq) {
133
135 out.block(0, 0, 3, 3) = rot_mat;
136 out.block(0, 3, 3, 1) = trans;
137 return out;
138}
139
141 const utils::RotoTrans& rt,
142 const utils::String& seq) {
143 return utils::Rotation::toEulerAngles(rt.block<3, 3>(0, 0), seq);
144}
145
146#ifndef BIORBD_USE_CASADI_MATH
147utils::RotoTrans utils::RotoTrans::mean(const std::vector<RotoTrans>& mToMean) {
148 // The translation part is just the actual mean
149 RigidBodyDynamics::Math::Vector3d v_tp;
150 v_tp.setZero();
151 for (size_t i = 0; i < mToMean.size(); ++i) {
152 v_tp += mToMean[i].trans();
153 }
154 v_tp = v_tp / mToMean.size();
155
156 // The rotation part should call the proper way implemented in Rotation
157 std::vector<utils::Rotation> rotations;
158 for (size_t i = 0; i < mToMean.size(); ++i) {
159 rotations.push_back(mToMean[i].block<3, 3>(0, 0));
160 }
161 utils::RotoTrans m_out(utils::Rotation::mean(rotations), v_tp);
162 return m_out;
163}
164#endif
165
166RigidBodyDynamics::Math::Vector4d utils::RotoTrans::expand3dTo4d(
167 const utils::Vector3d& v1) {
168 RigidBodyDynamics::Math::Vector4d v2;
169 v2.block(0, 0, 3, 1) = v1;
170 v2(3) = 1;
171 return v2;
172}
173
175#ifndef BIORBD_USE_CASADI_MATH
176#ifndef SKIP_ASSERT
177 this->rot(); // Automatically cast the test for the rotation part
179 this->block(3, 0, 1, 4).sum() == 1.,
180 "Last row of the RotoTrans should be (0,0,0,1");
182 (*this)(3, 3) == 1., "Last row of the RotoTrans should be (0,0,0,1");
183#endif
184#endif
185}
186
187std::ostream& operator<<(std::ostream& os, const utils::RotoTrans& a) {
188 os << a.block(0, 0, 4, 4);
189 return os;
190}
A point attached to a segment, generally speaking a skin marker.
Definition NodeSegment.h:20
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
static utils::Rotation mean(const std::vector< utils::Rotation > &mToMean)
Get the mean of the Rotation matrices.
Definition Rotation.cpp:275
static Rotation fromEulerAngles(const Vector &rot, const String &seq)
Create a Rotation from Euler angles.
Definition Rotation.cpp:57
static Vector toEulerAngles(const Rotation &r, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
Definition Rotation.cpp:187
static Rotation fromMarkers(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 defined by markers
Definition Rotation.cpp:168
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).
utils::Matrix3d rotation() const
Return the rotation part of the transform.
utils::Vector3d translation() const
Return the translation part of the transform.
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