| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_device) { |
39 |
✓✗✓✗ |
6 |
TestDevice aDevice(std::string("simple_humanoid")); |
40 |
|||
41 |
/// Fix constant vector for the control entry in position |
||
42 |
✓✗ | 4 |
dg::Vector aStateVector(38); |
43 |
✓✗ | 4 |
dg::Vector aVelocityVector(38); |
44 |
✓✗✓✗ |
4 |
dg::Vector aLowerVelBound(38), anUpperVelBound(38); |
45 |
✓✗✓✗ |
4 |
dg::Vector aLowerBound(38), anUpperBound(38); |
46 |
✓✗ | 4 |
dg::Vector anAccelerationVector(38); |
47 |
✓✗ | 4 |
dg::Vector aControlVector(38); |
48 |
|||
49 |
✓✓ | 78 |
for (unsigned int i = 0; i < 38; i++) { |
50 |
// Specify lower velocity bound |
||
51 |
✓✗ | 76 |
aLowerVelBound[i] = -3.14; |
52 |
// Specify lower position bound |
||
53 |
✓✗ | 76 |
aLowerBound[i] = -3.14; |
54 |
// Specify state vector |
||
55 |
✓✗ | 76 |
aStateVector[i] = 0.1; |
56 |
// Specify upper velocity bound |
||
57 |
✓✗ | 76 |
anUpperVelBound[i] = 3.14; |
58 |
// Specify upper position bound |
||
59 |
✓✗ | 76 |
anUpperBound[i] = 3.14; |
60 |
// Specify control vector |
||
61 |
✓✗ | 76 |
aControlVector(i) = 0.1; |
62 |
} |
||
63 |
|||
64 |
✓✗ | 4 |
dg::Vector expected = aStateVector; // backup initial state vector |
65 |
|||
66 |
/// Specify state size |
||
67 |
✓✗ | 2 |
aDevice.setStateSize(38); |
68 |
/// Specify state bounds |
||
69 |
✓✗ | 2 |
aDevice.setPositionBounds(aLowerBound, anUpperBound); |
70 |
/// Specify velocity size |
||
71 |
✓✗ | 2 |
aDevice.setVelocitySize(38); |
72 |
/// Specify velocity |
||
73 |
✓✗ | 2 |
aDevice.setVelocity(aStateVector); |
74 |
/// Specify velocity bounds |
||
75 |
✓✗ | 2 |
aDevice.setVelocityBounds(aLowerVelBound, anUpperVelBound); |
76 |
/// Specify current state value |
||
77 |
✓✗ | 2 |
aDevice.setState(aStateVector); // entry signal in position |
78 |
/// Specify constant control value |
||
79 |
✓✗ | 2 |
aDevice.controlSIN.setConstant(aControlVector); |
80 |
|||
81 |
2 |
const double dt = 0.001; |
|
82 |
2 |
const unsigned int N = 2000; |
|
83 |
✓✓ | 4002 |
for (unsigned int i = 0; i < N; i++) { |
84 |
✓✗ | 4000 |
aDevice.increment(dt); |
85 |
✓✓ | 4000 |
if (i == 0) { |
86 |
✓✗ | 2 |
aDevice.stateSOUT.get(std::cout); |
87 |
✓✗ | 4 |
std::ostringstream anoss; |
88 |
✓✗ | 2 |
aDevice.stateSOUT.get(anoss); |
89 |
} |
||
90 |
✓✓ | 4000 |
if (i == 1) { |
91 |
✓✗ | 2 |
aDevice.stateSOUT.get(std::cout); |
92 |
✓✗ | 4 |
std::ostringstream anoss; |
93 |
✓✗ | 2 |
aDevice.stateSOUT.get(anoss); |
94 |
} |
||
95 |
} |
||
96 |
|||
97 |
✓✗ | 2 |
aDevice.display(std::cout); |
98 |
✓✗ | 2 |
aDevice.cmdDisplay(); |
99 |
|||
100 |
// verify correct integration |
||
101 |
typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3; |
||
102 |
✓✗✓✗ |
2 |
Eigen::Matrix<double, 7, 1> qin, qout; |
103 |
✓✗✓✗ ✓✗ |
2 |
qin.head<3>() = expected.head<3>(); |
104 |
|||
105 |
✓✗✓✗ |
2 |
Eigen::QuaternionMapd quat(qin.tail<4>().data()); |
106 |
✓✗✓✗ ✓✗✓✗ |
2 |
quat = Eigen::AngleAxisd(expected(5), Eigen::Vector3d::UnitZ()) * |
107 |
✓✗✓✗ ✓✗✓✗ |
4 |
Eigen::AngleAxisd(expected(4), Eigen::Vector3d::UnitY()) * |
108 |
✓✗✓✗ ✓✗✓✗ |
4 |
Eigen::AngleAxisd(expected(3), Eigen::Vector3d::UnitX()); |
109 |
|||
110 |
2 |
const double T = dt * N; |
|
111 |
✓✗✓✗ ✓✗ |
2 |
Eigen::Matrix<double, 6, 1> control = aControlVector.head<6>() * T; |
112 |
✓✗✓✗ |
2 |
SE3().integrate(qin, control, qout); |
113 |
|||
114 |
// Manual integration |
||
115 |
✓✗✓✗ ✓✗ |
2 |
expected.head<3>() = qout.head<3>(); |
116 |
✓✗✓✗ ✓✗ |
4 |
expected.segment<3>(3) = Eigen::QuaternionMapd(qout.tail<4>().data()) |
117 |
✓✗ | 2 |
.toRotationMatrix() |
118 |
✓✗ | 2 |
.eulerAngles(2, 1, 0) |
119 |
✓✗✓✗ |
4 |
.reverse(); |
120 |
✓✗✓✓ ✓✗ |
66 |
for (int i = 6; i < expected.size(); i++) expected[i] = 0.3; |
121 |
|||
122 |
✓✗✓✗ ✓✗ |
2 |
std::cout << expected.transpose() << std::endl; |
123 |
✓✗✓✗ ✓✗✓✗ |
2 |
std::cout << aDevice.stateSOUT(N).transpose() << std::endl; |
124 |
|||
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(aDevice.stateSOUT(N).isApprox(expected)); |
126 |
2 |
} |
| Generated by: GCOVR (Version 4.2) |