Add check of position and velocity bounds.

Author: jmirabel

include/sot/core/device.hh

@@ -75,11 +75,22 @@ namespace dynamicgraph {
     protected:
       dg::Vector state_;
       dg::Vector velocity_;
+      bool sanityCheck_;
       dg::Vector vel_control_;
       ControlInput controlInputType_;
       bool withForceSignals[4];
       PeriodicCall periodicCallBefore_;
       PeriodicCall periodicCallAfter_;
+
+      /// \name Robot bounds used for sanity checks
+      /// \{
+      Vector upperPosition_;
+      Vector upperVelocity_;
+      Vector upperTorque_;
+      Vector lowerPosition_;
+      Vector lowerVelocity_;
+      Vector lowerTorque_;
+      /// \}
     public:
 
       /* --- CONSTRUCTION --- */
@@ -96,6 +107,14 @@ namespace dynamicgraph {
       virtual void setControlInputType(const std::string& cit);
       virtual void increment(const double & dt = 5e-2);
 
+      /// \name Sanity check parameterization
+      /// \{
+      void setSanityCheck   (const bool & enableCheck);
+      void setPositionBounds(const Vector& lower, const Vector& upper);
+      void setVelocityBounds(const Vector& lower, const Vector& upper);
+      void setTorqueBounds  (const Vector& lower, const Vector& upper);
+      /// \}
+
     public: /* --- DISPLAY --- */
       virtual void display(std::ostream& os) const;
       SOT_CORE_EXPORT friend std::ostream&
@@ -144,6 +163,17 @@ namespace dynamicgraph {
       /// Store Position of free flyer joint
       MatrixHomogeneous ffPose_;
       /// Compute the new position, from the current control.
+      ///
+      /// When sanity checks are enabled, this checks that the control is within
+      /// bounds. There are three cases, depending on what the control is:
+      /// - position: checks that the position is within bounds,
+      /// - velocity: checks that the velocity and the future position are
+      ///             within bounds,
+      /// - acceleration: checks that the acceleration, the future velocity and
+      ///                 position are within bounds.
+      ///                 \todo in order to check the acceleration, we need
+      ///                 pinocchio and the contact forces in order to estimate
+      ///                 the joint torques for the given acceleration.
       virtual void integrate( const double & dt );
     protected:
       /// Get freeflyer pose

src/tools/device.cpp

@@ -88,7 +88,7 @@ Device::
 Device( const std::string& n )
   :Entity(n)
   ,state_(6)
-  ,vel_controlInit_(false)
+  ,sanityCheck_(true)
   ,controlInputType_(CONTROL_INPUT_ONE_INTEGRATION)
   ,controlSIN( NULL,"Device("+n+")::input(double)::control" )   
   ,attitudeSIN(NULL,"Device("+ n +")::input(vector3)::attitudeIN")
@@ -282,6 +282,88 @@ setControlInputType(const std::string& cit)
   sotDEBUG(25)<<"Unrecognized control input type: "<<cit<<endl;
 }
 
+void Device::
+setSanityCheck(const bool & enableCheck)
+{
+  if (enableCheck) {
+    const Vector::Index& s = state_.size();
+    switch (controlInputType_) {
+      case CONTROL_INPUT_TWO_INTEGRATION:
+        dgRTLOG()
+          << "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";
+        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");
+      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");
+      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.");
+    }
+  }
+  sanityCheck_ = enableCheck;
+}
+
+void Device::
+setPositionBounds(const Vector& lower, const Vector& upper)
+{
+  std::ostringstream oss;
+  if (lower.size() != state_.size()) {
+    oss << "Lower bound size should be " << state_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  if (upper.size() != state_.size()) {
+    oss << "Upper bound size should be " << state_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  lowerPosition_ = lower;
+  upperPosition_ = upper;
+}
+
+void Device::
+setVelocityBounds(const Vector& lower, const Vector& upper)
+{
+  std::ostringstream oss;
+  if (lower.size() != velocity_.size()) {
+    oss << "Lower bound size should be " << velocity_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  if (upper.size() != velocity_.size()) {
+    oss << "Upper bound size should be " << velocity_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  lowerVelocity_ = lower;
+  upperVelocity_ = upper;
+}
+
+void Device::
+setTorqueBounds  (const Vector& lower, const Vector& upper)
+{
+  // TODO I think the torque bounds size are state_.size()-6...
+  std::ostringstream oss;
+  if (lower.size() != state_.size()) {
+    oss << "Lower bound size should be " << state_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  if (upper.size() != state_.size()) {
+    oss << "Lower bound size should be " << state_.size();
+    throw std::invalid_argument (oss.str());
+  }
+  lowerTorque_ = lower;
+  upperTorque_ = upper;
+}
+
 void Device::
 increment( const double & dt )
 {
@@ -371,6 +453,23 @@ increment( const double & dt )
   motorcontrolSOUT .setConstant( state_ );
 }
 
+// Return true if it saturates.
+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; }
+  return false;
+}
+
+#define CHECK_BOUNDS(val, lower, upper, what)                                  \
+  for (int i = 0; i < val.size(); ++i) {                                       \
+    if (saturateBounds (val(i), lower(i), upper(i)))                           \
+      dgRTLOG () << "A robot "what" bound would be violated at DoF " << i <<   \
+      " " << val(i);                                                           \
+  }
+
 void Device::integrate( const double & dt )
 {
   const Vector & controlIN = controlSIN.accessCopy();
@@ -379,6 +478,7 @@ void Device::integrate( const double & dt )
   {
     assert(state_.size()==controlIN.size()+6);
     state_.tail(controlIN.size()) = controlIN;
+    CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
     return;
   }
 
@@ -391,6 +491,7 @@ void Device::integrate( const double & dt )
 
   if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION)
   {
+    // TODO check acceleration
     // Position increment
     vel_control_ = velocity_.tail(controlIN.size()) + (0.5*dt)*controlIN;
     // Velocity integration.
@@ -401,12 +502,23 @@ void Device::integrate( const double & dt )
     vel_control_ = controlIN;
   }
 
+  // Velocity bounds check
+  if (sanityCheck_) {
+    CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity");
+  }
+
+  // Freeflyer integration
   if (vel_control_.size() == state_.size()) {
     integrateRollPitchYaw(state_, vel_control_, dt);
   }
 
   // Position integration
   state_.tail(controlIN.size()) += vel_control_ * dt;
+
+  // Position bounds check
+  if (sanityCheck_) {
+    CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
+  }
 }
 
 /* --- DISPLAY ------------------------------------------------------------ */