GCC Code Coverage Report

Directory: ./
File: tests/tools/test_device.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 62 62 100.0%
Branches: 126 242 52.1%
Line Branch Exec Source
1 /*
2 * Copyright 2019,
3 * Olivier Stasse,
4 *
5 * CNRS/AIST
6 *
7 */
8
9 #include <iostream>
10 #include <pinocchio/multibody/liegroup/special-euclidean.hpp>
11 #include <sot/core/debug.hh>
12
13 #ifndef WIN32
14 #include <unistd.h>
15 #endif
16
17 using namespace std;
18
19 #include <dynamic-graph/entity.h>
20 #include <dynamic-graph/factory.h>
21
22 #include <boost/test/unit_test.hpp>
23 #include <sot/core/device.hh>
24 #include <sstream>
25
26 using namespace dynamicgraph;
27 using namespace dynamicgraph::sot;
28 namespace dg = dynamicgraph;
29
30 class TestDevice : public dg::sot::Device {
31 public:
32 1 TestDevice(const std::string &RobotName) : Device(RobotName) {
33 1 timestep_ = 0.001;
34 1 }
35 2 ~TestDevice() {}
36 };
37
38
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_device) {
39
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 TestDevice aDevice(std::string("simple_humanoid"));
40
41 /// Fix constant vector for the control entry in position
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dg::Vector aStateVector(38);
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dg::Vector aVelocityVector(38);
44
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 dg::Vector aLowerVelBound(38), anUpperVelBound(38);
45
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 dg::Vector aLowerBound(38), anUpperBound(38);
46
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dg::Vector anAccelerationVector(38);
47
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dg::Vector aControlVector(38);
48
49
2/2
✓ Branch 0 taken 38 times.
✓ Branch 1 taken 1 times.
 78 for (unsigned int i = 0; i < 38; i++) {
50 // Specify lower velocity bound
51
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 aLowerVelBound[i] = -3.14;
52 // Specify lower position bound
53
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 aLowerBound[i] = -3.14;
54 // Specify state vector
55
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 aStateVector[i] = 0.1;
56 // Specify upper velocity bound
57
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 anUpperVelBound[i] = 3.14;
58 // Specify upper position bound
59
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 anUpperBound[i] = 3.14;
60 // Specify control vector
61
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 76 aControlVector(i) = 0.1;
62 }
63
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 dg::Vector expected = aStateVector; // backup initial state vector
65
66 /// Specify state size
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setStateSize(38);
68 /// Specify state bounds
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setPositionBounds(aLowerBound, anUpperBound);
70 /// Specify velocity size
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setVelocitySize(38);
72 /// Specify velocity
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setVelocity(aStateVector);
74 /// Specify velocity bounds
75
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound);
76 /// Specify current state value
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.setState(aStateVector); // entry signal in position
78 /// Specify constant control value
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.controlSIN.setConstant(aControlVector);
80
81 2 const double dt = 0.001;
82 2 const unsigned int N = 2000;
83
2/2
✓ Branch 0 taken 2000 times.
✓ Branch 1 taken 1 times.
 4002 for (unsigned int i = 0; i < N; i++) {
84
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 4000 aDevice.increment(dt);
85
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1999 times.
 4000 if (i == 0) {
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.stateSOUT.get(std::cout);
87
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::ostringstream anoss;
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.stateSOUT.get(anoss);
89 2 }
90
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1999 times.
 4000 if (i == 1) {
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.stateSOUT.get(std::cout);
92
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::ostringstream anoss;
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.stateSOUT.get(anoss);
94 2 }
95 }
96
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.display(std::cout);
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 aDevice.cmdDisplay();
99
100 // verify correct integration
101 typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
102
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Eigen::Matrix<double, 7, 1> qin, qout;
103
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 qin.head<3>() = expected.head<3>();
104
105
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Eigen::QuaternionMapd quat(qin.tail<4>().data());
106
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 quat = Eigen::AngleAxisd(expected(5), Eigen::Vector3d::UnitZ()) *
107
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Eigen::AngleAxisd(expected(4), Eigen::Vector3d::UnitY()) *
108
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Eigen::AngleAxisd(expected(3), Eigen::Vector3d::UnitX());
109
110 2 const double T = dt * N;
111
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 Eigen::Matrix<double, 6, 1> control = aControlVector.head<6>() * T;
112
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 SE3().integrate(qin, control, qout);
113
114 // Manual integration
115
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 expected.head<3>() = qout.head<3>();
116
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 4 expected.segment<3>(3) = Eigen::QuaternionMapd(qout.tail<4>().data())
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .toRotationMatrix()
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .eulerAngles(2, 1, 0)
119
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .reverse();
120
3/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✓ Branch 5 taken 1 times.
 66 for (int i = 6; i < expected.size(); i++) expected[i] = 0.3;
121
122
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 std::cout << expected.transpose() << std::endl;
123
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 std::cout << aDevice.stateSOUT(N).transpose() << std::endl;
124
125
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK(aDevice.stateSOUT(N).isApprox(expected));
126 2 }
127