Update to modifications in the Stack of Tasks

  - the device now only gets a control vector as input and sends the
    corresponding commands to the robot via roscontrol. Integration is
    now performed by a specific entity.

Author: florent-lamiraux

src/roscontrol-sot-controller.cpp

@@ -171,20 +171,8 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw,
   ROS_WARN("initRequest 4");
   /// Create SoT
   SotLoaderBasic::Initialization();
+  sotController_->setControlSize((int)joints_name_.size());
   ROS_WARN("initRequest 5");
-  /// Fill desired position during the phase where the robot is waiting.
-  for (unsigned int idJoint = 0; idJoint < joints_.size(); idJoint++) {
-    std::string joint_name = joints_name_[idJoint];
-    std::map<std::string, ControlPDMotorControlData>::iterator search_ecpd =
-        effort_mode_pd_motors_.find(joint_name);
-
-    if (search_ecpd != effort_mode_pd_motors_.end()) {
-      /// If we are in effort mode then the device
-      /// should not do any integration.
-      sotController_->setNoIntegration();
-    }
-  }
-  ROS_WARN("initRequest 6");
   return true;
 }
 

tests/sot-test-controller.cpp

@@ -101,6 +101,15 @@ void SoTTestController::getControl(
   }
 }
 
+void SoTTestController::setControlSize(const int& size)
+{
+  device_->setControlSize(size);
+}
+
+void SoTTestController::initialize()
+{
+}
+
 void SoTTestController::setNoIntegration(void) { device_->setNoIntegration(); }
 
 void SoTTestController::setSecondOrderIntegration(void) {

tests/sot-test-controller.hh

@@ -41,6 +41,8 @@ class SoTTestController : public dgsot::AbstractSotExternalInterface {
   void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut,
                   const double &period);
 
+  void setControlSize(const int& size);
+  void initialize();
   void setNoIntegration(void);
   void setSecondOrderIntegration(void);
 

tests/sot-test-device.cpp

@@ -101,16 +101,6 @@ SoTTestDevice::SoTTestDevice(std::string RobotName)
 
   using namespace dynamicgraph::command;
   std::string docstring;
-  /* Command increment. */
-  docstring =
-      "\n"
-      "    Integrate dynamics for time step provided as input\n"
-      "\n"
-      "      take one floating point number as input\n"
-      "\n";
-  addCommand("increment",
-             makeCommandVoid1((Device &)*this, &Device::increment, docstring));
-
   sotDEBUGOUT(25);
 }
 
@@ -303,8 +293,6 @@ void SoTTestDevice::getControl(map<string, dgsot::ControlValues> &controlOut,
   vector<double> anglesOut;
   anglesOut.resize(state_.size());
 
-  // Integrate control
-  increment(period);
   sotDEBUG(25) << "state = " << state_ << std::endl;
   sotDEBUG(25) << "diff  = "
                << ((previousState_.size() == state_.size())