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 |