Skip to content

Commit f8183fa

Browse files
[Device] Fix bound setting.
1 parent c761031 commit f8183fa

File tree

2 files changed

+52
-188
lines changed

2 files changed

+52
-188
lines changed

include/sot/core/device.hh

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,10 +32,10 @@ namespace sot {
3232

3333
/// Define the type of input expected by the robot
3434
enum ControlInput {
35-
CONTROL_INPUT_NO_INTEGRATION = 0,
36-
CONTROL_INPUT_ONE_INTEGRATION = 1,
37-
CONTROL_INPUT_TWO_INTEGRATION = 2,
38-
CONTROL_INPUT_SIZE = 3
35+
POSITION_CONTROL = 0,
36+
VELOCITY_CONTROL = 1,
37+
TORQUE_CONTROL = 2,
38+
CONTROL_SIZE = 3
3939
};
4040

4141
const std::string ControlInput_s[] = {"noInteg", "oneInteg", "twoInteg"};

src/tools/device.cpp

Lines changed: 48 additions & 184 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,36 @@ using namespace dynamicgraph;
3535

3636
const std::string Device::CLASS_NAME = "Device";
3737

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+
3868
/* --------------------------------------------------------------------- */
3969
/* --- CLASS ----------------------------------------------------------- */
4070
/* --------------------------------------------------------------------- */
@@ -76,7 +106,7 @@ Device::Device(const std::string &n)
76106
: Entity(n),
77107
state_(6),
78108
sanityCheck_(true),
79-
controlInputType_(CONTROL_INPUT_ONE_INTEGRATION),
109+
controlInputType_(POSITION_CONTROL),
80110
controlSIN(NULL, "Device(" + n + ")::input(double)::control"),
81111
attitudeSIN(NULL, "Device(" + n + ")::input(vector3)::attitudeIN"),
82112
zmpSIN(NULL, "Device(" + n + ")::input(vector3)::zmp"),
@@ -226,10 +256,13 @@ void Device::getControl(map<string,ControlValues> &controlOut,
226256
std::vector<double> control;
227257
lastTimeControlWasRead_ += (int)floor(period/Integrator::dt);
228258

229-
const Vector& dgControl(controlSIN(lastTimeControlWasRead_));
259+
Vector dgControl(controlSIN(lastTimeControlWasRead_));
230260
// Specify the joint values for the controller.
231261
control.resize(dgControl.size());
232262

263+
if (controlInputType_ == POSITION_CONTROL){
264+
CHECK_BOUNDS(dgControl, lowerPosition_, upperPosition_, "position", 1e-6);
265+
}
233266
for(unsigned int i=0; i < dgControl.size();++i)
234267
control[i] = dgControl[i];
235268
controlOut["control"].setValues(control);
@@ -267,49 +300,6 @@ void Device::setVelocitySize(const unsigned int &size) {
267300
}
268301

269302
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_);
313303
}
314304

315305
void Device::setVelocity(const Vector &vel) {
@@ -331,74 +321,26 @@ void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
331321
}
332322

333323
void Device::setSecondOrderIntegration() {
334-
controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
335-
velocity_.resize(state_.size());
336-
velocity_.setZero();
337-
velocitySOUT.setConstant(velocity_);
338324
}
339325

340326
void Device::setNoIntegration() {
341-
controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
342-
velocity_.resize(state_.size());
343-
velocity_.setZero();
344-
velocitySOUT.setConstant(velocity_);
345327
}
346328

347329
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;
355330
}
356331

357332
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;
391333
}
392334

393335
void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
394336
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 "
397339
<< lower.size();
398340
throw std::invalid_argument(oss.str());
399341
}
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 "
402344
<< upper.size();
403345
throw std::invalid_argument(oss.str());
404346
}
@@ -408,13 +350,13 @@ void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
408350

409351
void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
410352
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 "
413355
<< lower.size();
414356
throw std::invalid_argument(oss.str());
415357
}
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 "
418360
<< upper.size();
419361
throw std::invalid_argument(oss.str());
420362
}
@@ -423,15 +365,15 @@ void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
423365
}
424366

425367
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...
427369
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 "
430372
<< lower.size();
431373
throw std::invalid_argument(oss.str());
432374
}
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 "
435377
<< upper.size();
436378
throw std::invalid_argument(oss.str());
437379
}
@@ -440,85 +382,7 @@ void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
440382
}
441383

442384

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-
473385
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-
}
522386
}
523387

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

0 commit comments

Comments
 (0)