Merge pull request #72 from prashanthr05/feature/bypass-resetOffset

Add option to skip initial calibration and use zero offsets

CHANGELOG.md

@@ -4,6 +4,10 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## Unreleased
+### Added
+- Added 'startWithZeroFTSensorOffsets' option when passed starts the WBD device with zero offsets for FT sensors, bypassing the offset calibration. (See [!72](https://github.com/robotology/whole-body-estimators/pull/72)).
+
 ## [0.2.1] - 2020-04-08
 ### Fixed
 - Fixed launch-wholebodydynamics-`*`.xml configuration files in order to properly open `wholebodydynamics` device without silently skipping it. (See [!56](https://github.com/robotology/whole-body-estimators/pull/56)).

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

@@ -924,6 +924,19 @@ bool WholeBodyDynamicsDevice::loadSettingsFromConfig(os::Searchable& config)
     if (settings.useJointVelocity) { yInfo() << "wholeBodyDynamics: jointVelFilterCutoffInHz: " <<   settings.jointVelFilterCutoffInHz << " Hz."; }
     if (settings.useJointAcceleration) { yInfo() << "wholeBodyDynamics: jointAccFilterCutoffInHz: " <<   settings.jointAccFilterCutoffInHz << " Hz."; }
 
+    if ( !(prop.check("startWithZeroFTSensorOffsets") && prop.find("startWithZeroFTSensorOffsets").isBool()) )
+    {
+        settings.startWithZeroFTSensorOffsets = false;
+    }
+    else
+    {
+        settings.startWithZeroFTSensorOffsets = prop.find("startWithZeroFTSensorOffsets").asBool();
+        if (settings.startWithZeroFTSensorOffsets)
+        {
+            yInfo() << "wholeBodyDynamics: setting startWithZeroFTSensorOffsets was set to true , FT sensor offsets will be automatically reset to zero.";
+        }
+    }
+
     return true;
 }
 
@@ -942,7 +955,7 @@ bool WholeBodyDynamicsDevice::applyLPFSettingsFromConfig(const yarp::os::Propert
     {        
         return false;
     }
-    
+
     return true;
 }
 
@@ -1363,8 +1376,16 @@ bool WholeBodyDynamicsDevice::attachAll(const PolyDriverList& p)
     ok = ok && this->attachAllVirtualAnalogSensor(p);
     ok = ok && this->attachAllFTs(p);
     ok = ok && this->attachAllIMUs(p);
-
-    ok = ok && this->setupCalibrationWithExternalWrenchOnOneFrame("base_link",100);
+
+    if (settings.startWithZeroFTSensorOffsets)
+    {
+        this->setFTSensorOffsetsToZero();
+        validOffsetAvailable = true;
+    }
+    else
+    {
+        ok = ok && this->setupCalibrationWithExternalWrenchOnOneFrame("base_link",100);
+    }
 
     if( ok )
     {
@@ -1375,6 +1396,14 @@ bool WholeBodyDynamicsDevice::attachAll(const PolyDriverList& p)
     return ok;
 }
 
+void WholeBodyDynamicsDevice::setFTSensorOffsetsToZero()
+{
+    for(size_t ft = 0; ft < this->getNrOfFTSensors(); ft++)
+    {
+        ftProcessors[ft].offset().zero();            
+    }
+}
+
 double deg2rad(const double angleInDeg)
 {
     return angleInDeg*M_PI/180.0;
@@ -2342,10 +2371,7 @@ bool WholeBodyDynamicsDevice::resetOffset(const std::string& calib_code)
 
     yWarning() << "wholeBodyDynamics : calib ignoring calib_code " << calib_code;
 
-    for(size_t ft = 0; ft < this->getNrOfFTSensors(); ft++)
-    {
-        ftProcessors[ft].offset().zero();
-    }
+    this->setFTSensorOffsetsToZero();
 
     return true;
 }

devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h

@@ -126,6 +126,7 @@ class wholeBodyDynamicsDeviceFilters
  * | forceTorqueFilterCutoffInHz | - | double            | Hz    |      -        | Yes      | Cutoff frequency of the filter used to filter FT measures.  |  The used filter is a simple first order filter. |
  * | jointVelFilterCutoffInHz    | - | double            | Hz    |      -        | Yes      | Cutoff frequency of the filter used to filter joint velocities measures. | The used filter is a simple first order filter. |
  * | jointAccFilterCutoffInHz    | - | double            | Hz    |      -        | Yes      | Cutoff frequency of the filter used to filter joint accelerations measures. | The used filter is a simple first order filter. |
+ * | startWithZeroFTSensorOffsets    | - | bool            | -    |      False       | No      | Use zero FT sensor offsets at startup. If this flag is set to false, the device estimates the offsets of FT sensors at startup|
  * | defaultContactFrames      | -   | vector of strings (name of frames ) |-| - |  Yes     | Vector of default contact frames. If no external force read from the skin is found on a given submodel, the defaultContactFrames list is scanned and the first frame found on the submodel is the one at which origin the unknown contact force is assumed to be. | - |
  * | alwaysUpdateAllVirtualTorqueSensors | -     |  bool |  -    |      -        |  Yes     | Enforce that a virtual sensor for each estimated axes is available. | Tipically this is set to false when the device is running in the robot, while to true if it is running outside the robot. |
  * | defaultContactFrames |      -   | vector of strings |  -    |    -          | Yes      | If not data is read from the skin, specify the location of the default contacts | For each submodel induced by the FT sensor, the first not used frame that belongs to that submodel is selected from the list. An error is raised if not suitable frame is found for a submodel. |
@@ -490,7 +491,12 @@ class WholeBodyDynamicsDevice :  public yarp::dev::DeviceDriver,
      * Buffers related methods
      */
     void resizeBuffers();
-
+
+    /**
+     * Reset related methods
+     */
+    void setFTSensorOffsetsToZero();
+
     /*
      * Buffers
      *

devices/wholeBodyDynamics/app/wholebodydynamics-icub-external-six-fts-sim.xml

@@ -14,6 +14,7 @@
         <param name="forceTorqueFilterCutoffInHz">3.0</param>
         <param name="jointVelFilterCutoffInHz">3.0</param> <!-- used if useJointVelocity is set to true -->
         <param name="jointAccFilterCutoffInHz">3.0</param> <!-- used if useJointAcceleration is set to true -->
+        <param name="startWithZeroFTSensorOffsets">true</param> <!-- bypass using resetOffset of FT sensors in simulation -->
 
         <!-- map between iDynTree links (identified by a name)
              and skinDynLib links (identified by their frame name, a BodyPart enum