sot-talos-balance  1.7.0
saturation.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 "-------------------------------------------------------"
38 
39 #define PROFILE_SATURATION_SOUT_COMPUTATION "Saturation: sOut computation"
40 
41 #define INPUT_SIGNALS m_xSIN << m_ySIN << m_kSIN << m_xLimSIN << m_yLimSIN
42 
43 #define OUTPUT_SIGNALS m_yOutSOUT
44 
47 typedef Saturation EntityClassName;
48 
49 /* --- DG FACTORY ---------------------------------------------------- */
51  "Saturation");
52 
53 /* ------------------------------------------------------------------- */
54 /* --- CONSTRUCTION -------------------------------------------------- */
55 /* ------------------------------------------------------------------- */
56 Saturation::Saturation(const std::string &name)
57  : Entity(name)
58  , CONSTRUCT_SIGNAL_IN(x, dynamicgraph::Vector)
59  , CONSTRUCT_SIGNAL_IN(y, dynamicgraph::Vector)
60  , CONSTRUCT_SIGNAL_IN(k, double)
61  , CONSTRUCT_SIGNAL_IN(xLim, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_IN(yLim, dynamicgraph::Vector)
63  , CONSTRUCT_SIGNAL_OUT(yOut, dynamicgraph::Vector, INPUT_SIGNALS)
64 {
65  Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
66 }
67 
68 /* ------------------------------------------------------------------- */
69 /* --- SIGNALS ------------------------------------------------------- */
70 /* ------------------------------------------------------------------- */
71 
73 {
74  getProfiler().start(PROFILE_SATURATION_SOUT_COMPUTATION);
75 
76  const double &x = m_xSIN(iter)[0];
77  s = m_ySIN(iter);
78  const double &y = s[0];
79  const double &k = m_kSIN(iter);
80  const double &xLim = m_xLimSIN(iter)[0];
81  const double &yLim = m_yLimSIN(iter)[0];
82 
83  double r = y;
84 
85  assert(k > 0 && "k must be strictly positive");
86  assert(xLim > 0 && "xLim must be strictly positive");
87  assert(yLim > 0 && "yLim must be strictly positive");
88 
89  if ((x <= -xLim) or (x > xLim))
90  {
91  r = 0.0;
92  }
93  else if (-xLim + yLim / k < x and x <= xLim - yLim / k)
94  {
95  r = std::min(std::max(y, -yLim), yLim);
96  }
97  else if (-xLim < x and x <= -xLim + yLim / k)
98  {
99  r = std::min(std::max(y, -k * (x + xLim)), k * (x + xLim));
100  }
101  else if (xLim - yLim / k < x and x <= xLim)
102  {
103  r = std::min(std::max(y, -yLim + k * (x - xLim + yLim / k)), yLim - k * (x - xLim + yLim / k));
104  }
105 
106  s[0] = r;
107 
108  getProfiler().stop(PROFILE_SATURATION_SOUT_COMPUTATION);
109 
110  return s;
111 }
112 
113 /* --- COMMANDS ---------------------------------------------------------- */
114 
115 /* ------------------------------------------------------------------- */
116 /* --- ENTITY -------------------------------------------------------- */
117 /* ------------------------------------------------------------------- */
118 
119 void Saturation::display(std::ostream &os) const
120 {
121  os << "Saturation " << getName();
122  try
123  {
124  getProfiler().report_all(3, os);
125  }
126  catch (ExceptionSignal e)
127  {
128  }
129 }
130 } // namespace talos_balance
131 } // namespace sot
132 } // namespace dynamicgraph
#define INPUT_SIGNALS
Definition: saturation.cpp:41
#define OUTPUT_SIGNALS
Definition: saturation.cpp:43
#define PROFILE_SATURATION_SOUT_COMPUTATION
Definition: saturation.cpp:39
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Saturation(const std::string &name)
Definition: saturation.cpp:56
virtual void display(std::ostream &os) const
Definition: saturation.cpp:119