sot-talos-balance  1.5.0
state-transformation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Gepetto team, LAAS-CNRS
3  *
4  * This file is part of sot-talos-balance.
5  * sot-talos-balance is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-talos-balance is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
18 
19 #include <sot/core/debug.hh>
20 #include <dynamic-graph/factory.h>
21 #include <dynamic-graph/command-bind.h>
22 
23 #include <dynamic-graph/all-commands.h>
24 #include <sot/core/stop-watch.hh>
25 
26 namespace dynamicgraph
27 {
28  namespace sot
29  {
30  namespace talos_balance
31  {
32  namespace dg = ::dynamicgraph;
33  using namespace dg;
34  using namespace dg::command;
35 
36 //Size to be aligned "-------------------------------------------------------"
37 #define PROFILE_STATETRANSFORMATION_Q_COMPUTATION "State transformation q computation "
38 #define PROFILE_STATETRANSFORMATION_V_COMPUTATION "State transformation v computation "
39 
40 #define INPUT_SIGNALS m_referenceFrameSIN << m_q_inSIN << m_v_inSIN
41 
42 #define OUTPUT_SIGNALS m_qSOUT << m_vSOUT
43 
46  typedef StateTransformation EntityClassName;
47 
48  /* --- DG FACTORY ---------------------------------------------------- */
49  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StateTransformation,
50  "StateTransformation");
51 
52  /* ------------------------------------------------------------------- */
53  /* --- CONSTRUCTION -------------------------------------------------- */
54  /* ------------------------------------------------------------------- */
56  : Entity(name)
57  , CONSTRUCT_SIGNAL_IN(referenceFrame, MatrixHomogeneous)
58  , CONSTRUCT_SIGNAL_IN(q_in, dynamicgraph::Vector)
59  , CONSTRUCT_SIGNAL_IN(v_in, dynamicgraph::Vector)
60  , CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, m_referenceFrameSIN << m_q_inSIN)
61  , CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, m_referenceFrameSIN << m_v_inSIN)
62  , m_initSucceeded(false)
63  {
64  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
65 
66  /* Commands. */
67  addCommand("init", makeCommandVoid0(*this, &StateTransformation::init, docCommandVoid0("Initialize the entity.")));
68  }
69 
71  {
72  m_initSucceeded = true;
73  }
74 
75  /* ------------------------------------------------------------------- */
76  /* --- SIGNALS ------------------------------------------------------- */
77  /* ------------------------------------------------------------------- */
78 
80  {
81  getProfiler().start(PROFILE_STATETRANSFORMATION_Q_COMPUTATION);
82 
83  const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
84 
85  // convert q base to homogeneous matrix
86  const dynamicgraph::Vector & input = m_q_inSIN(iter);
87 
88  const Eigen::Vector3d & euler = input.segment<3>(3);
89 
90  const double roll = euler[0];
91  const double pitch = euler[1];
92  const double yaw = euler[2];
93 
94  Eigen::Quaterniond quat;
95  quat = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
96  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
97  * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
98 
99  MatrixHomogeneous M;
100  M.linear() = quat.toRotationMatrix();
101  M.translation() = input.head<3>();
102 
103  // Convert the matrix
104  MatrixHomogeneous res = referenceFrame*M;
105 
106  // Write the result
107  size_t sz = input.size();
108  s.resize(sz);
109 
110  s.head<3>() = res.translation();
111 
112  s.segment<3>(3) = res.linear().eulerAngles(2, 1, 0).reverse();
113 
114  if(sz>6)
115  s.tail(sz-6) = input.tail(sz-6);
116 
117  getProfiler().stop(PROFILE_STATETRANSFORMATION_Q_COMPUTATION);
118 
119  return s;
120  }
121 
123  {
124  getProfiler().start(PROFILE_STATETRANSFORMATION_V_COMPUTATION);
125 
126  const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
127 
128  // Retrieve the input
129  const dynamicgraph::Vector & input = m_v_inSIN(iter);
130 
131  // Write the result
132  size_t sz = input.size();
133  s.resize(sz);
134 
135  s.head<3>() = referenceFrame.linear() * input.head<3>();
136 
137  s.segment<3>(3) = referenceFrame.linear() * input.segment<3>(3);
138 
139  if(sz>6)
140  s.tail(sz-6) = input.tail(sz-6);
141 
142  getProfiler().stop(PROFILE_STATETRANSFORMATION_V_COMPUTATION);
143 
144  return s;
145  }
146 
147  /* --- COMMANDS ---------------------------------------------------------- */
148 
149  /* ------------------------------------------------------------------- */
150  /* --- ENTITY -------------------------------------------------------- */
151  /* ------------------------------------------------------------------- */
152 
153  void StateTransformation::display(std::ostream& os) const
154  {
155  os << "StateTransformation " << getName();
156  try
157  {
158  getProfiler().report_all(3, os);
159  }
160  catch (ExceptionSignal e) {}
161  }
162  } // namespace talos_balance
163  } // namespace sot
164 } // namespace dynamicgraph
165 
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
#define INPUT_SIGNALS
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define PROFILE_STATETRANSFORMATION_V_COMPUTATION
EIGEN_MAKE_ALIGNED_OPERATOR_NEW StateTransformation(const std::string &name)
#define OUTPUT_SIGNALS
#define PROFILE_STATETRANSFORMATION_Q_COMPUTATION