device.cpp 19.7 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
2
/*
 * Copyright 2010,
3
 * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
Francois Bleibel's avatar
Francois Bleibel committed
4
 *
5
 * CNRS
Francois Bleibel's avatar
Francois Bleibel committed
6
 *
Thomas Moulard's avatar
Thomas Moulard committed
7
 */
Francois Bleibel's avatar
Francois Bleibel committed
8
9
10
11
12
13

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* SOT */
Joseph Mirabel's avatar
Joseph Mirabel committed
14
15
#define ENABLE_RT_LOG

16
#include "sot/core/device.hh"
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
17
#include <iostream>
Francesco Morsillo's avatar
Francesco Morsillo committed
18
#include <sot/core/debug.hh>
Maximilien Naveau's avatar
Maximilien Naveau committed
19
#include <sot/core/macros.hh>
Francois Bleibel's avatar
Francois Bleibel committed
20
21
using namespace std;

22
#include <Eigen/Geometry>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
23
24
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/factory.h>
25
#include <dynamic-graph/linear-algebra.h>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
26
#include <dynamic-graph/real-time-logger.h>
27
#include <sot/core/matrix-geometry.hh>
Florent Lamiraux's avatar
Florent Lamiraux committed
28

29
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
30
using namespace dynamicgraph::sot;
Francois Bleibel's avatar
Francois Bleibel committed
31
32
using namespace dynamicgraph;

