sot-talos-balance  1.6.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  const dynamicgraph::Vector & input = m_q_inSIN(iter);
82  const size_t sz = input.size();
83  if((size_t)(s.size())!=sz)
84  s.resize(sz);
85 
86  getProfiler().start(PROFILE_STATETRANSFORMATION_Q_COMPUTATION);
87 
88  const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
89 
90  // convert q base to homogeneous matrix
91  const Eigen::Vector3d & euler = input.segment<3>(3);
92 
93  const double roll = euler[0];
94  const double pitch = euler[1];
95  const double yaw = euler[2];
96 
97  Eigen::Quaterniond quat;
98  quat = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
99  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
100  * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
101 
102  MatrixHomogeneous M;
103  M.linear() = quat.toRotationMatrix();
104  M.translation() = input.head<3>();
105 
106  // Convert the matrix
107  MatrixHomogeneous res = referenceFrame*M;
108 
109  // Write the result
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  const dynamicgraph::Vector & input = m_v_inSIN(iter);
125  const size_t sz = input.size();
126  if((size_t)(s.size())!=sz)
127  s.resize(sz);
128 
129  getProfiler().start(PROFILE_STATETRANSFORMATION_V_COMPUTATION);
130 
131  const MatrixHomogeneous & referenceFrame = m_referenceFrameSIN(iter);
132 
133  // Write the result
134  s.head<3>() = referenceFrame.linear() * input.head<3>();
135 
136  s.segment<3>(3) = referenceFrame.linear() * input.segment<3>(3);
137 
138  if(sz>6)
139  s.tail(sz-6) = input.tail(sz-6);
140 
141  getProfiler().stop(PROFILE_STATETRANSFORMATION_V_COMPUTATION);
142 
143  return s;
144  }
145 
146  /* --- COMMANDS ---------------------------------------------------------- */
147 
148  /* ------------------------------------------------------------------- */
149  /* --- ENTITY -------------------------------------------------------- */
150  /* ------------------------------------------------------------------- */
151 
152  void StateTransformation::display(std::ostream& os) const
153  {
154  os << "StateTransformation " << getName();
155  try
156  {
157  getProfiler().report_all(3, os);
158  }
159  catch (ExceptionSignal e) {}
160  }
161  } // namespace talos_balance
162  } // namespace sot
163 } // namespace dynamicgraph
164 
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