Add unit test for device and feature_point6d.

.clang-format

+---
+ColumnLimit:     80
+...

unitTesting/CMakeLists.txt

@@ -98,6 +98,7 @@ SET (tests
   task/test_task
 
   tools/test_boost
+  tools/test_device  
   tools/test_mailbox
   tools/test_matrix
   tools/test_robot_utils

unitTesting/features/test_feature_point6d.cpp

@@ -105,10 +105,24 @@ class TestPoint6d
     dynamicgraph::Vector manual;
     const std::vector<dynamicgraph::sot::MultiBound> & taskTaskSOUT
         =task_.taskSOUT(time_);
-        
+    
+    /// Verify the computation of the desired frame.
+    /// -gain *(s-sd) - ([w]x (sd -s)-vd)
+    dynamicgraph::Matrix aM;
+    dynamicgraph::Vector aV;
+    aM.resize(3,3);
+    aV.resize(3);
+    aM(0,0) =    0.0;aM(0,1) = -vd(5); aM(0,2) = vd(4);
+    aM(1,0) =  vd(5);aM(1,1) = 0.0;    aM(1,2) =-vd(3);
+    aM(2,0) = -vd(4); aM(2,1) = vd(3); aM(2,2) = 0.0;
+    for(unsigned int i=0;i<3;i++)
+      aV(i) = sd(i,3)-s(i,3);
+    
+    aV = aM*aV;
+  
     for(unsigned int i=0;i<3;i++)
     {
-      manual_[i]  = - gain*(s(i,3)-sd(i,3)) - (-vd(i));
+      manual_[i]  = - gain*(s(i,3)-sd(i,3)) - ( aV(i)-vd(i));
       if (manual_[i]!=taskTaskSOUT[i].getSingleBound())
           return -1;
     }

unitTesting/tools/test_device.cpp

+/*
+ * Copyright 2019,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+
+#ifndef WIN32
+#include <unistd.h>
+#endif
+
+using namespace std;
+
+#include <boost/test/unit_test.hpp>
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/entity.h>
+#include <sot/core/device.hh>
+#include <sstream>
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+
+class TestDevice: public dg::sot::Device
+{
+ public:
+  TestDevice(const std::string & RobotName):
+      Device(RobotName)
+  {
+    timestep_=0.001;
+  }
+  ~TestDevice()
+  {
+  }
+  
+};
+  
+BOOST_AUTO_TEST_CASE(test_device)
+{
+  TestDevice aDevice(std::string("simple_humanoid"));
+
+  /// Fix constant vector for the control entry in position
+  dg::Vector aStateVector(38);
+  dg::Vector aVelocityVector(38);
+  dg::Vector aLowerVelBound(38),anUpperVelBound(38);
+  dg::Vector anAccelerationVector(38);
+  std::ostringstream ossControlVector;
+  std::istringstream issControlVector;
+
+
+  ossControlVector<<"[38](";
+  for (unsigned int i = 0; i < 38; i++) {
+    aLowerVelBound[i]= -3.14;
+    aStateVector[i] = -0.5;
+    anUpperVelBound[i]= 3.14;
+    ossControlVector << "-0.1";
+    if (i<37)
+      ossControlVector << ",";
+  }
+  ossControlVector << ")";
+  issControlVector.str(ossControlVector.str());
+  aDevice.setVelocitySize(38);
+  aDevice.setVelocityBounds(aLowerVelBound,anUpperVelBound);
+  aDevice.setVelocity(aStateVector);
+  aDevice.setState(aStateVector); // entry signal in position
+  aDevice.controlSIN.set(issControlVector);
+      
+  for (unsigned int i = 0; i < 2000; i++)
+  {
+    aDevice.stateSOUT.recompute(i);
+    if (i==1)
+      std::cout << " First integration " << aDevice.stateSOUT << std::endl;
+        
+  }
+
+  aDevice.display(std::cout);
+  aDevice.cmdDisplay();
+  const dg::Vector & aControl = aDevice.motorcontrolSOUT(2001);
+  double diff = 0, ldiff;
+
+}