@@ -35,6 +35,36 @@ using namespace dynamicgraph;
35
35
36
36
const std::string Device::CLASS_NAME = " Device" ;
37
37
38
+ // Return positive difference between input value and bounds if it saturates,
39
+ // 0 if it does not saturate
40
+ inline double saturateBounds (double &val, const double &lower,
41
+ const double &upper) {
42
+ double res = 0 ;
43
+ assert (lower <= upper);
44
+ if (val < lower) {
45
+ res = lower - val;
46
+ val = lower;
47
+ return res;
48
+ }
49
+ if (upper < val) {
50
+ res = val - upper;
51
+ val = upper;
52
+ return res;
53
+ }
54
+ return res;
55
+ }
56
+
57
+ #define CHECK_BOUNDS (val, lower, upper, what, eps ) \
58
+ for (int i = 0 ; i < val.size(); ++i) { \
59
+ double old = val (i); \
60
+ if (saturateBounds (val (i), lower (i), upper (i)) > eps) { \
61
+ std::ostringstream oss; \
62
+ oss << " Robot " what " bound violation at DoF " << i << " : requested " \
63
+ << old << " but set " << val (i) << ' \n ' ; \
64
+ SEND_ERROR_STREAM_MSG (oss.str ()); \
65
+ } \
66
+ }
67
+
38
68
/* --------------------------------------------------------------------- */
39
69
/* --- CLASS ----------------------------------------------------------- */
40
70
/* --------------------------------------------------------------------- */
@@ -76,7 +106,7 @@ Device::Device(const std::string &n)
76
106
: Entity(n),
77
107
state_(6 ),
78
108
sanityCheck_(true ),
79
- controlInputType_(CONTROL_INPUT_ONE_INTEGRATION ),
109
+ controlInputType_(POSITION_CONTROL ),
80
110
controlSIN(NULL , " Device(" + n + " )::input(double)::control" ),
81
111
attitudeSIN(NULL , " Device(" + n + " )::input(vector3)::attitudeIN" ),
82
112
zmpSIN(NULL , " Device(" + n + " )::input(vector3)::zmp" ),
@@ -226,10 +256,13 @@ void Device::getControl(map<string,ControlValues> &controlOut,
226
256
std::vector<double > control;
227
257
lastTimeControlWasRead_ += (int )floor (period/Integrator::dt);
228
258
229
- const Vector& dgControl (controlSIN (lastTimeControlWasRead_));
259
+ Vector dgControl (controlSIN (lastTimeControlWasRead_));
230
260
// Specify the joint values for the controller.
231
261
control.resize (dgControl.size ());
232
262
263
+ if (controlInputType_ == POSITION_CONTROL){
264
+ CHECK_BOUNDS (dgControl, lowerPosition_, upperPosition_, " position" , 1e-6 );
265
+ }
233
266
for (unsigned int i=0 ; i < dgControl.size ();++i)
234
267
control[i] = dgControl[i];
235
268
controlOut[" control" ].setValues (control);
@@ -267,49 +300,6 @@ void Device::setVelocitySize(const unsigned int &size) {
267
300
}
268
301
269
302
void Device::setState (const Vector &st) {
270
- if (sanityCheck_) {
271
- const Vector::Index &s = st.size ();
272
- SOT_CORE_DISABLE_WARNING_PUSH
273
- SOT_CORE_DISABLE_WARNING_FALLTHROUGH
274
- switch (controlInputType_) {
275
- case CONTROL_INPUT_TWO_INTEGRATION:
276
- dgRTLOG ()
277
- << " Sanity check for this control is not well supported. "
278
- " In order to make it work, use pinocchio and the contact forces "
279
- " to estimate the joint torques for the given acceleration.\n " ;
280
- if (s != lowerTorque_.size () || s != upperTorque_.size ()) {
281
- std::ostringstream os;
282
- os << " dynamicgraph::sot::Device::setState: upper and/or lower torque"
283
- " bounds do not match state size. Input State size = "
284
- << st.size () << " , lowerTorque_.size() = " << lowerTorque_.size ()
285
- << " , upperTorque_.size() = " << upperTorque_.size ()
286
- << " . Set them first with setTorqueBounds." ;
287
- throw std::invalid_argument (os.str ().c_str ());
288
- // fall through
289
- }
290
- case CONTROL_INPUT_ONE_INTEGRATION:
291
- if (s > lowerVelocity_.size () || s > upperVelocity_.size ()) {
292
- std::ostringstream os;
293
- os << " dynamicgraph::sot::Device::setState: upper and/or lower "
294
- " velocity"
295
- " bounds should be less than state size. Input State size = "
296
- << st.size ()
297
- << " , lowerVelocity_.size() = " << lowerVelocity_.size ()
298
- << " , upperVelocity_.size() = " << upperVelocity_.size ()
299
- << " . Set them first with setVelocityBounds." ;
300
- throw std::invalid_argument (os.str ().c_str ());
301
- }
302
- // fall through
303
- case CONTROL_INPUT_NO_INTEGRATION:
304
- break ;
305
- default :
306
- throw std::invalid_argument (" Invalid control mode type." );
307
- }
308
- SOT_CORE_DISABLE_WARNING_POP
309
- }
310
- state_ = st;
311
- stateSOUT.setConstant (state_);
312
- motorcontrolSOUT.setConstant (state_);
313
303
}
314
304
315
305
void Device::setVelocity (const Vector &vel) {
@@ -331,74 +321,26 @@ void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
331
321
}
332
322
333
323
void Device::setSecondOrderIntegration () {
334
- controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
335
- velocity_.resize (state_.size ());
336
- velocity_.setZero ();
337
- velocitySOUT.setConstant (velocity_);
338
324
}
339
325
340
326
void Device::setNoIntegration () {
341
- controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
342
- velocity_.resize (state_.size ());
343
- velocity_.setZero ();
344
- velocitySOUT.setConstant (velocity_);
345
327
}
346
328
347
329
void Device::setControlInputType (const std::string &cit) {
348
- for (int i = 0 ; i < CONTROL_INPUT_SIZE; i++)
349
- if (cit == ControlInput_s[i]) {
350
- controlInputType_ = (ControlInput)i;
351
- sotDEBUG (25 ) << " Control input type: " << ControlInput_s[i] << endl;
352
- return ;
353
- }
354
- sotDEBUG (25 ) << " Unrecognized control input type: " << cit << endl;
355
330
}
356
331
357
332
void Device::setSanityCheck (const bool &enableCheck) {
358
- if (enableCheck) {
359
- const Vector::Index &s = state_.size ();
360
- SOT_CORE_DISABLE_WARNING_PUSH
361
- SOT_CORE_DISABLE_WARNING_FALLTHROUGH
362
- switch (controlInputType_) {
363
- case CONTROL_INPUT_TWO_INTEGRATION:
364
- dgRTLOG ()
365
- << " Sanity check for this control is not well supported. "
366
- " In order to make it work, use pinocchio and the contact forces "
367
- " to estimate the joint torques for the given acceleration.\n " ;
368
- if (s != lowerTorque_.size () || s != upperTorque_.size ())
369
- throw std::invalid_argument (
370
- " Upper and/or lower torque bounds "
371
- " do not match state size. Set them first with setTorqueBounds" );
372
- // fall through
373
- case CONTROL_INPUT_ONE_INTEGRATION:
374
- if (s != lowerVelocity_.size () || s != upperVelocity_.size ())
375
- throw std::invalid_argument (
376
- " Upper and/or lower velocity bounds "
377
- " do not match state size. Set them first with setVelocityBounds" );
378
- // fall through
379
- case CONTROL_INPUT_NO_INTEGRATION:
380
- if (s != lowerPosition_.size () || s != upperPosition_.size ())
381
- throw std::invalid_argument (
382
- " Upper and/or lower position bounds "
383
- " do not match state size. Set them first with setPositionBounds" );
384
- break ;
385
- default :
386
- throw std::invalid_argument (" Invalid control mode type." );
387
- }
388
- SOT_CORE_DISABLE_WARNING_POP
389
- }
390
- sanityCheck_ = enableCheck;
391
333
}
392
334
393
335
void Device::setPositionBounds (const Vector &lower, const Vector &upper) {
394
336
std::ostringstream oss;
395
- if (lower.size () != state_. size () ) {
396
- oss << " Lower bound size should be " << state_. size () << " , got "
337
+ if (lower.size () != controlSize_ ) {
338
+ oss << " Lower bound size should be " << controlSize_ << " , got "
397
339
<< lower.size ();
398
340
throw std::invalid_argument (oss.str ());
399
341
}
400
- if (upper.size () != state_. size () ) {
401
- oss << " Upper bound size should be " << state_. size () << " , got "
342
+ if (upper.size () != controlSize_ ) {
343
+ oss << " Upper bound size should be " << controlSize_ << " , got "
402
344
<< upper.size ();
403
345
throw std::invalid_argument (oss.str ());
404
346
}
@@ -408,13 +350,13 @@ void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
408
350
409
351
void Device::setVelocityBounds (const Vector &lower, const Vector &upper) {
410
352
std::ostringstream oss;
411
- if (lower.size () != velocity_. size () ) {
412
- oss << " Lower bound size should be " << velocity_. size () << " , got "
353
+ if (lower.size () != controlSize_ ) {
354
+ oss << " Lower bound size should be " << controlSize_ << " , got "
413
355
<< lower.size ();
414
356
throw std::invalid_argument (oss.str ());
415
357
}
416
- if (upper.size () != velocity_. size () ) {
417
- oss << " Upper bound size should be " << velocity_. size () << " , got "
358
+ if (upper.size () != controlSize_ ) {
359
+ oss << " Upper bound size should be " << controlSize_ << " , got "
418
360
<< upper.size ();
419
361
throw std::invalid_argument (oss.str ());
420
362
}
@@ -423,15 +365,15 @@ void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
423
365
}
424
366
425
367
void Device::setTorqueBounds (const Vector &lower, const Vector &upper) {
426
- // TODO I think the torque bounds size are state_.size() -6...
368
+ // TODO I think the torque bounds size are controlSize_ -6...
427
369
std::ostringstream oss;
428
- if (lower.size () != state_. size () ) {
429
- oss << " Lower bound size should be " << state_. size () << " , got "
370
+ if (lower.size () != controlSize_ ) {
371
+ oss << " Lower bound size should be " << controlSize_ << " , got "
430
372
<< lower.size ();
431
373
throw std::invalid_argument (oss.str ());
432
374
}
433
- if (upper.size () != state_. size () ) {
434
- oss << " Lower bound size should be " << state_. size () << " , got "
375
+ if (upper.size () != controlSize_ ) {
376
+ oss << " Lower bound size should be " << controlSize_ << " , got "
435
377
<< upper.size ();
436
378
throw std::invalid_argument (oss.str ());
437
379
}
@@ -440,85 +382,7 @@ void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
440
382
}
441
383
442
384
443
- // Return positive difference between input value and bounds if it saturates,
444
- // 0 if it does not saturate
445
- inline double saturateBounds (double &val, const double &lower,
446
- const double &upper) {
447
- double res = 0 ;
448
- assert (lower <= upper);
449
- if (val < lower) {
450
- res = lower - val;
451
- val = lower;
452
- return res;
453
- }
454
- if (upper < val) {
455
- res = val - upper;
456
- val = upper;
457
- return res;
458
- }
459
- return res;
460
- }
461
-
462
- #define CHECK_BOUNDS (val, lower, upper, what, eps ) \
463
- for (int i = 0 ; i < val.size(); ++i) { \
464
- double old = val (i); \
465
- if (saturateBounds (val (i), lower (i), upper (i)) > eps) { \
466
- std::ostringstream oss; \
467
- oss << " Robot " what " bound violation at DoF " << i << " : requested " \
468
- << old << " but set " << val (i) << ' \n ' ; \
469
- SEND_ERROR_STREAM_MSG (oss.str ()); \
470
- } \
471
- }
472
-
473
385
void Device::integrate (const double &dt) {
474
- const Vector &controlIN = controlSIN.accessCopy ();
475
-
476
- if (sanityCheck_ && controlIN.hasNaN ()) {
477
- dgRTLOG () << " Device::integrate: Control has NaN values: " << ' \n '
478
- << controlIN.transpose () << ' \n ' ;
479
- return ;
480
- }
481
-
482
- if (controlInputType_ == CONTROL_INPUT_NO_INTEGRATION) {
483
- state_.tail (controlIN.size ()) = controlIN;
484
- return ;
485
- }
486
-
487
- if (vel_control_.size () == 0 ) vel_control_ = Vector::Zero (controlIN.size ());
488
-
489
- // If control size is state size - 6, integrate joint angles,
490
- // if control and state are of same size, integrate 6 first degrees of
491
- // freedom as a translation and roll pitch yaw.
492
-
493
- if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
494
- // TODO check acceleration
495
- // Position increment
496
- vel_control_ = velocity_.tail (controlIN.size ()) + (0.5 * dt) * controlIN;
497
- // Velocity integration.
498
- velocity_.tail (controlIN.size ()) += controlIN * dt;
499
- } else {
500
- vel_control_ = controlIN;
501
- }
502
-
503
- // Velocity bounds check
504
- if (sanityCheck_) {
505
- CHECK_BOUNDS (velocity_, lowerVelocity_, upperVelocity_, " velocity" , 1e-6 );
506
- }
507
-
508
- if (vel_control_.size () == state_.size ()) {
509
- // Freeflyer integration
510
- integrateRollPitchYaw (state_, vel_control_, dt);
511
- // Joints integration
512
- state_.tail (state_.size () - 6 ) += vel_control_.tail (state_.size () - 6 ) * dt;
513
- } else {
514
- // Position integration
515
- state_.tail (controlIN.size ()) += vel_control_ * dt;
516
- }
517
-
518
- // Position bounds check
519
- if (sanityCheck_) {
520
- CHECK_BOUNDS (state_, lowerPosition_, upperPosition_, " position" , 1e-6 );
521
- }
522
386
}
523
387
524
388
/* --- DISPLAY ------------------------------------------------------------ */
0 commit comments