sot-talos-balance  1.6.0
ankle-joint-selector.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/all-commands.h>
22 #include <sot/core/stop-watch.hh>
23 
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 
36 #define INPUT_SIGNALS m_phaseSIN << m_rightRollCoupledSIN << m_rightRollDecoupledSIN << m_rightPitchCoupledSIN << m_rightPitchDecoupledSIN << m_leftRollCoupledSIN << m_leftRollDecoupledSIN << m_leftPitchCoupledSIN << m_leftPitchDecoupledSIN
37 
38 #define OUTPUT_SIGNALS m_selecLeftSOUT << m_selecRightSOUT << m_rightRollSOUT << m_rightPitchSOUT << m_leftRollSOUT << m_leftPitchSOUT
39 
42  typedef AnkleJointSelector EntityClassName;
43 
44  /* --- DG FACTORY ---------------------------------------------------- */
45  DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AnkleJointSelector,
46  "AnkleJointSelector");
47 
48  /* ------------------------------------------------------------------- */
49  /* --- CONSTRUCTION -------------------------------------------------- */
50  /* ------------------------------------------------------------------- */
51  AnkleJointSelector::AnkleJointSelector(const std::string& name)
52  : Entity(name)
53  , CONSTRUCT_SIGNAL_IN(phase, int)
54  , CONSTRUCT_SIGNAL_IN(rightRollCoupled, dynamicgraph::Vector)
55  , CONSTRUCT_SIGNAL_IN(rightRollDecoupled, dynamicgraph::Vector)
56  , CONSTRUCT_SIGNAL_IN(rightPitchCoupled, dynamicgraph::Vector)
57  , CONSTRUCT_SIGNAL_IN(rightPitchDecoupled, dynamicgraph::Vector)
58  , CONSTRUCT_SIGNAL_IN(leftRollCoupled, dynamicgraph::Vector)
59  , CONSTRUCT_SIGNAL_IN(leftRollDecoupled, dynamicgraph::Vector)
60  , CONSTRUCT_SIGNAL_IN(leftPitchCoupled, dynamicgraph::Vector)
61  , CONSTRUCT_SIGNAL_IN(leftPitchDecoupled, dynamicgraph::Vector)
62  , CONSTRUCT_SIGNAL_OUT(selecLeft, Flags, m_phaseSIN)
63  , CONSTRUCT_SIGNAL_OUT(selecRight, Flags, m_phaseSIN)
64  , CONSTRUCT_SIGNAL_OUT(rightRoll, dynamicgraph::Vector, m_phaseSIN << m_rightRollCoupledSIN << m_rightRollDecoupledSIN)
65  , CONSTRUCT_SIGNAL_OUT(rightPitch, dynamicgraph::Vector, m_phaseSIN << m_rightPitchCoupledSIN << m_rightPitchDecoupledSIN)
66  , CONSTRUCT_SIGNAL_OUT(leftRoll, dynamicgraph::Vector, m_phaseSIN << m_leftRollCoupledSIN << m_leftRollDecoupledSIN)
67  , CONSTRUCT_SIGNAL_OUT(leftPitch, dynamicgraph::Vector, m_phaseSIN << m_leftPitchCoupledSIN << m_leftPitchDecoupledSIN)
68  , m_zeros()
69  , m_ones(true)
70  , m_initSucceeded(false)
71  {
72  Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
73 
74  /* Commands. */
75  addCommand("init", makeCommandVoid1(*this, &AnkleJointSelector::init, docCommandVoid1("Initialize the entity.","Number of joints")));
76  }
77 
78  void AnkleJointSelector::init(const int & n)
79  {
80  m_n = n;
81  m_initSucceeded = true;
82  }
83 
84  /* ------------------------------------------------------------------- */
85  /* --- SIGNALS ------------------------------------------------------- */
86  /* ------------------------------------------------------------------- */
87 
88  DEFINE_SIGNAL_OUT_FUNCTION(selecLeft, Flags)
89  {
90  if(!m_initSucceeded)
91  {
92  SEND_WARNING_STREAM_MSG("Cannot compute selecLeft before initialization!");
93  return s;
94  }
95 
96  const int & phase = m_phaseSIN(iter);
97 
98  s = phase>=0 ? m_ones : m_zeros;
99 
100  return s;
101  }
102 
103  DEFINE_SIGNAL_OUT_FUNCTION(selecRight, Flags)
104  {
105  if(!m_initSucceeded)
106  {
107  SEND_WARNING_STREAM_MSG("Cannot compute selecRight before initialization!");
108  return s;
109  }
110 
111  const int & phase = m_phaseSIN(iter);
112 
113  s = phase<=0 ? m_ones : m_zeros;
114 
115  return s;
116  }
117 
119  {
120  if(!m_initSucceeded)
121  {
122  SEND_WARNING_STREAM_MSG("Cannot compute leftRoll before initialization!");
123  return s;
124  }
125  if(s.size()!=1)
126  s.resize(1);
127 
128  const int & phase = m_phaseSIN(iter);
129  const dg::Vector & leftRollCoupled = m_leftRollCoupledSIN(iter);
130  const dg::Vector & leftRollDecoupled = m_leftRollDecoupledSIN(iter);
131 
132  if(phase>0)
133  s = leftRollDecoupled;
134  else if(phase==0)
135  s = leftRollCoupled;
136  else
137  s.setZero();
138 
139  return s;
140  }
141 
143  {
144  if(!m_initSucceeded)
145  {
146  SEND_WARNING_STREAM_MSG("Cannot compute leftPitch before initialization!");
147  return s;
148  }
149  if(s.size()!=1)
150  s.resize(1);
151 
152  const int & phase = m_phaseSIN(iter);
153  const dg::Vector & leftPitchCoupled = m_leftPitchCoupledSIN(iter);
154  const dg::Vector & leftPitchDecoupled = m_leftPitchDecoupledSIN(iter);
155 
156  if(phase>0)
157  s = leftPitchDecoupled;
158  else if(phase==0)
159  s = leftPitchCoupled;
160  else
161  s.setZero();
162 
163  return s;
164  }
165 
167  {
168  if(!m_initSucceeded)
169  {
170  SEND_WARNING_STREAM_MSG("Cannot compute rightRoll before initialization!");
171  return s;
172  }
173  if(s.size()!=1)
174  s.resize(1);
175 
176  const int & phase = m_phaseSIN(iter);
177  const dg::Vector & rightRollCoupled = m_rightRollCoupledSIN(iter);
178  const dg::Vector & rightRollDecoupled = m_rightRollDecoupledSIN(iter);
179 
180  if(phase<0)
181  s = rightRollDecoupled;
182  else if(phase==0)
183  s = rightRollCoupled;
184  else
185  s.setZero();
186 
187  return s;
188  }
189 
191  {
192  if(!m_initSucceeded)
193  {
194  SEND_WARNING_STREAM_MSG("Cannot compute rightPitch before initialization!");
195  return s;
196  }
197  if(s.size()!=1)
198  s.resize(1);
199 
200  const int & phase = m_phaseSIN(iter);
201  const dg::Vector & rightPitchCoupled = m_rightPitchCoupledSIN(iter);
202  const dg::Vector & rightPitchDecoupled = m_rightPitchDecoupledSIN(iter);
203 
204  if(phase<0)
205  s = rightPitchDecoupled;
206  else if(phase==0)
207  s = rightPitchCoupled;
208  else
209  s.setZero();
210 
211  return s;
212  }
213 
214  /* --- COMMANDS ---------------------------------------------------------- */
215 
216  /* ------------------------------------------------------------------- */
217  /* --- ENTITY -------------------------------------------------------- */
218  /* ------------------------------------------------------------------- */
219 
220  void AnkleJointSelector::display(std::ostream& os) const
221  {
222  os << "AnkleJointSelector " << getName();
223  try
224  {
225  getProfiler().report_all(3, os);
226  }
227  catch (ExceptionSignal e) {}
228  }
229 
230  } // namespace talos_balance
231  } // namespace sot
232 } // namespace dynamicgraph
233 
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AnkleJointSelector(const std::string &name)
#define OUTPUT_SIGNALS
#define INPUT_SIGNALS