sot-talos-balance  1.6.0
ankle-admittance-controller.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 <sot/core/stop-watch.hh>
21 #include <dynamic-graph/factory.h>
22 #include <dynamic-graph/command-bind.h>
23 #include <dynamic-graph/all-commands.h>
24 
25 namespace dynamicgraph
26 {
27  namespace sot
28  {
29  namespace talos_balance
30  {
31  namespace dg = ::dynamicgraph;
32  using namespace dg;
33  using namespace dg::command;
34 
35 #define PROFILE_ANKLEADMITTANCECONTROLLER_DRP_COMPUTATION \
36 "AnkleAdmittanceController: dRP computation "
37 
38 #define INPUT_SIGNALS m_gainsXYSIN << m_wrenchSIN << m_pRefSIN
39 
40 #define OUTPUT_SIGNALS m_dRPSOUT << m_vDesSOUT
41 
44  typedef AnkleAdmittanceController EntityClassName;
45 
46  /* --- DG FACTORY ---------------------------------------------------- */
47  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AnkleAdmittanceController,
48  "AnkleAdmittanceController");
49 
50  /* ------------------------------------------------------------------- */
51  /* --- CONSTRUCTION -------------------------------------------------- */
52  /* ------------------------------------------------------------------- */
54  : Entity(name)
55  , CONSTRUCT_SIGNAL_IN(gainsXY, dynamicgraph::Vector)
56  , CONSTRUCT_SIGNAL_IN(wrench, dynamicgraph::Vector)
57  , CONSTRUCT_SIGNAL_IN(pRef, dynamicgraph::Vector)
58  , CONSTRUCT_SIGNAL_OUT(dRP, dynamicgraph::Vector, INPUT_SIGNALS)
59  , CONSTRUCT_SIGNAL_OUT(vDes, dynamicgraph::Vector, m_dRPSOUT)
60  , m_initSucceeded(false)
61  {
62  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
63 
64  /* Commands. */
65  addCommand("init", makeCommandVoid0(*this, &AnkleAdmittanceController::init, docCommandVoid0("Initialize the entity.")));
66  }
67 
69  {;
70  if(!m_gainsXYSIN.isPlugged())
71  return SEND_MSG("Init failed: signal gainsXY is not plugged", MSG_TYPE_ERROR);
72  if(!m_wrenchSIN.isPlugged())
73  return SEND_MSG("Init failed: signal wrench is not plugged", MSG_TYPE_ERROR);
74  if(!m_pRefSIN.isPlugged())
75  return SEND_MSG("Init failed: signal pRef is not plugged", MSG_TYPE_ERROR);
76 
77  m_initSucceeded = true;
78  }
79 
80  /* ------------------------------------------------------------------- */
81  /* --- SIGNALS ------------------------------------------------------- */
82  /* ------------------------------------------------------------------- */
83 
85  {
86  if(!m_initSucceeded)
87  {
88  SEND_WARNING_STREAM_MSG("Can't compute dqRef before initialization!");
89  return s;
90  }
91  if(s.size()!=2)
92  s.resize(2);
93 
95 
96  const Eigen::Vector3d pRef = m_pRefSIN(iter);
97  const Vector & wrench = m_wrenchSIN(iter);
98  const Vector & gainsXY = m_gainsXYSIN(iter);
99 
100  assert(pRef.size() == 3 && "Unexpected size of signal pRef");
101  assert(wrench.size() == 6 && "Unexpected size of signal wrench");
102  assert(gainsXY.size() == 2 && "Unexpected size of signal gainsXY");
103 
104  const Eigen::Vector3d error = wrench.tail<3>() - pRef.cross(wrench.head<3>());
105  Eigen::Vector2d dRP;
106 
107  dRP[0] = gainsXY[0] * error[0];
108  dRP[1] = gainsXY[1] * error[1];
109 
110  s = dRP;
111 
113 
114  return s;
115  }
116 
118  {
119  if(!m_initSucceeded)
120  {
121  SEND_WARNING_STREAM_MSG("Can't compute vDes before initialization!");
122  return s;
123  }
124  if(s.size()!=6)
125  s.resize(6);
126 
127  const Vector & dRP = m_dRPSOUT(iter);
128 
129  s.head<3>().setZero();
130  s.segment<2>(3) = dRP;
131  s[5] = 0;
132 
133  return s;
134  }
135 
136  /* --- COMMANDS ---------------------------------------------------------- */
137 
138  /* ------------------------------------------------------------------- */
139  /* --- ENTITY -------------------------------------------------------- */
140  /* ------------------------------------------------------------------- */
141 
142  void AnkleAdmittanceController::display(std::ostream& os) const
143  {
144  os << "AnkleAdmittanceController " << getName();
145  try
146  {
147  getProfiler().report_all(3, os);
148  }
149  catch (ExceptionSignal e) {}
150  }
151  } // namespace talos_balance
152  } // namespace sot
153 } // namespace dynamicgraph
154 
#define INPUT_SIGNALS
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
#define PROFILE_ANKLEADMITTANCECONTROLLER_DRP_COMPUTATION
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
#define OUTPUT_SIGNALS
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AnkleAdmittanceController(const std::string &name)