[unitTesting] control/test_control_pd.cpp was missing.

This commit fix this.

Author: olivier-stasse

unitTesting/control/test_control_pd.cpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019,
+ * Olivier Stasse,
+ *
+ * CNRS/AIST
+ *
+ */
+
+#include <iostream>
+#include <sot/core/debug.hh>
+
+
+#ifndef WIN32
+#include <unistd.h>
+#endif
+
+using namespace std;
+
+#include <dynamic-graph/factory.h>
+#include <dynamic-graph/entity.h>
+#include <sot/core/control-pd.hh>
+#include <sstream>
+
+using namespace dynamicgraph;
+using namespace dynamicgraph::sot;
+
+#define BOOST_TEST_MODULE debug-control-pd
+
+#include <boost/test/unit_test.hpp>
+#include <boost/test/output_test_stream.hpp>
+
+BOOST_AUTO_TEST_CASE(control_pd)
+{
+  sot::ControlPD *aControlPD = new ControlPD("acontrol_pd");
+  aControlPD->init(0.001);
+  std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)");
+  std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)");
+  aControlPD->KpSIN.set(Kpiss);
+  aControlPD->KdSIN.set(Kdiss);
+  std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)");
+  aControlPD->positionSIN.set(posiss);
+  std::istringstream dposiss("[5](3.0,3.1,3.2,3.3,3.4)");
+  aControlPD->desiredpositionSIN.set(dposiss);
+  std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)");
+  aControlPD->velocitySIN.set(veliss);
+  std::istringstream dveliss("[5](1.5,1.4,1.3,1.2,1.1)");
+  aControlPD->desiredvelocitySIN.set(dveliss);
+
+  aControlPD->controlSOUT.recompute(0);
+  aControlPD->positionErrorSOUT.recompute(0);
+  aControlPD->velocityErrorSOUT.recompute(0);
+  {
+    boost::test_tools::output_test_stream output;
+    aControlPD->controlSOUT.get(output);
+    BOOST_CHECK(output.is_equal(" 20.15\n 42.28\n 66.39\n 92.48\n120.55\n"));
+  }
+  {
+    boost::test_tools::output_test_stream output;
+    aControlPD->positionErrorSOUT.get(output);
+    BOOST_CHECK(output.is_equal("  2\n2.1\n2.2\n2.3\n2.4\n"));
+  }
+  {
+    boost::test_tools::output_test_stream output;    
+    aControlPD->velocityErrorSOUT.get(output);
+    BOOST_CHECK(output.is_equal("1.5\n1.4\n1.3\n1.2\n1.1\n"));
+  }
+}
+
+