Biorbd
Rotation.cpp
1 #define BIORBD_API_EXPORTS
2 #include "Utils/Rotation.h"
3 
4 #include "Utils/Error.h"
5 #include "Utils/Vector3d.h"
6 #include "Utils/String.h"
7 #include "Utils/Vector.h"
8 #include "Utils/Matrix.h"
9 
10 #include "RigidBody/NodeSegment.h"
11 
12 biorbd::utils::Rotation::Rotation(
13  const RigidBodyDynamics::Math::Matrix3d& matrix) :
14  RigidBodyDynamics::Math::Matrix3d(matrix)
15 {
16  checkUnitary();
17 }
18 
19 #ifdef BIORBD_USE_CASADI_MATH
20 
21 biorbd::utils::Rotation::Rotation(
22  const RigidBodyDynamics::Math::MatrixNd &m) :
23  RigidBodyDynamics::Math::Matrix3d(m)
24 {
25 
26 }
27 
28 #endif
29 
30 biorbd::utils::Rotation::Rotation(
31  const biorbd::utils::Scalar& v00, const biorbd::utils::Scalar& v01, const biorbd::utils::Scalar& v02,
32  const biorbd::utils::Scalar& v10, const biorbd::utils::Scalar& v11, const biorbd::utils::Scalar& v12,
33  const biorbd::utils::Scalar& v20, const biorbd::utils::Scalar& v21, const biorbd::utils::Scalar& v22) :
34  RigidBodyDynamics::Math::Matrix3d (v00, v01, v02, v10, v11, v12, v20, v21, v22)
35 {
36 
37 }
38 
39 biorbd::utils::Rotation::Rotation(
40  const biorbd::utils::Vector& rotation,
41  const biorbd::utils::String& rotationSequence) :
42  RigidBodyDynamics::Math::Matrix3d(fromEulerAngles(rotation, rotationSequence))
43 {
44 
45 }
46 
47 biorbd::utils::Rotation::Rotation(
48  const RigidBodyDynamics::Math::SpatialTransform &st) :
49  RigidBodyDynamics::Math::Matrix3d(st.E)
50 {
51 
52 }
53 
54 biorbd::utils::Vector3d biorbd::utils::Rotation::axe(unsigned int idx) const
55 {
56  biorbd::utils::Error::check(idx<=2, "Axis must be between 0 and 2 included");
57  return this->block(0,idx, 3, 1);
58 }
59 
60 biorbd::utils::Rotation biorbd::utils::Rotation::fromSpatialTransform(
61  const RigidBodyDynamics::Math::SpatialTransform& st)
62 {
63  return st.E;
64 }
65 
66 biorbd::utils::Rotation biorbd::utils::Rotation::fromEulerAngles(
67  const biorbd::utils::Vector &rot,
68  const biorbd::utils::String& seq)
69 {
70  // Check for size consistency
71  biorbd::utils::Error::check(
72  seq.length() == static_cast<unsigned int>(rot.rows()),
73  "Rotation and sequence of rotation must be the same length");
74 
75  biorbd::utils::Rotation out;
76  out.setIdentity();
77  // Set the actual rotation matrix to this
78  RigidBodyDynamics::Math::Matrix3d tp;
79  for (unsigned int i=0; i<seq.length(); ++i){
80  biorbd::utils::Scalar cosVi(std::cos(rot[i]));
81  biorbd::utils::Scalar sinVi(std::sin(rot[i]));
82  if (seq.tolower()[i] == 'x')
83  tp = RigidBodyDynamics::Math::Matrix3d(1, 0, 0,
84  0, cosVi, -sinVi,
85  0, sinVi, cosVi);
86 
87  else if (seq.tolower()[i] == 'y')
88  tp = RigidBodyDynamics::Math::Matrix3d(cosVi, 0, sinVi,
89  0, 1, 0,
90  -sinVi, 0, cosVi);
91 
92  else if (seq.tolower()[i] == 'z')
93  tp = RigidBodyDynamics::Math::Matrix3d(cosVi, -sinVi, 0,
94  sinVi, cosVi, 0,
95  0, 0, 1);
96  else
97  biorbd::utils::Error::raise("Rotation sequence not recognized");
98 
99  out.block(0,0,3,3) = out.block(0,0,3,3) * tp;
100  }
101  return out;
102 }
103 
104 biorbd::utils::Matrix biorbd::utils::Rotation::fromMarkersNonNormalized(
105  const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment> &axis1markers,
106  const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment> &axis2markers,
107  const std::pair<biorbd::utils::String, biorbd::utils::String>& axesNames,
108  const biorbd::utils::String &axisToRecalculate)
109 {
110  // Figure out where to put the axes
111  std::vector<unsigned int> map(3);
112  std::vector<unsigned int> toMultiply(2);
113  if (!axesNames.first.tolower().compare("x")){
114  map[0] = 0;
115  if (!axesNames.second.tolower().compare("y")){
116  map[1] = 1;
117  map[2] = 2;
118  toMultiply[0] = 0;
119  toMultiply[1] = 1;
120  } else {
121  map[1] = 2;
122  map[2] = 1;
123  toMultiply[0] = 2;
124  toMultiply[1] = 0;
125  }
126  }
127  else if (!axesNames.first.tolower().compare("y")){
128  map[0] = 1;
129  if (!axesNames.second.tolower().compare("x")){
130  map[1] = 0;
131  map[2] = 2;
132  toMultiply[0] = 0;
133  toMultiply[1] = 1;
134  } else {
135  map[1] = 2;
136  map[2] = 0;
137  toMultiply[0] = 1;
138  toMultiply[1] = 2;
139  }
140  }
141  else if (!axesNames.first.tolower().compare("z")){
142  map[0] = 2;
143  if (!axesNames.second.tolower().compare("x")){
144  map[1] = 0;
145  map[2] = 1;
146  toMultiply[0] = 2;
147  toMultiply[1] = 0;
148  } else {
149  map[1] = 1;
150  map[2] = 0;
151  toMultiply[0] = 1;
152  toMultiply[1] = 2;
153  }
154  }
155 
156  // Get the system of axis XYZ
157  std::vector<biorbd::utils::Vector3d> axes(3);
158  axes[map[0]] = axis1markers.second - axis1markers.first;
159  axes[map[1]] = axis2markers.second - axis2markers.first;
160  axes[map[2]] = axes[toMultiply[0]].cross(axes[toMultiply[1]]);
161 
162  // Recalculate one axis
163  if (!axisToRecalculate.tolower().compare("x")){
164  axes[0] = axes[1].cross(axes[2]);
165  } else if (!axisToRecalculate.tolower().compare("y")){
166  axes[1] = axes[2].cross(axes[0]);
167  } else if (!axisToRecalculate.tolower().compare("z")){
168  axes[2] = axes[0].cross(axes[1]);
169  }
170 
171  // Organize them in a non-normalized matrix
172  biorbd::utils::Matrix r_out(3, 3);
173  for (unsigned int i=0; i<3; ++i){
174  r_out.block(0, i, 3, 1) = axes[i];
175  }
176  return r_out;
177 }
178 
179 biorbd::utils::Rotation biorbd::utils::Rotation::fromMarkers(
180  const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment> &axis1markers,
181  const std::pair<biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment> &axis2markers,
182  const std::pair<biorbd::utils::String, biorbd::utils::String>& axesNames,
183  const biorbd::utils::String &axisToRecalculate)
184 {
185  biorbd::utils::Matrix r_out(
186  fromMarkersNonNormalized(axis1markers, axis2markers, axesNames, axisToRecalculate));
187 
188  // Organize them in a normalized matrix
189  for (unsigned int i=0; i<3; ++i){
190  // Normalize axes
191  r_out.block<3, 1>(0, i).normalize();
192  }
193 
194  return r_out;
195 }
196 
197 biorbd::utils::Vector biorbd::utils::Rotation::toEulerAngles(
198  const biorbd::utils::Rotation &r,
199  const biorbd::utils::String &seq)
200 {
201  biorbd::utils::Vector v;
202  if (!seq.compare("zyzz"))
203  v = biorbd::utils::Vector(3);
204  else
205  v = biorbd::utils::Vector(static_cast<unsigned int>(seq.length()));
206 
207  if (!seq.compare("x")) {
208  v[0] = std::asin(r(2, 1)); // x
209  }
210  else if (!seq.compare("y")) {
211  v[0] = std::asin(r(0, 2)); // y
212  }
213  else if (!seq.compare("z")) {
214  v[0] = std::asin(r(1, 0)); // z
215  }
216  else if (!seq.compare("xy")) {
217  v[0] = std::asin(r(2,1)); // x
218  v[1] = std::asin(r(0,2)); // y
219  }
220  else if (!seq.compare("xz")) {
221  v[0] = -std::asin(r(1,2)); // x
222  v[1] = -std::asin(r(0,1)); // z
223  }
224  else if (!seq.compare("yx")) {
225  v[0] = -std::asin(r(2,0)); // y
226  v[1] = -std::asin(r(1,2)); // x
227  }
228  else if (!seq.compare("yz")) {
229  v[0] = std::asin(r(0,2)); // y
230  v[1] = std::asin(r(1,0)); // z
231  }
232  else if (!seq.compare("zx")) {
233  v[0] = std::asin(r(1,0)); // z
234  v[1] = std::asin(r(2,1)); // x
235  }
236  else if (!seq.compare("zy")) {
237  v[0] = -std::asin(r(0,1)); // z
238  v[1] = -std::asin(r(2,0)); // y
239  }
240  else if (!seq.compare("xyz")) {
241  v[0] = std::atan2(-r(1,2), r(2,2)); // x
242  v[1] = std::asin(r(0,2)); // y
243  v[2] = std::atan2(-r(0,1), r(0,0)); // z
244  }
245  else if (!seq.compare("xzy")) {
246  v[0] = std::atan2(r(2,1), r(1,1)); // x
247  v[1] = std::asin(-r(0,1)); // z
248  v[2] = std::atan2(r(0,2), r(0,0)); // y
249  }
250  else if (!seq.compare("yxz")) {
251  v[0] = std::atan2(r(0,2), r(2,2)); // y
252  v[1] = std::asin(-r(1,2)); // x
253  v[2] = std::atan2(r(1,0), r(1,1)); // z
254  }
255  else if (!seq.compare("yzx")) {
256  v[0] = std::atan2(-r(2,0), r(0,0)); // y
257  v[1] = std::asin(r(1,0)); // z
258  v[2] = std::atan2(-r(1,2), r(1,1)); // x
259  }
260  else if (!seq.compare("zxy")) {
261  v[0] = std::atan2(-r(0,1), r(1,1)); // z
262  v[1] = std::asin(r(2,1)); // x
263  v[2] = std::atan2(-r(2,0), r(2,2)); // y
264  }
265  else if (!seq.compare("zyx")) {
266  v[0] = std::atan2(r(1,0), r(0,0)); // z
267  v[1] = std::asin(-r(2,0)); // y
268  v[2] = std::atan2(r(2,1), r(2,2)); // x
269  }
270  else if (!seq.compare("zxz")) {
271  v[0] = std::atan2(r(0,2), -r(1,2)); // z
272  v[1] = std::acos(r(2,2)); // x
273  v[2] = std::atan2(r(2,0), r(2,1)); // z
274  }
275  else if (!seq.compare("zyz")) {
276  v[0] = std::atan2(r(1,2), r(0,2)); // z
277  v[1] = std::acos(r(2,2)); // y
278  v[2] = std::atan2(r(2,1), -r(2,0)); // z
279  }
280  else if (!seq.compare("zyzz")) {
281  v[0] = std::atan2(r(1,2), r(0,2)); // z
282  v[1] = std::acos(r(2,2)); // y
283  v[2] = std::atan2(r(2,1), -r(2,0)) + v[0]; // z+z
284  }
285  else {
286  biorbd::utils::Error::raise("Angle sequence is not recognized");
287  }
288 
289  return v;
290 }
291 
292 #ifndef BIORBD_USE_CASADI_MATH
293 biorbd::utils::Rotation biorbd::utils::Rotation::mean(
294  const std::vector<biorbd::utils::Rotation> & mToMean)
295 {
296  RigidBodyDynamics::Math::Matrix3d m_tp;
297  m_tp.setZero();
298 
299  // Initial guess being the actual arithmetic mean
300  for (unsigned int i = 0; i<mToMean.size(); ++i){
301  m_tp += mToMean[i];
302  }
303  m_tp = m_tp/mToMean.size();
304 
305  // SVD decomposition
306  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
307  m_tp, Eigen::ComputeFullU | Eigen::ComputeFullV);
308 
309  // Normalize the matrix
310  biorbd::utils::Rotation m_out(svd.matrixU() * svd.matrixV().transpose());
311 
312  return m_out;
313 }
314 #endif
315 
316 void biorbd::utils::Rotation::checkUnitary()
317 {
318 #ifndef BIORBD_USE_CASADI_MATH
319 #ifndef SKIP_ASSERT
320  biorbd::utils::Error::check(fabs(this->squaredNorm() - 3.) < 1e-10,
321  "The Rotation matrix norm is not equal to one");
322 #endif
323 #endif
324 }
325 
326 std::ostream &operator<<(std::ostream &os, const biorbd::utils::Rotation &a)
327 {
328  os << a.block(0,0,3,3);
329  return os;
330 }
biorbd::utils::Rotation::fromMarkersNonNormalized
static biorbd::utils::Matrix fromMarkersNonNormalized(const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis1markers, const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis2markers, const std::pair< biorbd::utils::String, biorbd::utils::String > &axesNames, const biorbd::utils::String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes defined by markers
Definition: Rotation.cpp:104
biorbd::utils::Rotation::checkUnitary
void checkUnitary()
Check if the Rotation is a unitary matrix of rotation.
Definition: Rotation.cpp:316
biorbd::utils::Vector
Wrapper of the Eigen VectorXd.
Definition: Vector.h:20
biorbd::utils::Vector3d
Wrapper around Eigen Vector3d and attach it to a parent.
Definition: Vector3d.h:24
biorbd::utils::Rotation::Rotation
Rotation(const RigidBodyDynamics::Math::Matrix3d &matrix=RigidBodyDynamics::Math::Matrix3d::Identity())
Construct Rotation matrix.
Definition: Rotation.cpp:12
biorbd::utils::Error::raise
static void raise(const biorbd::utils::String &message)
Throw an error message.
Definition: Error.cpp:4
biorbd::utils::Rotation::fromEulerAngles
static biorbd::utils::Rotation fromEulerAngles(const biorbd::utils::Vector &rot, const biorbd::utils::String &seq)
Create a Rotation from Euler angles.
Definition: Rotation.cpp:66
biorbd::utils::String::tolower
static biorbd::utils::String tolower(const biorbd::utils::String &str)
Convert a string to a lower case string.
Definition: String.cpp:97
biorbd::utils::Matrix
A wrapper for the Eigen::MatrixXd.
Definition: Matrix.h:21
biorbd::utils::Rotation::toEulerAngles
static biorbd::utils::Vector toEulerAngles(const biorbd::utils::Rotation &r, const biorbd::utils::String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
Definition: Rotation.cpp:197
biorbd::utils::Rotation::fromSpatialTransform
static biorbd::utils::Rotation fromSpatialTransform(const RigidBodyDynamics::Math::SpatialTransform &st)
set the Rotation from a spatial transform
Definition: Rotation.cpp:60
biorbd::utils::Rotation::axe
biorbd::utils::Vector3d axe(unsigned int idx) const
Get a particular axis of the Rotation matrix.
Definition: Rotation.cpp:54
biorbd::utils::Rotation::fromMarkers
static biorbd::utils::Rotation fromMarkers(const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis1markers, const std::pair< biorbd::rigidbody::NodeSegment, biorbd::rigidbody::NodeSegment > &axis2markers, const std::pair< biorbd::utils::String, biorbd::utils::String > &axesNames, const biorbd::utils::String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes defined by markers
Definition: Rotation.cpp:179
biorbd::utils::Rotation
Rotation matrix.
Definition: Rotation.h:34
biorbd::utils::String
Wrapper around the std::string class with augmented functionality.
Definition: String.h:17
biorbd::utils::Rotation::mean
static biorbd::utils::Rotation mean(const std::vector< biorbd::utils::Rotation > &mToMean)
Get the mean of the Rotation matrices.
Definition: Rotation.cpp:293
biorbd::utils::Error::check
static void check(bool cond, const biorbd::utils::String &message)
Assert that raises the error message if false.
Definition: Error.cpp:10
Generated by   doxygen 1.8.18