GCC Code Coverage Report

Directory: ./
File: src/tools/device.cpp
Date: 2024-08-13 12:13:25
Exec Total Coverage
Lines: 188 309 60.8%
Branches: 263 868 30.3%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
4 *
5 * CNRS
6 *
7 */
8
9 /* --------------------------------------------------------------------- */
10 /* --- INCLUDE --------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12
13 /* SOT */
14 #define ENABLE_RT_LOG
15
16 #include "sot/core/device.hh"
17
18 #include <iostream>
19 #include <sot/core/debug.hh>
20 #include <sot/core/macros.hh>
21 using namespace std;
22
23 #include <dynamic-graph/all-commands.h>
24 #include <dynamic-graph/factory.h>
25 #include <dynamic-graph/linear-algebra.h>
26 #include <dynamic-graph/real-time-logger.h>
27
28 #include <Eigen/Geometry>
29 #include <pinocchio/multibody/liegroup/special-euclidean.hpp>
30 #include <sot/core/matrix-geometry.hh>
31 using namespace dynamicgraph::sot;
32 using namespace dynamicgraph;
33
34 const std::string Device::CLASS_NAME = "Device";
35
36 /* --------------------------------------------------------------------- */
37 /* --- CLASS ----------------------------------------------------------- */
38 /* --------------------------------------------------------------------- */
39
40 2000 void Device::integrateRollPitchYaw(Vector &state, const Vector &control,
41 double dt) {
42 using Eigen::AngleAxisd;
43 using Eigen::QuaternionMapd;
44 using Eigen::Vector3d;
45
46 typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3;
47
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 Eigen::Matrix<double, 7, 1> qin, qout;
48
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 qin.head<3>() = state.head<3>();
49
50
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2000 times.
✗ Branch 6 not taken.
 2000 QuaternionMapd quat(qin.tail<4>().data());
51
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 2000 quat = AngleAxisd(state(5), Vector3d::UnitZ()) *
52
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 4000 AngleAxisd(state(4), Vector3d::UnitY()) *
53
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 4000 AngleAxisd(state(3), Vector3d::UnitX());
54
55
4/8
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 2000 SE3().integrate(qin, control.head<6>() * dt, qout);
56
57 // Update freeflyer pose
58
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 ffPose_.translation() = qout.head<3>();
59
3/6
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
 2000 state.head<3>() = qout.head<3>();
60
61
5/10
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2000 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2000 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2000 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2000 times.
✗ Branch 15 not taken.
 2000 ffPose_.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
62
5/10
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2000 times.
✗ Branch 14 not taken.
 2000 state.segment<3>(3) = ffPose_.linear().eulerAngles(2, 1, 0).reverse();
63 2000 }
64
65 ✗ const MatrixHomogeneous &Device::freeFlyerPose() const { return ffPose_; }
66
67 4 Device::~Device() {
68
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
 20 for (unsigned int i = 0; i < 4; ++i) {
69
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
 16 delete forcesSOUT[i];
70 }
71 }
72
73 2 Device::Device(const std::string &n)
74 : Entity(n),
75 ✗ state_(6),
76 2 sanityCheck_(true),
77 2 controlInputType_(CONTROL_INPUT_ONE_INTEGRATION),
78
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 controlSIN(NULL, "Device(" + n + ")::input(double)::control"),
79
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 attitudeSIN(NULL, "Device(" + n + ")::input(vector3)::attitudeIN"),
80
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 zmpSIN(NULL, "Device(" + n + ")::input(vector3)::zmp"),
81
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 stateSOUT("Device(" + n + ")::output(vector)::state")
82 //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN")
83 ,
84
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 velocitySOUT("Device(" + n + ")::output(vector)::velocity"),
85
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 attitudeSOUT("Device(" + n + ")::output(matrixRot)::attitude"),
86
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 motorcontrolSOUT("Device(" + n + ")::output(vector)::motorcontrol"),
87
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 previousControlSOUT("Device(" + n + ")::output(vector)::previousControl"),
88
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 ZMPPreviousControllerSOUT("Device(" + n +
89 ")::output(vector)::zmppreviouscontroller"),
90
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 robotState_("Device(" + n + ")::output(vector)::robotState"),
91
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity"),
92
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 pseudoTorqueSOUT("Device(" + n + ")::output(vector)::ptorque")
93
94 ,
95
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ffPose_(),
96
12/24
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 2 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 2 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 2 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 2 times.
✗ Branch 36 not taken.
 4 forceZero6(6) {
97
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 forceZero6.fill(0);
98 /* --- SIGNALS --- */
99
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
 10 for (int i = 0; i < 4; ++i) {
100 8 withForceSignals[i] = false;
101 }
102 4 forcesSOUT[0] =
103
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRLEG");
104 4 forcesSOUT[1] =
105
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLLEG");
106 4 forcesSOUT[2] =
107
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceRARM");
108 4 forcesSOUT[3] =
109
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 new Signal<Vector, int>("Device(" + n + ")::output(vector6)::forceLARM");
110
111 4 signalRegistration(
112
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 4 controlSIN << stateSOUT << robotState_ << robotVelocity_ << velocitySOUT
113
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 << attitudeSOUT << attitudeSIN << zmpSIN << *forcesSOUT[0]
114
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 << *forcesSOUT[1] << *forcesSOUT[2] << *forcesSOUT[3]
115
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 << previousControlSOUT << pseudoTorqueSOUT << motorcontrolSOUT
116
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 << ZMPPreviousControllerSOUT);
117
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 state_.fill(.0);
118
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 stateSOUT.setConstant(state_);
119
120
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 velocity_.resize(state_.size());
121
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 velocity_.setZero();
122
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 velocitySOUT.setConstant(velocity_);
123
124 /* --- Commands --- */
125 {
126 2 std::string docstring;
127 /* Command setStateSize. */
128 docstring =
129 "\n"
130 " Set size of state vector\n"
131
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
132
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 addCommand("resize", new command::Setter<Device, unsigned int>(
133
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 *this, &Device::setStateSize, docstring));
134 docstring =
135 "\n"
136 " Set state vector value\n"
137
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
138
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 addCommand("set", new command::Setter<Device, Vector>(
139
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 *this, &Device::setState, docstring));
140
141 docstring =
142 "\n"
143 " Set velocity vector value\n"
144
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
145
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 addCommand("setVelocity", new command::Setter<Device, Vector>(
146
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 *this, &Device::setVelocity, docstring));
147
148 2 void (Device::*setRootPtr)(const Matrix &) = &Device::setRoot;
149
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 docstring = command::docCommandVoid1("Set the root position.",
150 2 "matrix homogeneous");
151
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("setRoot",
152
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeCommandVoid1(*this, setRootPtr, docstring));
153
154 /* Second Order Integration set. */
155 docstring =
156 "\n"
157 " Set the position calculous starting from \n"
158 " acceleration measure instead of velocity \n"
159
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
160
161
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("setSecondOrderIntegration",
162
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeCommandVoid0(
163 *this, &Device::setSecondOrderIntegration, docstring));
164
165 /* Display information */
166 docstring =
167 "\n"
168 " Display information on device \n"
169
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
170
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 addCommand("display", command::makeCommandVoid0(*this, &Device::cmdDisplay,
171 docstring));
172
173 /* SET of control input type. */
174 docstring =
175 "\n"
176 " Set the type of control input which can be \n"
177 " acceleration, velocity, or position\n"
178
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
179
180
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 addCommand("setControlInputType",
181 new command::Setter<Device, string>(
182
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 *this, &Device::setControlInputType, docstring));
183
184 docstring =
185 "\n"
186 " Enable/Disable sanity checks\n"
187
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 "\n";
188
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 addCommand("setSanityCheck",
189 new command::Setter<Device, bool>(*this, &Device::setSanityCheck,
190
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 docstring));
191
192
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("setPositionBounds",
193
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeCommandVoid2(
194 *this, &Device::setPositionBounds,
195
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 4 command::docCommandVoid2("Set robot position bounds",
196 "vector: lower bounds",
197 "vector: upper bounds")));
198
199
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("setVelocityBounds",
200
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeCommandVoid2(
201 *this, &Device::setVelocityBounds,
202
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 4 command::docCommandVoid2("Set robot velocity bounds",
203 "vector: lower bounds",
204 "vector: upper bounds")));
205
206
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("setTorqueBounds",
207
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeCommandVoid2(
208 *this, &Device::setTorqueBounds,
209
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 4 command::docCommandVoid2("Set robot torque bounds",
210 "vector: lower bounds",
211 "vector: upper bounds")));
212
213
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 addCommand("getTimeStep",
214
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 command::makeDirectGetter(
215 *this, &this->timestep_,
216
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 command::docDirectGetter("Time step", "double")));
217 2 }
218 2 }
219
220 1 void Device::setStateSize(const unsigned int &size) {
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_.resize(size);
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_.fill(.0);
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 stateSOUT.setConstant(state_);
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 previousControlSOUT.setConstant(state_);
225
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pseudoTorqueSOUT.setConstant(state_);
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 motorcontrolSOUT.setConstant(state_);
227
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Device::setVelocitySize(size);
229
230
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Vector zmp(3);
231
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 zmp.fill(.0);
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ZMPPreviousControllerSOUT.setConstant(zmp);
233 1 }
234
235 2 void Device::setVelocitySize(const unsigned int &size) {
236 2 velocity_.resize(size);
237
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 velocity_.fill(.0);
238 2 velocitySOUT.setConstant(velocity_);
239 2 }
240
241 1 void Device::setState(const Vector &st) {
242
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (sanityCheck_) {
243 1 const Vector::Index &s = st.size();
244 SOT_CORE_DISABLE_WARNING_PUSH
245 SOT_CORE_DISABLE_WARNING_FALLTHROUGH
246
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 switch (controlInputType_) {
247 ✗ case CONTROL_INPUT_TWO_INTEGRATION:
248 ✗ dgRTLOG()
249 << "Sanity check for this control is not well supported. "
250 "In order to make it work, use pinocchio and the contact forces "
251 ✗ "to estimate the joint torques for the given acceleration.\n";
252 ✗ if (s != lowerTorque_.size() || s != upperTorque_.size()) {
253 ✗ std::ostringstream os;
254 os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
255 ✗ "bounds do not match state size. Input State size = "
256 ✗ << st.size() << ", lowerTorque_.size() = " << lowerTorque_.size()
257 ✗ << ", upperTorque_.size() = " << upperTorque_.size()
258 ✗ << ". Set them first with setTorqueBounds.";
259 ✗ throw std::invalid_argument(os.str().c_str());
260 // fall through
261 }
262 case CONTROL_INPUT_ONE_INTEGRATION:
263
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 if (s != lowerVelocity_.size() || s != upperVelocity_.size()) {
264 ✗ std::ostringstream os;
265 os << "dynamicgraph::sot::Device::setState: upper and/or lower "
266 "velocity"
267 ✗ " bounds do not match state size. Input State size = "
268 ✗ << st.size()
269 ✗ << ", lowerVelocity_.size() = " << lowerVelocity_.size()
270 ✗ << ", upperVelocity_.size() = " << upperVelocity_.size()
271 ✗ << ". Set them first with setVelocityBounds.";
272 ✗ throw std::invalid_argument(os.str().c_str());
273 }
274 // fall through
275 case CONTROL_INPUT_NO_INTEGRATION:
276 1 break;
277 ✗ default:
278 ✗ throw std::invalid_argument("Invalid control mode type.");
279 }
280 SOT_CORE_DISABLE_WARNING_POP
281 }
282 1 state_ = st;
283 1 stateSOUT.setConstant(state_);
284 1 motorcontrolSOUT.setConstant(state_);
285 1 }
286
287 1 void Device::setVelocity(const Vector &vel) {
288 1 velocity_ = vel;
289 1 velocitySOUT.setConstant(velocity_);
290 1 }
291
292 ✗ void Device::setRoot(const Matrix &root) {
293 ✗ Eigen::Matrix4d _matrix4d(root);
294 ✗ MatrixHomogeneous _root(_matrix4d);
295 ✗ setRoot(_root);
296 }
297
298 ✗ void Device::setRoot(const MatrixHomogeneous &worldMwaist) {
299 ✗ VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2, 1, 0)).reverse();
300 ✗ Vector q = state_;
301 ✗ q = worldMwaist.translation(); // abusive ... but working.
302 ✗ for (unsigned int i = 0; i < 3; ++i) q(i + 3) = r(i);
303 }
304
305 ✗ void Device::setSecondOrderIntegration() {
306 ✗ controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION;
307 ✗ velocity_.resize(state_.size());
308 ✗ velocity_.setZero();
309 ✗ velocitySOUT.setConstant(velocity_);
310 }
311
312 ✗ void Device::setNoIntegration() {
313 ✗ controlInputType_ = CONTROL_INPUT_NO_INTEGRATION;
314 ✗ velocity_.resize(state_.size());
315 ✗ velocity_.setZero();
316 ✗ velocitySOUT.setConstant(velocity_);
317 }
318
319 ✗ void Device::setControlInputType(const std::string &cit) {
320 ✗ for (int i = 0; i < CONTROL_INPUT_SIZE; i++)
321 ✗ if (cit == ControlInput_s[i]) {
322 ✗ controlInputType_ = (ControlInput)i;
323 sotDEBUG(25) << "Control input type: " << ControlInput_s[i] << endl;
324 ✗ return;
325 }
326 sotDEBUG(25) << "Unrecognized control input type: " << cit << endl;
327 }
328
329 ✗ void Device::setSanityCheck(const bool &enableCheck) {
330 ✗ if (enableCheck) {
331 ✗ const Vector::Index &s = state_.size();
332 SOT_CORE_DISABLE_WARNING_PUSH
333 SOT_CORE_DISABLE_WARNING_FALLTHROUGH
334 ✗ switch (controlInputType_) {
335 ✗ case CONTROL_INPUT_TWO_INTEGRATION:
336 ✗ dgRTLOG()
337 << "Sanity check for this control is not well supported. "
338 "In order to make it work, use pinocchio and the contact forces "
339 ✗ "to estimate the joint torques for the given acceleration.\n";
340 ✗ if (s != lowerTorque_.size() || s != upperTorque_.size())
341 ✗ throw std::invalid_argument(
342 "Upper and/or lower torque bounds "
343 ✗ "do not match state size. Set them first with setTorqueBounds");
344 // fall through
345 case CONTROL_INPUT_ONE_INTEGRATION:
346 ✗ if (s != lowerVelocity_.size() || s != upperVelocity_.size())
347 ✗ throw std::invalid_argument(
348 "Upper and/or lower velocity bounds "
349 ✗ "do not match state size. Set them first with setVelocityBounds");
350 // fall through
351 case CONTROL_INPUT_NO_INTEGRATION:
352 ✗ if (s != lowerPosition_.size() || s != upperPosition_.size())
353 ✗ throw std::invalid_argument(
354 "Upper and/or lower position bounds "
355 ✗ "do not match state size. Set them first with setPositionBounds");
356 ✗ break;
357 ✗ default:
358 ✗ throw std::invalid_argument("Invalid control mode type.");
359 }
360 SOT_CORE_DISABLE_WARNING_POP
361 }
362 ✗ sanityCheck_ = enableCheck;
363 }
364
365 1 void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
366
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream oss;
367
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (lower.size() != state_.size()) {
368 ✗ oss << "Lower bound size should be " << state_.size() << ", got "
369 ✗ << lower.size();
370 ✗ throw std::invalid_argument(oss.str());
371 }
372
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (upper.size() != state_.size()) {
373 ✗ oss << "Upper bound size should be " << state_.size() << ", got "
374 ✗ << upper.size();
375 ✗ throw std::invalid_argument(oss.str());
376 }
377
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 lowerPosition_ = lower;
378
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 upperPosition_ = upper;
379 1 }
380
381 1 void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
382
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream oss;
383
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (lower.size() != velocity_.size()) {
384 ✗ oss << "Lower bound size should be " << velocity_.size() << ", got "
385 ✗ << lower.size();
386 ✗ throw std::invalid_argument(oss.str());
387 }
388
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (upper.size() != velocity_.size()) {
389 ✗ oss << "Upper bound size should be " << velocity_.size() << ", got "
390 ✗ << upper.size();
391 ✗ throw std::invalid_argument(oss.str());
392 }
393
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 lowerVelocity_ = lower;
394
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 upperVelocity_ = upper;
395 1 }
396
397 ✗ void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
398 // TODO I think the torque bounds size are state_.size()-6...
399 ✗ std::ostringstream oss;
400 ✗ if (lower.size() != state_.size()) {
401 ✗ oss << "Lower bound size should be " << state_.size() << ", got "
402 ✗ << lower.size();
403 ✗ throw std::invalid_argument(oss.str());
404 }
405 ✗ if (upper.size() != state_.size()) {
406 ✗ oss << "Lower bound size should be " << state_.size() << ", got "
407 ✗ << upper.size();
408 ✗ throw std::invalid_argument(oss.str());
409 }
410 ✗ lowerTorque_ = lower;
411 ✗ upperTorque_ = upper;
412 }
413
414 2000 void Device::increment(const double &dt) {
415 2000 int time = stateSOUT.getTime();
416 sotDEBUG(25) << "Time : " << time << std::endl;
417
418 // Run Synchronous commands and evaluate signals outside the main
419 // connected component of the graph.
420 try {
421
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 periodicCallBefore_.run(time + 1);
422 ✗ } catch (std::exception &e) {
423 ✗ dgRTLOG() << "exception caught while running periodical commands (before): "
424 ✗ << e.what() << std::endl;
425 ✗ } catch (const char *str) {
426 ✗ dgRTLOG() << "exception caught while running periodical commands (before): "
427 ✗ << str << std::endl;
428 ✗ } catch (...) {
429 ✗ dgRTLOG() << "unknown exception caught while"
430 ✗ << " running periodical commands (before)" << std::endl;
431 }
432
433 /* Force the recomputation of the control. */
434
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 controlSIN(time);
435 sotDEBUG(25) << "u" << time << " = " << controlSIN.accessCopy() << endl;
436
437 /* Integration of numerical values. This function is virtual. */
438
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 integrate(dt);
439 sotDEBUG(25) << "q" << time << " = " << state_ << endl;
440
441 /* Position the signals corresponding to sensors. */
442
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 stateSOUT.setConstant(state_);
443 2000 stateSOUT.setTime(time + 1);
444 // computation of the velocity signal
445
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
446 ✗ velocitySOUT.setConstant(velocity_);
447 ✗ velocitySOUT.setTime(time + 1);
448
1/2
✓ Branch 0 taken 2000 times.
✗ Branch 1 not taken.
 2000 } else if (controlInputType_ == CONTROL_INPUT_ONE_INTEGRATION) {
449
2/4
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
 2000 velocitySOUT.setConstant(controlSIN.accessCopy());
450 2000 velocitySOUT.setTime(time + 1);
451 }
452
2/2
✓ Branch 0 taken 8000 times.
✓ Branch 1 taken 2000 times.
 10000 for (int i = 0; i < 4; ++i) {
453
2/4
✓ Branch 0 taken 8000 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 8000 times.
✗ Branch 4 not taken.
 8000 if (!withForceSignals[i]) forcesSOUT[i]->setConstant(forceZero6);
454 }
455
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 Vector zmp(3);
456
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 zmp.fill(.0);
457
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 ZMPPreviousControllerSOUT.setConstant(zmp);
458
459 // Run Synchronous commands and evaluate signals outside the main
460 // connected component of the graph.
461 try {
462
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 periodicCallAfter_.run(time + 1);
463 ✗ } catch (std::exception &e) {
464 ✗ dgRTLOG() << "exception caught while running periodical commands (after): "
465 ✗ << e.what() << std::endl;
466 ✗ } catch (const char *str) {
467 ✗ dgRTLOG() << "exception caught while running periodical commands (after): "
468 ✗ << str << std::endl;
469 ✗ } catch (...) {
470 ✗ dgRTLOG() << "unknown exception caught while"
471 ✗ << " running periodical commands (after)" << std::endl;
472 }
473
474 // Others signals.
475
1/2
✓ Branch 1 taken 2000 times.
✗ Branch 2 not taken.
 2000 motorcontrolSOUT.setConstant(state_);