Florent Lamiraux's avatar
Florent Lamiraux committed
33
const std::string Device::CLASS_NAME = "Device";
Francois Bleibel's avatar
Francois Bleibel committed
34
35
36
37
38

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
39
40
void Device::integrateRollPitchYaw(Vector &state, const Vector &control,
                                   double dt) {
41
  using Eigen::AngleAxisd;
42
  using Eigen::QuaternionMapd;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
43
  using Eigen::Vector3d;
44

45
  typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
46
  Eigen::Matrix<double, 7, 1> qin, qout;
47
48
  qin.head<3>() = state.head<3>();

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
49
50
51
52
  QuaternionMapd quat(qin.tail<4>().data());
  quat = AngleAxisd(state(5), Vector3d::UnitZ()) *
         AngleAxisd(state(4), Vector3d::UnitY()) *
         AngleAxisd(state(3), Vector3d::UnitX());
53

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
54
  SE3().integrate(qin, control.head<6>() * dt, qout);
Joseph Mirabel's avatar
Joseph Mirabel committed
55
56
57

  // Update freeflyer pose
  ffPose_.translation() = qout.head<3>();
58
  state.head<3>() = qout.head<3>();
Joseph Mirabel's avatar
Joseph Mirabel committed
59

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
60
61
  ffPose_.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
  state.segment<3>(3) = ffPose_.linear().eulerAngles(2, 1, 0).reverse();
62
}
Francois Bleibel's avatar
Francois Bleibel committed
63

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
64
const MatrixHomogeneous &Device::freeFlyerPose() const { return ffPose_; }
65

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
66
67
Device::~Device() {
  for (unsigned int i = 0; i < 4; ++i) {
68
69
    delete forcesSOUT[i];
  }
70
71
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
Device::Device(const std::string &n)
    : Entity(n), state_(6), sanityCheck_(true),
      controlInputType_(CONTROL_INPUT_ONE_INTEGRATION),
      controlSIN(NULL, "Device(" + n + ")::input(double)::control"),
      attitudeSIN(NULL, "Device(" + n + ")::input(vector3)::attitudeIN"),
      zmpSIN(NULL, "Device(" + n + ")::input(vector3)::zmp"),
      stateSOUT("Device(" + n + ")::output(vector)::state")
      //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
      ,
      velocitySOUT("Device(" + n + ")::output(vector)::velocity"),
      attitudeSOUT("Device(" + n + ")::output(matrixRot)::attitude"),
      motorcontrolSOUT("Device(" + n + ")::output(vector)::motorcontrol"),
      previousControlSOUT("Device(" + n + ")::output(vector)::previousControl"),
      ZMPPreviousControllerSOUT("Device(" + n +
                                ")::output(vector)::zmppreviouscontroller"),
      robotState_("Device(" + n + ")::output(vector)::robotState"),
      robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity"),
      pseudoTorqueSOUT("Device(" + n + ")::output(vector)::ptorque")

      ,
      ffPose_(), forceZero6(6) {
  forceZero6.fill(0);
94
  /* --- SIGNALS --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
95
96
97
  for (int i = 0; i < 4; ++i) {
    withForceSignals[i] = false;
  }
Francois Bleibel's avatar
Francois Bleibel committed
98
  forcesSOUT[0] =
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
99
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRLEG");
Francois Bleibel's avatar
Francois Bleibel committed
100
  forcesSOUT[1] =
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
101
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLLEG");
Francois Bleibel's avatar
Francois Bleibel committed
102
  forcesSOUT[2] =
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
103
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRARM");
Francois Bleibel's avatar
Francois Bleibel committed
104
  forcesSOUT[3] =
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
105
      new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLARM");
Francois Bleibel's avatar
Francois Bleibel committed
106

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
107
108
109
110
111
112
113
114
  signalRegistration(
      controlSIN << stateSOUT << robotState_ << robotVelocity_ << velocitySOUT
                 << attitudeSOUT << attitudeSIN << zmpSIN << *forcesSOUT[0]
                 << *forcesSOUT[1] << *forcesSOUT[2] << *forcesSOUT[3]
                 << previousControlSOUT << pseudoTorqueSOUT << motorcontrolSOUT
                 << ZMPPreviousControllerSOUT);
  state_.fill(.0);
  stateSOUT.setConstant(state_);
115

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
116
117
118
  velocity_.resize(state_.size());
  velocity_.setZero();
  velocitySOUT.setConstant(velocity_);
119

120
  /* --- Commands --- */
121
122
123
  {
    std::string docstring;
    /* Command setStateSize. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
    docstring = "\n"
                "    Set size of state vector\n"
                "\n";
    addCommand("resize", new command::Setter<Device, unsigned int>(
                             *this, &Device::setStateSize, docstring));
    docstring = "\n"
                "    Set state vector value\n"
                "\n";
    addCommand("set", new command::Setter<Device, Vector>(
                          *this, &Device::setState, docstring));

    docstring = "\n"
                "    Set velocity vector value\n"
                "\n";
    addCommand("setVelocity", new command::Setter<Device, Vector>(
                                  *this, &Device::setVelocity, docstring));

    void (Device::*setRootPtr)(const Matrix &) = &Device::setRoot;
    docstring = command::docCommandVoid1("Set the root position.",
                                         "matrix homogeneous");
144
    addCommand("setRoot",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
145
               command::makeCommandVoid1(*this, setRootPtr, docstring));
146

147
    /* Second Order Integration set. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
148
149
150
151
152
153
154
155
    docstring = "\n"
                "    Set the position calculous starting from  \n"
                "    acceleration measure instead of velocity \n"
                "\n";

    addCommand("setSecondOrderIntegration",
               command::makeCommandVoid0(
                   *this, &Device::setSecondOrderIntegration, docstring));
156

Olivier Stasse's avatar
Olivier Stasse committed
157
    /* Display information */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
158
159
160
161
162
    docstring = "\n"
                "    Display information on device  \n"
                "\n";
    addCommand("display", command::makeCommandVoid0(*this, &Device::cmdDisplay,
                                                    docstring));
Olivier Stasse's avatar
Olivier Stasse committed
163

164
    /* SET of control input type. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
165
166
167
168
    docstring = "\n"
                "    Set the type of control input which can be  \n"
                "    acceleration, velocity, or position\n"
                "\n";
169
170

    addCommand("setControlInputType",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
171
172
               new command::Setter<Device, string>(
                   *this, &Device::setControlInputType, docstring));
173

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
174
175
176
    docstring = "\n"
                "    Enable/Disable sanity checks\n"
                "\n";
177
    addCommand("setSanityCheck",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
178
179
               new command::Setter<Device, bool>(*this, &Device::setSanityCheck,
                                                 docstring));
180
181

    addCommand("setPositionBounds",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
182
183
184
185
186
               command::makeCommandVoid2(
                   *this, &Device::setPositionBounds,
                   command::docCommandVoid2("Set robot position bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));
187
188

    addCommand("setVelocityBounds",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
189
190
191
192
193
               command::makeCommandVoid2(
                   *this, &Device::setVelocityBounds,
                   command::docCommandVoid2("Set robot velocity bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));
194
195

    addCommand("setTorqueBounds",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
196
197
198
199
200
               command::makeCommandVoid2(
                   *this, &Device::setTorqueBounds,
                   command::docCommandVoid2("Set robot torque bounds",
                                            "vector: lower bounds",
                                            "vector: upper bounds")));
201

202
    addCommand("getTimeStep",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
203
204
205
               command::makeDirectGetter(
                   *this, &this->timestep_,
                   command::docDirectGetter("Time step", "double")));
206
  }
Francois Bleibel's avatar
Francois Bleibel committed
207
208
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
209
210
211
212
213
214
215
void Device::setStateSize(const unsigned int &size) {
  state_.resize(size);
  state_.fill(.0);
  stateSOUT.setConstant(state_);
  previousControlSOUT.setConstant(state_);
  pseudoTorqueSOUT.setConstant(state_);
  motorcontrolSOUT.setConstant(state_);
florent's avatar
florent committed
216

217
218
  Device::setVelocitySize(size);

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
219
220
221
  Vector zmp(3);
  zmp.fill(.0);
  ZMPPreviousControllerSOUT.setConstant(zmp);
Francois Bleibel's avatar
Francois Bleibel committed
222
223
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
224
void Device::setVelocitySize(const unsigned int &size) {
225
226
  velocity_.resize(size);
  velocity_.fill(.0);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
227
  velocitySOUT.setConstant(velocity_);
228
229
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
230
void Device::setState(const Vector &st) {
231
  if (sanityCheck_) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
232
    const Vector::Index &s = st.size();
Maximilien Naveau's avatar
Maximilien Naveau committed
233
234
    SOT_CORE_DISABLE_WARNING_PUSH
    SOT_CORE_DISABLE_WARNING_FALLTHROUGH
235
    switch (controlInputType_) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
236
237
    case CONTROL_INPUT_TWO_INTEGRATION:
      dgRTLOG()
238
239
240
          << "Sanity check for this control is not well supported. "
             "In order to make it work, use pinocchio and the contact forces "
             "to estimate the joint torques for the given acceleration.\n";
Maximilien Naveau's avatar
Maximilien Naveau committed
241
242
243
244
245
246
247
248
249
      if (s != lowerTorque_.size() || s != upperTorque_.size()) {
        std::ostringstream os;
        os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
              "bounds do not match state size. Input State size = "
           << st.size() << ", lowerTorque_.size() = " << lowerTorque_.size()
           << ", upperTorque_.size() = " << upperTorque_.size()
           << ". Set them first with setTorqueBounds.";
        throw std::invalid_argument(os.str().c_str());
        // fall through
250
      }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
251
    case CONTROL_INPUT_ONE_INTEGRATION:
Maximilien Naveau's avatar
Maximilien Naveau committed
252
253
254
255
256
257
258
259
      if (s != lowerVelocity_.size() || s != upperVelocity_.size()) {
        std::ostringstream os;
        os << "dynamicgraph::sot::Device::setState: upper and/or lower velocity"
              " bounds do not match state size. Input State size = "
           << st.size() << ", lowerVelocity_.size() = " << lowerVelocity_.size()
           << ", upperVelocity_.size() = " << upperVelocity_.size()
           << ". Set them first with setVelocityBounds.";
        throw std::invalid_argument(os.str().c_str());
260
      }
261
      // fall through
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
262
263
264
265
    case CONTROL_INPUT_NO_INTEGRATION:
      break;
    default:
      throw std::invalid_argument("Invalid control mode type.");
266
    }
Maximilien Naveau's avatar
Maximilien Naveau committed
267
    SOT_CORE_DISABLE_WARNING_POP
268
  }
269
  state_ = st;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
270
271
  stateSOUT.setConstant(state_);
  motorcontrolSOUT.setConstant(state_);
Francois Bleibel's avatar
Francois Bleibel committed
272
273
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
274
void Device::setVelocity(const Vector &vel) {
275
  velocity_ = vel;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
276
  velocitySOUT.setConstant(velocity_);
277
278
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
279
void Device::setRoot(const Matrix &root) {
280
281
  Eigen::Matrix4d _matrix4d(root);
  MatrixHomogeneous _root(_matrix4d);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
282
  setRoot(_root);
283
}
284

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
285
286
void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
  VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2, 1, 0)).reverse();
287
288
  Vector q = state_;
  q = worldMwaist.translation(); // abusive ... but working.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
289
290
  for (unsigned int i = 0; i < 3; ++i)
    q(i + 3) = r(i);
291
292
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
293
void Device::setSecondOrderIntegration() {
Rohan Budhiraja's avatar
Rohan Budhiraja committed
294
295
296
  controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
  velocity_.resize(state_.size());
  velocity_.setZero();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
297
  velocitySOUT.setConstant(velocity_);
Rohan Budhiraja's avatar
Rohan Budhiraja committed
298
299
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
300
void Device::setNoIntegration() {
Rohan Budhiraja's avatar
Rohan Budhiraja committed
301
  controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
302
  velocity_.resize(state_.size());
Francesco Morsillo's avatar
Francesco Morsillo committed
303
  velocity_.setZero();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
304
  velocitySOUT.setConstant(velocity_);
305
306
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
307
308
309
void Device::setControlInputType(const std::string &cit) {
  for (int i = 0; i < CONTROL_INPUT_SIZE; i++)
    if (cit == ControlInput_s[i]) {
310
      controlInputType_ = (ControlInput)i;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
311
      sotDEBUG(25) << "Control input type: " << ControlInput_s[i] << endl;
312
313
      return;
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
314
  sotDEBUG(25) << "Unrecognized control input type: " << cit << endl;
315
316
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
317
void Device::setSanityCheck(const bool &enableCheck) {
318
  if (enableCheck) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
319
    const Vector::Index &s = state_.size();
Maximilien Naveau's avatar
Maximilien Naveau committed
320
321
    SOT_CORE_DISABLE_WARNING_PUSH
    SOT_CORE_DISABLE_WARNING_FALLTHROUGH
322
    switch (controlInputType_) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
323
324
    case CONTROL_INPUT_TWO_INTEGRATION:
      dgRTLOG()
325
326
327
          << "Sanity check for this control is not well supported. "
             "In order to make it work, use pinocchio and the contact forces "
             "to estimate the joint torques for the given acceleration.\n";
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
328
329
330
331
      if (s != lowerTorque_.size() || s != upperTorque_.size())
        throw std::invalid_argument(
            "Upper and/or lower torque bounds "
            "do not match state size. Set them first with setTorqueBounds");
332
      // fall through
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
333
334
335
336
337
    case CONTROL_INPUT_ONE_INTEGRATION:
      if (s != lowerVelocity_.size() || s != upperVelocity_.size())
        throw std::invalid_argument(
            "Upper and/or lower velocity bounds "
            "do not match state size. Set them first with setVelocityBounds");
338
      // fall through
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
339
340
341
342
343
344
345
346
    case CONTROL_INPUT_NO_INTEGRATION:
      if (s != lowerPosition_.size() || s != upperPosition_.size())
        throw std::invalid_argument(
            "Upper and/or lower position bounds "
            "do not match state size. Set them first with setPositionBounds");
      break;
    default:
      throw std::invalid_argument("Invalid control mode type.");
347
    }
Maximilien Naveau's avatar
Maximilien Naveau committed
348
    SOT_CORE_DISABLE_WARNING_POP
349
350
351
352
  }
  sanityCheck_ = enableCheck;
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
353
void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
354
355
  std::ostringstream oss;
  if (lower.size() != state_.size()) {
356
    oss << "Lower bound size should be " << state_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
357
        << lower.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
358
    throw std::invalid_argument(oss.str());
359
360
  }
  if (upper.size() != state_.size()) {
361
    oss << "Upper bound size should be " << state_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
362
        << upper.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
363
    throw std::invalid_argument(oss.str());
364
365
366
367
368
  }
  lowerPosition_ = lower;
  upperPosition_ = upper;
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
369
void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
370
371
  std::ostringstream oss;
  if (lower.size() != velocity_.size()) {
372
    oss << "Lower bound size should be " << velocity_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
373
        << lower.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
374
    throw std::invalid_argument(oss.str());
375
376
  }
  if (upper.size() != velocity_.size()) {
377
    oss << "Upper bound size should be " << velocity_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
378
        << upper.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
379
    throw std::invalid_argument(oss.str());
380
381
382
383
384
  }
  lowerVelocity_ = lower;
  upperVelocity_ = upper;
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
385
void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
386
387
388
  // TODO I think the torque bounds size are state_.size()-6...
  std::ostringstream oss;
  if (lower.size() != state_.size()) {
389
    oss << "Lower bound size should be " << state_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
390
        << lower.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
391
    throw std::invalid_argument(oss.str());
392
393
  }
  if (upper.size() != state_.size()) {
394
    oss << "Lower bound size should be " << state_.size() << ", got "
Maximilien Naveau's avatar
Maximilien Naveau committed
395
        << upper.size();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
396
    throw std::invalid_argument(oss.str());
397
398
399
400
401
  }
  lowerTorque_ = lower;
  upperTorque_ = upper;
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
402
void Device::increment(const double &dt) {
403
  int time = stateSOUT.getTime();
404
405
406
407
  sotDEBUG(25) << "Time : " << time << std::endl;

  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
408
409
410
411
412
413
414
415
416
417
418
  try {
    periodicCallBefore_.run(time + 1);
  } catch (std::exception &e) {
    dgRTLOG() << "exception caught while running periodical commands (before): "
              << e.what() << std::endl;
  } catch (const char *str) {
    dgRTLOG() << "exception caught while running periodical commands (before): "
              << str << std::endl;
  } catch (...) {
    dgRTLOG() << "unknown exception caught while"
              << " running periodical commands (before)" << std::endl;
419
  }
Francois Bleibel's avatar
Francois Bleibel committed
420

421
  /* Force the recomputation of the control. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
422
423
  controlSIN(time);
  sotDEBUG(25) << "u" << time << " = " << controlSIN.accessCopy() << endl;
Francois Bleibel's avatar
Francois Bleibel committed
424

425
  /* Integration of numerical values. This function is virtual. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
426
  integrate(dt);
427
428
  sotDEBUG(25) << "q" << time << " = " << state_ << endl;

429
  /* Position the signals corresponding to sensors. */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
430
431
432
433
434
435
436
437
438
  stateSOUT.setConstant(state_);
  stateSOUT.setTime(time + 1);
  // computation of the velocity signal
  if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
    velocitySOUT.setConstant(velocity_);
    velocitySOUT.setTime(time + 1);
  } else if (controlInputType_ == CONTROL_INPUT_ONE_INTEGRATION) {
    velocitySOUT.setConstant(controlSIN.accessCopy());
    velocitySOUT.setTime(time + 1);
439
  }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
440
441
442
  for (int i = 0; i < 4; ++i) {
    if (!withForceSignals[i])
      forcesSOUT[i]->setConstant(forceZero6);
443
  }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
444
445
446
  Vector zmp(3);
  zmp.fill(.0);
  ZMPPreviousControllerSOUT.setConstant(zmp);
447

448
449
  // Run Synchronous commands and evaluate signals outside the main
  // connected component of the graph.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
450
451
452
453
454
455
456
457
458
459
460
  try {
    periodicCallAfter_.run(time + 1);
  } catch (std::exception &e) {
    dgRTLOG() << "exception caught while running periodical commands (after): "
              << e.what() << std::endl;
  } catch (const char *str) {
    dgRTLOG() << "exception caught while running periodical commands (after): "
              << str << std::endl;
  } catch (...) {
    dgRTLOG() << "unknown exception caught while"
              << " running periodical commands (after)" << std::endl;
461
  }
462

463
  // Others signals.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
464
  motorcontrolSOUT.setConstant(state_);
465
466
}

467
// Return true if it saturates.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
468
469
470
471
472
473
474
475
476
477
478
inline bool saturateBounds(double &val, const double &lower,
                           const double &upper) {
  assert(lower <= upper);
  if (val < lower) {
    val = lower;
    return true;
  }
  if (upper < val) {
    val = upper;
    return true;
  }
479
480
481
  return false;
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
482
483
484
485
486
487
488
489
490
#define CHECK_BOUNDS(val, lower, upper, what)                                  \
  for (int i = 0; i < val.size(); ++i) {                                       \
    double old = val(i);                                                       \
    if (saturateBounds(val(i), lower(i), upper(i))) {                          \
      std::ostringstream oss;                                                  \
      oss << "Robot " what " bound violation at DoF " << i << ": requested "   \
          << old << " but set " << val(i) << '\n';                             \
      SEND_ERROR_STREAM_MSG(oss.str());                                        \
    }                                                                          \
491
492
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
493
494
void Device::integrate(const double &dt) {
  const Vector &controlIN = controlSIN.accessCopy();
495

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
496
497
498
  if (sanityCheck_ && controlIN.hasNaN()) {
    dgRTLOG() << "Device::integrate: Control has NaN values: " << '\n'
              << controlIN.transpose() << '\n';
499
500
501
    return;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
502
  if (controlInputType_ == CONTROL_INPUT_NO_INTEGRATION) {
Joseph Mirabel's avatar
Joseph Mirabel committed
503
    state_.tail(controlIN.size()) = controlIN;
504
505
506
    return;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
507
  if (vel_control_.size() == 0)
508
    vel_control_ = Vector::Zero(controlIN.size());
509

510
511
512
  // If control size is state size - 6, integrate joint angles,
  // if control and state are of same size, integrate 6 first degrees of
  // freedom as a translation and roll pitch yaw.
513

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
514
  if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
515
    // TODO check acceleration
Joseph Mirabel's avatar
Joseph Mirabel committed
516
    // Position increment
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
517
    vel_control_ = velocity_.tail(controlIN.size()) + (0.5 * dt) * controlIN;
Joseph Mirabel's avatar
Joseph Mirabel committed
518
    // Velocity integration.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
519
520
    velocity_.tail(controlIN.size()) += controlIN * dt;
  } else {
Rohan Budhiraja's avatar
Rohan Budhiraja committed
521
    vel_control_ = controlIN;
522
  }
523

524
525
526
527
528
  // Velocity bounds check
  if (sanityCheck_) {
    CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity");
  }

529
  if (vel_control_.size() == state_.size()) {
530
    // Freeflyer integration
531
    integrateRollPitchYaw(state_, vel_control_, dt);
532
    // Joints integration
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
533
534
    state_.tail(state_.size() - 6) += vel_control_.tail(state_.size() - 6) * dt;
  } else {
535
536
    // Position integration
    state_.tail(controlIN.size()) += vel_control_ * dt;
537
  }
538
539
540
541
542

  // Position bounds check
  if (sanityCheck_) {
    CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
  }
Francois Bleibel's avatar
Francois Bleibel committed
543
544
545
546
}

/* --- DISPLAY ------------------------------------------------------------ */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
547
548
549
void Device::display(std::ostream &os) const {
  os << name << ": " << state_ << endl
     << "sanityCheck: " << sanityCheck_ << endl
550
551
     << "controlInputType:" << controlInputType_ << endl;
}
Olivier Stasse's avatar
Olivier Stasse committed
552

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
553
554
555
556
void Device::cmdDisplay() {
  std::cout << name << ": " << state_ << endl
            << "sanityCheck: " << sanityCheck_ << endl
            << "controlInputType:" << controlInputType_ << endl;
Olivier Stasse's avatar
Olivier Stasse committed
557
}