Add timeStep to sot-talos-controller.cpp

src/sot-talos-controller.cpp

@@ -71,6 +71,8 @@ void SoTTalosController::init()
   boost::unique_lock<boost::mutex> lock(mut);
   cond.wait(lock);
 
+  double ts = ros::param::param<double> ("/sot_controller/dt", SoTTalosDevice::TIMESTEP_DEFAULT);
+  device_.timeStep(ts);
 }
 
 SoTTalosController::~SoTTalosController()

src/sot-talos-device.hh

@@ -56,6 +56,8 @@ dgsot::Device
 
   void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
 
+  void timeStep(double ts) { timestep_ =ts;}
+
 protected:
 
   /// \brief Current integration step.