GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/control/test_control_pd.cpp Lines: 28 28 100.0 %
Date: 2023-03-13 12:09:37 Branches: 88 176 50.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2019,
3
 * Olivier Stasse,
4
 *
5
 * CNRS/AIST
6
 *
7
 */
8
9
#include <iostream>
10
#include <sot/core/debug.hh>
11
12
#ifndef WIN32
13
#include <unistd.h>
14
#endif
15
16
using namespace std;
17
18
#include <dynamic-graph/entity.h>
19
#include <dynamic-graph/factory.h>
20
21
#include <sot/core/control-pd.hh>
22
#include <sstream>
23
24
using namespace dynamicgraph;
25
using namespace dynamicgraph::sot;
26
27
#define BOOST_TEST_MODULE debug - control - pd
28
29
#include <boost/test/output_test_stream.hpp>
30
#include <boost/test/unit_test.hpp>
31
32
















4
BOOST_AUTO_TEST_CASE(control_pd) {
33

2
  sot::ControlPD *aControlPD = new ControlPD("acontrol_pd");
34
2
  aControlPD->init(0.001);
35

6
  std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)");
36

6
  std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)");
37
2
  aControlPD->KpSIN.set(Kpiss);
38
2
  aControlPD->KdSIN.set(Kdiss);
39

6
  std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)");
40
2
  aControlPD->positionSIN.set(posiss);
41

6
  std::istringstream dposiss("[5](3.0,3.1,3.2,3.3,3.4)");
42
2
  aControlPD->desiredpositionSIN.set(dposiss);
43

6
  std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)");
44
2
  aControlPD->velocitySIN.set(veliss);
45

6
  std::istringstream dveliss("[5](1.5,1.4,1.3,1.2,1.1)");
46
2
  aControlPD->desiredvelocitySIN.set(dveliss);
47
48
2
  aControlPD->controlSOUT.recompute(0);
49
2
  aControlPD->positionErrorSOUT.recompute(0);
50
2
  aControlPD->velocityErrorSOUT.recompute(0);
51
  {
52

4
    boost::test_tools::output_test_stream output;
53
2
    aControlPD->controlSOUT.get(output);
54



2
    BOOST_CHECK(output.is_equal("20.15 42.28 66.39 92.48 120.55"));
55
  }
56
  {
57

4
    boost::test_tools::output_test_stream output;
58
2
    aControlPD->positionErrorSOUT.get(output);
59



2
    BOOST_CHECK(output.is_equal("2 2.1 2.2 2.3 2.4"));
60
  }
61
  {
62

4
    boost::test_tools::output_test_stream output;
63
2
    aControlPD->velocityErrorSOUT.get(output);
64



2
    BOOST_CHECK(output.is_equal("1.5 1.4 1.3 1.2 1.1"));
65
  }
66
2
}