[tests] Add test on feature point6d + fix other unit tests

unitTesting/CMakeLists.txt

@@ -45,6 +45,14 @@ SET(TEST_test_task_LIBS
   gain-adaptive feature-visual-point task
   )
 
+SET(TEST_test_feature_point6d_LIBS
+  gain-adaptive feature-point6d task
+  )
+
+SET(TEST_test_feature_generic_LIBS
+  gain-adaptive feature-generic task
+  )
+
 SET(TEST_test_mailbox_LIBS
   mailbox-vector
   )
@@ -67,6 +75,9 @@ SET (tests
 
   control/test_control_pd
 
+  features/test_feature_point6d
+  features/test_feature_generic
+  
   filters/test_filter_differentiator
   filters/test_madgwick_ahrs
   

unitTesting/features/test_feature_point6d.cpp

+/*
+ * Copyright 2019,
+ * François Bleibel,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+#include <iostream>
+
+#include <sot/core/sot.hh>
+#include <sot/core/feature-point6d.hh>
+#include <sot/core/feature-abstract.hh>
+#include <sot/core/debug.hh>
+#include <sot/core/task.hh>
+#include <sot/core/gain-adaptive.hh>
+#include <dynamic-graph/linear-algebra.h>
+using namespace std;
+using namespace dynamicgraph::sot;
+
+class TestPoint6d
+{
+ public:
+  FeaturePoint6d feature_,featureDes_;
+  Task task_;
+  int time_;
+  int dim_,robotDim_,featureDim_;
+  dynamicgraph::Vector manual_;
+  
+  TestPoint6d(unsigned dim,std::string &name):
+      feature_("feature"+name),
+      featureDes_("featureDes"+name),
+      task_("task"+name),
+      time_(0)
+
+  {
+    feature_.computationFrame("desired");
+    feature_.setReference(&featureDes_);
+    feature_.selectionSIN=Flags(true);
+
+    task_.addFeature(feature_);
+    task_.setWithDerivative(true);
+    dim_ = dim;
+    robotDim_ = featureDim_ = dim;
+
+    dynamicgraph::Matrix Jq(dim,dim);
+    Jq.setIdentity();
+    feature_.articularJacobianSIN.setReference(&Jq);
+
+    manual_.resize(dim);
+  }
+
+  void setInputs(MatrixHomogeneous & s,
+                  MatrixHomogeneous &sd,
+                  dynamicgraph::Vector &vd,
+                  double gain)
+  {
+    feature_.positionSIN = s;
+    feature_.positionSIN.access(time_);
+    feature_.positionSIN.setReady();
+    
+    featureDes_.positionSIN = sd;
+    featureDes_.positionSIN.access(time_);
+    featureDes_.positionSIN.setReady();
+    
+    featureDes_.velocitySIN = vd;
+    featureDes_.velocitySIN.access(time_);
+    featureDes_.velocitySIN.setReady();
+    
+    task_.controlGainSIN = gain;
+    task_.controlGainSIN.access(time_);
+    task_.controlGainSIN.setReady();
+  }
+
+  void printInputs()
+  {
+    std::cout << "----- inputs -----" << std::endl;
+    std::cout << "feature_.position: " << feature_.positionSIN(time_)
+              << std::endl;
+    std::cout << "featureDes_.position: " << featureDes_.positionSIN(time_)
+              << std::endl;
+    std::cout << "featureDes_.velocity: " << featureDes_.velocitySIN(time_)
+              << std::endl;
+    std::cout << "task.controlGain: " << task_.controlGainSIN(time_)
+              << std::endl;
+  }
+
+  int recompute()
+  {
+    
+    feature_.errorSOUT.recompute(time_);
+    feature_.errordotSOUT.recompute(time_);
+    task_.taskSOUT.recompute(time_);
+    task_.errorSOUT.recompute(time_);
+    task_.errorTimeDerivativeSOUT.recompute(time_);    
+    time_++;
+    MatrixHomogeneous s = feature_.positionSIN;
+    MatrixHomogeneous sd = featureDes_.positionSIN;
+    dynamicgraph::Vector vd = featureDes_.velocitySIN;
+    double gain = task_.controlGainSIN;
+    dynamicgraph::Vector manual;
+    const std::vector<dynamicgraph::sot::MultiBound> & taskTaskSOUT
+        =task_.taskSOUT(time_);
+        
+    for(unsigned int i=0;i<3;i++)
+    {
+      manual_[i]  = vd[i] + gain*(sd(i,3)-s(i,3));
+      if (manual_[i]!=taskTaskSOUT[i].getSingleBound())
+          return -1;
+    }
+    return 0;
+  }
+
+  void printOutput()
+  {
+    std::cout << "----- output -----" << std::endl;
+    std::cout << "time: " << time_ << std::endl;
+    std::cout << "feature.errorSOUT: " << feature_.errorSOUT(time_)
+              << std::endl;
+    std::cout << "feature.errordotSOUT: " << feature_.errordotSOUT(time_)
+              << std::endl;
+    std::cout << "task.taskSOUT: " << task_.taskSOUT(time_)
+              << std::endl;    
+    //    std::cout << "task.errorSOUT: " << task_.errorSOUT(time_)
+    //<< std::endl;
+    //std::cout << "task.errordtSOUT: " << task_.errorTimeDerivativeSOUT(time_)
+    //<< std::endl;    
+    std::cout << "manual: " << manual_ << std::endl;
+  }
+
+  int runTest(MatrixHomogeneous &s,
+               MatrixHomogeneous &sd,
+               dynamicgraph::Vector &vd,
+               double gain)
+  {
+    setInputs(s,sd,vd,gain);
+    printInputs();
+    int r= recompute();
+    printOutput();
+    return r;
+  }
+};
+
+
+int main( void )
+{
+  // Name of the robot
+  std::string srobot("Robot");
+  // Dimension of the robot.
+  unsigned int dim=6;
+  // Feature and Desired Feature
+  MatrixHomogeneous s, sd;
+  // Desired velocity
+  dynamicgraph::Vector vd(6);
+  // Task gain.
+  double gain;
+  // Result of test
+  int r;
+  
+  TestPoint6d testFeaturePoint6d(dim,srobot);
+  
+  std::cout << " ----- Test Velocity -----" << std::endl;
+  s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
+  sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
+  for(unsigned int i=0;i<6;i++)
+    vd(i)=1.0;
+  gain=0.0;
+  
+  if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
+  {
+    std::cerr << "Failure on 1st test." << std::endl;
+    return r;
+  }
+  
+  std::cout << " ----- Test Position -----" << std::endl;
+  s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
+  sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
+  sd(2,3) = 2.0;
+  for(unsigned int i=0;i<6;i++)
+    vd(i)=0.0;
+  gain=1.0;
+  
+  if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
+  {
+    std::cerr << "Failure on 2nd test." << std::endl;
+    return r;
+  }
+
+  std::cout << " ----- Test both -----" << std::endl;
+  s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
+  sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
+  sd(2,3) = 2.0;
+  for(unsigned int i=0;i<3;i++)
+    vd(i)=1.0;
+  gain=3.0;
+  
+  if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
+  {
+    std::cerr << "Failure on 3th test." << std::endl;
+    return r;
+  }
+
+  std::cout << " ----- Test both again -----" << std::endl;
+  s(0,0) = 1.0; s(1,1) = 1.0; s(2,2) = 1.0;
+  sd(0,0) = 1.0; sd(1,1) = 1.0; sd(2,2) = 1.0;
+  sd(2,3) = 2.0;
+  for(unsigned int i=0;i<6;i++)
+    vd(i)=1.0;
+  gain=3.0;
+  
+  if ((r=testFeaturePoint6d.runTest(s,sd,vd,gain))<0)
+  {
+    std::cerr << "Failure on 4th test." << std::endl;
+    return r;
+  }
+
+  std::cout << "Test successfull !" << std::endl;
+  return 0;
+}

unitTesting/task/test_task.cpp

@@ -37,8 +37,10 @@ int main( void )
   srand(12);
   dynamicgraph::Matrix Jq(6,6); Jq.setIdentity();
 
-  dynamicgraph::Vector p1xy(2); p1xy(0)=1.; p1xy(1)=-2;
-
+  dynamicgraph::Vector p1xy(6);
+  p1xy(0)=1.; p1xy(1)=-2; p1xy(2)=1.;
+  p1xy(3)=1.; p1xy(4)=-2; p1xy(5)=1.;
+  
   sotDEBUGF("Create feature");
   FeatureVisualPoint * p1 = new FeatureVisualPoint("p1");
   FeatureVisualPoint * p1des = new FeatureVisualPoint("p1d");
@@ -48,14 +50,13 @@ int main( void )
   p1->setReference(p1des);
   p1->xySIN = p1xy;
 
-  p1des->xySIN = dynamicgraph::Vector(2);
+  p1des->xySIN = dynamicgraph::Vector(6);
 
   sotDEBUGF("Create Task");
   //  sotDEBUG(0) << dynamicgraph::MATLAB;
 
   Task * task = new Task("t");
   task->addFeature(*p1);
-  task->addFeature(*p1);
 
   GainAdaptive * lambda = new GainAdaptive("g");
   lambda->errorSIN.plug( &task->errorSOUT );

unitTesting/tools/test_robot_utils.cpp

@@ -41,7 +41,8 @@ int main( void )
       std::cout << "cp_get_joint_limits_for_id works !" << std::endl;  }
   else
   {
-      std::cout << "ERROR: cp_get_joint_limits_for_id does not work !" << std::endl;
+      std::cout << "ERROR: cp_get_joint_limits_for_id does not work !"
+                << std::endl;
   }
 
 
@@ -84,11 +85,13 @@ int main( void )
   robot_util->joints_sot_to_urdf(q_sot, q_test_urdf);
   if (q_urdf == q_test_urdf )
   {
-      std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !" << std::endl;
+      std::cout << "joints_urdf_to_sot and joints_sot_to_urdf work !"
+                << std::endl;
   }
   else
   {
-      std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf do not work !" << std::endl;
+      std::cout << "ERROR: joints_urdf_to_sot or joints_sot_to_urdf "
+          "do not work !" << std::endl;
   }
 
   /*Test velocity_sot_to_urdf and velocity_urdf_to_sot */
@@ -97,7 +100,8 @@ int main( void )
   dg::Vector v_sot(9);
   robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot);
   robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf);
