vector-to-rotation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #include <sot/core/debug.hh>
11 #include <sot/core/factory.hh>
13 #include <sot/core/macros.hh>
15 
16 using namespace std;
17 using namespace dynamicgraph::sot;
18 using namespace dynamicgraph;
19 
22 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation, "VectorToRotation");
24 
25 /* --------------------------------------------------------------------- */
26 /* --------------------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 VectorToRotation::VectorToRotation(const std::string &name)
30  : Entity(name),
31  size(0),
32  axes(0),
33  SIN(NULL, "sotVectorToRotation(" + name + ")::output(vector)::sin"),
34  SOUT(SOT_MEMBER_SIGNAL_1(VectorToRotation::computeRotation, SIN,
36  "sotVectorToRotation(" + name + ")::output(matrixRotation)::sout") {
37  signalRegistration(SIN << SOUT);
38 }
39 /* --------------------------------------------------------------------- */
40 /* --------------------------------------------------------------------- */
41 /* --------------------------------------------------------------------- */
42 
43 MatrixRotation &VectorToRotation::computeRotation(
44  const dynamicgraph::Vector &angles, MatrixRotation &res) {
45  res.setIdentity();
46  MatrixRotation Ra, Rtmp;
47  for (unsigned int i = 0; i < size; ++i) {
48  Ra.setIdentity();
49  const double ca = cos(angles(i));
50  const double sa = sin(angles(i));
51  const unsigned int i_X = 0, i_Y = 1, i_Z = 2;
52  switch (axes[i]) {
53  case AXIS_X: {
54  Ra(i_Y, i_Y) = ca;
55  Ra(i_Y, i_Z) = -sa;
56  Ra(i_Z, i_Y) = sa;
57  Ra(i_Z, i_Z) = ca;
58  break;
59  }
60  case AXIS_Y: {
61  Ra(i_Z, i_Z) = ca;
62  Ra(i_Z, i_X) = -sa;
63  Ra(i_X, i_Z) = sa;
64  Ra(i_X, i_X) = ca;
65  break;
66  }
67  case AXIS_Z: {
68  Ra(i_X, i_X) = ca;
69  Ra(i_X, i_Y) = -sa;
70  Ra(i_Y, i_X) = sa;
71  Ra(i_Y, i_Y) = ca;
72  break;
73  }
74  }
75 
76  sotDEBUG(15) << "R" << i << " = " << Ra;
77  Rtmp = res * Ra;
78  res = Rtmp;
79  }
80 
81  return res;
82 }
Eigen::VectorXd Vector
int i
Eigen::Matrix< double, 3, 3 > MatrixRotation
SOT_CORE_DISABLE_WARNING_PUSH SOT_CORE_DISABLE_WARNING_DEPRECATED DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation, "VectorToRotation")
#define SOT_CORE_DISABLE_WARNING_PUSH
Definition: macros.hh:28
#define SOT_CORE_DISABLE_WARNING_DEPRECATED
Definition: macros.hh:30
#define SOT_MEMBER_SIGNAL_1(sotFunction, sotArg1, sotArg1Type)
#define sotDEBUG(level)
Definition: debug.hh:165
FCL_REAL size() const
Matrix3f axes
#define SOT_CORE_DISABLE_WARNING_POP
Definition: macros.hh:29


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26