sot-talos-balance  1.5.0
simple-pidd.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_SIMPLE_PIDD_DDX_REF_COMPUTATION "SimplePIDD: ddx_ref computation "
38 
39 #define INPUT_SIGNALS m_KpSIN << m_KiSIN << m_KdSIN << m_decayFactorSIN << m_xSIN << m_x_desSIN << m_dxSIN << m_dx_desSIN << m_ddx_desSIN
40 
41 #define OUTPUT_SIGNALS m_ddx_refSOUT << m_dx_refSOUT
42 
45  typedef SimplePIDD EntityClassName;
46 
47  /* --- DG FACTORY ---------------------------------------------------- */
49  "SimplePIDD");
50 
51  /* ------------------------------------------------------------------- */
52  /* --- CONSTRUCTION -------------------------------------------------- */
53  /* ------------------------------------------------------------------- */
54  SimplePIDD::SimplePIDD(const std::string& name)
55  : Entity(name)
56  , CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
57  , CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector)
58  , CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector)
59  , CONSTRUCT_SIGNAL_IN(decayFactor, double)
60  , CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector)
61  , CONSTRUCT_SIGNAL_IN(x_des, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_IN(dx, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_IN(dx_des, dynamicgraph::Vector)
64  , CONSTRUCT_SIGNAL_IN(ddx_des, dynamicgraph::Vector)
65  , CONSTRUCT_SIGNAL_OUT(ddx_ref, dynamicgraph::Vector, INPUT_SIGNALS)
66  , CONSTRUCT_SIGNAL_OUT(dx_ref, dynamicgraph::Vector, m_ddx_refSOUT)
67  , m_initSucceeded(false)
68  {
69  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
70 
71  /* Commands. */
72  addCommand("init", makeCommandVoid2(*this, &SimplePIDD::init, docCommandVoid2("Initialize the entity.","time step","number of elements")));
73  addCommand("resetIntegralError", makeCommandVoid0(*this, &SimplePIDD::resetIntegralError, docCommandVoid0("Set integral error to zero.")));
74  addCommand("resetVelocity", makeCommandVoid0(*this, &SimplePIDD::resetIntegralError, docCommandVoid0("Set reference velocity to zero.")));
75  }
76 
77  void SimplePIDD::init(const double & dt, const int & N)
78  {
79  m_dt = dt;
80  m_integralError.setZero(N);
81  m_dx_ref.setZero(N);
82  m_initSucceeded = true;
83  }
84 
86  {
87  m_dx_ref.setZero();
88  }
89 
91  {
92  m_integralError.setZero();
93  }
94 
95  /* ------------------------------------------------------------------- */
96  /* --- SIGNALS ------------------------------------------------------- */
97  /* ------------------------------------------------------------------- */
98 
100  {
101  if(!m_initSucceeded)
102  {
103  SEND_WARNING_STREAM_MSG("Cannot compute signal ddx_ref before initialization!");
104  return s;
105  }
106 
107  getProfiler().start(PROFILE_SIMPLE_PIDD_DDX_REF_COMPUTATION);
108 
109 
110  const Vector & Kp = m_KpSIN(iter);
111  const Vector & Ki = m_KiSIN(iter);
112  const Vector & Kd = m_KiSIN(iter);
113  const double & decayFactor = m_decayFactorSIN(iter);
114 
115  const Vector & x = m_xSIN(iter);
116  const Vector & x_des = m_x_desSIN(iter);
117 
118  const Vector & dx = m_dxSIN(iter);
119  const Vector & dx_des = m_dx_desSIN(iter);
120 
121  const Vector & ddx_des = m_ddx_desSIN(iter);
122 
123  Vector x_err = x_des - x;
124  Vector dx_err = dx_des - dx;
125 
126  Vector ddx_ref = ddx_des + Kd.cwiseProduct(dx_err) + Kp.cwiseProduct(x_err) + Ki.cwiseProduct(m_integralError);
127 
128  // update the integrator (AFTER using its value)
129  m_integralError += ( x_err - decayFactor*m_integralError ) * m_dt;
130 
131  s = ddx_ref;
132 
133  getProfiler().stop(PROFILE_SIMPLE_PIDD_DDX_REF_COMPUTATION);
134 
135  return s;
136  }
137 
139  {
140  if(!m_initSucceeded)
141  {
142  SEND_WARNING_STREAM_MSG("Cannot compute signal dx_ref before initialization!");
143  return s;
144  }
145 
146  const Vector & ddx_ref = m_ddx_refSOUT(iter);
147 
148  m_dx_ref += ddx_ref * m_dt;
149 
150  s = m_dx_ref;
151 
152  return s;
153  }
154 
155  /* --- COMMANDS ---------------------------------------------------------- */
156 
157  /* ------------------------------------------------------------------- */
158  /* --- ENTITY -------------------------------------------------------- */
159  /* ------------------------------------------------------------------- */
160 
161  void SimplePIDD::display(std::ostream& os) const
162  {
163  os << "SimplePIDD " << getName();
164  try
165  {
166  getProfiler().report_all(3, os);
167  }
168  catch (ExceptionSignal e) {}
169  }
170  } // namespace talos_balance
171  } // namespace sot
172 } // namespace dynamicgraph
173 
void init(const double &dt, const int &N)
Definition: simple-pidd.cpp:77
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
virtual void display(std::ostream &os) const
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
dynamicgraph::Vector m_dx_ref
true if the entity has been successfully initialized
Definition: simple-pidd.hh:91
#define PROFILE_SIMPLE_PIDD_DDX_REF_COMPUTATION
Definition: simple-pidd.cpp:37
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SimplePIDD(const std::string &name)
Definition: simple-pidd.cpp:54
#define INPUT_SIGNALS
Definition: simple-pidd.cpp:39
#define OUTPUT_SIGNALS
Definition: simple-pidd.cpp:41