-  std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !" << std::endl;
+  std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !"
+            << std::endl;
 
   /*Test base_urdf_to_sot and base_sot_to_urdf */
   dg::Vector base_q_urdf(7);
@@ -140,7 +144,8 @@ int main( void )
   }
   else
   {
-      std::cout << "ERROR: force_util set and get id_from_name do not work !" << std::endl;
+      std::cout << "ERROR: force_util set and get id_from_name do not work !"
+                << std::endl;
   }
   if( robot_util->m_force_util.get_name_from_id(0) == rf &&
       robot_util->m_force_util.get_name_from_id(1) == lf &&
@@ -151,7 +156,8 @@ int main( void )
   }
   else
   {
-      std::cout << "ERROR: force_util get name_from_id does not work !" << std::endl;
+      std::cout << "ERROR: force_util get name_from_id does not work !"
+                << std::endl;
   }
   if (robot_util->m_force_util.get_limits_from_id(1).upper == uf_lim &&
       robot_util->m_force_util.get_limits_from_id(1).lower == lf_lim)
@@ -160,7 +166,8 @@ int main( void )
   }
   else
   {
-      std::cout << "ERROR: force_util set and get id to limits works do not work !" << std::endl;
+    std::cout << "ERROR: force_util set and get id to "
+        "limits works do not work !" << std::endl;
   }
   robot_util->m_force_util.display(std::cout);
   robot_util->m_foot_util.display(std::cout);