Pass time since last call to method getControl.

Author: florent-lamiraux

src/controller.cc

@@ -131,9 +131,9 @@ void SoTRobotArmController::cleanupSetSensors
 
 
 void SoTRobotArmController::getControl
-(map<string,ControlValues> &controlOut)
+(map<string,ControlValues> &controlOut, const double& period)
 {
-  device_->getControl(controlOut);
+  device_->getControl(controlOut, period);
 }
 
 void SoTRobotArmController::

src/controller.hh

@@ -57,7 +57,7 @@ class SoTRobotArmController: public AbstractSotExternalInterface
 
   void cleanupSetSensors(map<string, SensorValues> &sensorsIn);
 
-  void getControl(map<string, ControlValues> &anglesOut);
+  void getControl(map<string, ControlValues> &anglesOut, const double& period);
 
   void setNoIntegration(void);
   void setSecondOrderIntegration(void);

src/device.cc

@@ -208,13 +208,13 @@ void SoTRobotArmDevice::cleanupSetSensors(map<string, SensorValues>&
   setSensors (SensorsIn);
 }
 
-void SoTRobotArmDevice::getControl(map<string,ControlValues> &controlOut)
+void SoTRobotArmDevice::getControl(map<string,ControlValues> &controlOut, const double& period)
 {
   sotDEBUGIN(25) ;
   std::vector<double> anglesOut;
 
   // Integrate control
-  increment(timestep_);
+  increment(period);
   sotDEBUG (25) << "state = " << state_.transpose() << std::endl;
   previousState_ = state_;
 

src/device.hh

@@ -58,7 +58,7 @@ public:
 
   void cleanupSetSensors(std::map<std::string, SensorValues> &sensorsIn);
 
-  void getControl(std::map<std::string, ControlValues> &anglesOut);
+  void getControl(std::map<std::string, ControlValues> &anglesOut, const double& period);
 
   /// \todo this should go into the parent class, in sot-core package
   void setTimeStep (double dt)