476 2000 }
477
478 // Return true if it saturates.
479 152000 inline bool saturateBounds(double &val, const double &lower,
480 const double &upper) {
481
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 152000 times.
 152000 assert(lower <= upper);
482
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 152000 times.
 152000 if (val < lower) {
483 ✗ val = lower;
484 ✗ return true;
485 }
486
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 152000 times.
 152000 if (upper < val) {
487 ✗ val = upper;
488 ✗ return true;
489 }
490 152000 return false;
491 }
492
493 #define CHECK_BOUNDS(val, lower, upper, what) \
494 for (int i = 0; i < val.size(); ++i) { \
495 double old = val(i); \
496 if (saturateBounds(val(i), lower(i), upper(i))) { \
497 std::ostringstream oss; \
498 oss << "Robot " what " bound violation at DoF " << i << ": requested " \
499 << old << " but set " << val(i) << '\n'; \
500 SEND_ERROR_STREAM_MSG(oss.str()); \
501 } \
502 }
503
504 2000 void Device::integrate(const double &dt) {
505 2000 const Vector &controlIN = controlSIN.accessCopy();
506
507
3/6
✓ Branch 0 taken 2000 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2000 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2000 times.
 2000 if (sanityCheck_ && controlIN.hasNaN()) {
508 ✗ dgRTLOG() << "Device::integrate: Control has NaN values: " << '\n'
509 ✗ << controlIN.transpose() << '\n';
510 ✗ return;
511 }
512
513
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (controlInputType_ == CONTROL_INPUT_NO_INTEGRATION) {
514 ✗ state_.tail(controlIN.size()) = controlIN;
515 ✗ return;
516 }
517
518
3/4
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1999 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2000 if (vel_control_.size() == 0) vel_control_ = Vector::Zero(controlIN.size());
519
520 // If control size is state size - 6, integrate joint angles,
521 // if control and state are of same size, integrate 6 first degrees of
522 // freedom as a translation and roll pitch yaw.
523
524
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2000 times.
 2000 if (controlInputType_ == CONTROL_INPUT_TWO_INTEGRATION) {
525 // TODO check acceleration
526 // Position increment
527 ✗ vel_control_ = velocity_.tail(controlIN.size()) + (0.5 * dt) * controlIN;
528 // Velocity integration.
529 ✗ velocity_.tail(controlIN.size()) += controlIN * dt;
530 } else {
531 2000 vel_control_ = controlIN;
532 }
533
534 // Velocity bounds check
535
1/2
✓ Branch 0 taken 2000 times.
✗ Branch 1 not taken.
 2000 if (sanityCheck_) {
536
3/28
✗ Branch 5 not taken.
✓ Branch 6 taken 76000 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
✓ Branch 49 taken 76000 times.
✓ Branch 50 taken 2000 times.
 78000 CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity");
537 }
538
539
1/2
✓ Branch 2 taken 2000 times.
✗ Branch 3 not taken.
 2000 if (vel_control_.size() == state_.size()) {
540 // Freeflyer integration
541 2000 integrateRollPitchYaw(state_, vel_control_, dt);
542 // Joints integration
543
3/6
✓ Branch 3 taken 2000 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2000 times.
✗ Branch 11 not taken.
 2000 state_.tail(state_.size() - 6) += vel_control_.tail(state_.size() - 6) * dt;
544 } else {
545 // Position integration
546 ✗ state_.tail(controlIN.size()) += vel_control_ * dt;
547 }
548
549 // Position bounds check
550
1/2
✓ Branch 0 taken 2000 times.
✗ Branch 1 not taken.
 2000 if (sanityCheck_) {
551
3/28
✗ Branch 5 not taken.
✓ Branch 6 taken 76000 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
✓ Branch 49 taken 76000 times.
✓ Branch 50 taken 2000 times.
 78000 CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position");
552 }
553 }
554
555 /* --- DISPLAY ------------------------------------------------------------ */
556
557 1 void Device::display(std::ostream &os) const {
558 1 os << name << ": " << state_ << endl
559 1 << "sanityCheck: " << sanityCheck_ << endl
560 1 << "controlInputType:" << controlInputType_ << endl;
561 1 }
562
563 1 void Device::cmdDisplay() {
564 1 std::cout << name << ": " << state_ << endl
565 1 << "sanityCheck: " << sanityCheck_ << endl
566 1 << "controlInputType:" << controlInputType_ << endl;
567 1 }
568