1 |
|
|
/********************************************************************* |
2 |
|
|
* Software License Agreement (BSD License) |
3 |
|
|
* |
4 |
|
|
* Copyright (c) 2013, PAL Robotics, S.L. |
5 |
|
|
* All rights reserved. |
6 |
|
|
* |
7 |
|
|
* Redistribution and use in source and binary forms, with or without |
8 |
|
|
* modification, are permitted provided that the following conditions |
9 |
|
|
* are met: |
10 |
|
|
* |
11 |
|
|
* * Redistributions of source code must retain the above copyright |
12 |
|
|
* notice, this list of conditions and the following disclaimer. |
13 |
|
|
* * Redistributions in binary form must reproduce the above |
14 |
|
|
* copyright notice, this list of conditions and the following |
15 |
|
|
* disclaimer in the documentation and/or other materials provided |
16 |
|
|
* with the distribution. |
17 |
|
|
* * Neither the name of the PAL Robotics nor the names of its |
18 |
|
|
* contributors may be used to endorse or promote products derived |
19 |
|
|
* from this software without specific prior written permission. |
20 |
|
|
* |
21 |
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
22 |
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
23 |
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
24 |
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
25 |
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
26 |
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
27 |
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
28 |
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
29 |
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
30 |
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
31 |
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
32 |
|
|
* POSSIBILITY OF SUCH DAMAGE. |
33 |
|
|
*********************************************************************/ |
34 |
|
|
|
35 |
|
|
/* |
36 |
|
|
* Author: Bence Magyar, Enrique Fernández |
37 |
|
|
*/ |
38 |
|
|
|
39 |
|
|
#include <pinocchio/fwd.hpp> |
40 |
|
|
// include pinocchio before boost |
41 |
|
|
|
42 |
|
|
#include <diff_drive_controller.h> |
43 |
|
|
#include <dynamic-graph/command-bind.h> |
44 |
|
|
#include <dynamic-graph/command-direct-getter.h> |
45 |
|
|
#include <dynamic-graph/command-direct-setter.h> |
46 |
|
|
#include <dynamic-graph/factory.h> |
47 |
|
|
|
48 |
|
|
#include <cmath> |
49 |
|
|
|
50 |
|
|
namespace dynamicgraph { |
51 |
|
|
|
52 |
|
|
DiffDriveController::DiffDriveController(const std::string& name) |
53 |
|
|
: Entity(name), |
54 |
|
|
baseVelSIN(NULL, |
55 |
|
|
"DiffDriveController(" + name + ")::input(vector)::baseVelIn"), |
56 |
|
|
wheelsVelSOUT("DiffDriveController(" + name + |
57 |
|
|
")::output(vector)::wheelsVelOut"), |
58 |
|
|
basePoseSOUT("DiffDriveController(" + name + |
59 |
|
|
")::output(vector)::basePoseOut"), |
60 |
|
|
baseVelSOUT("DiffDriveController(" + name + |
61 |
|
|
")::output(vector)::baseVelOut"), |
62 |
|
|
wheelsPosSIN(NULL, "DiffDriveController(" + name + |
63 |
|
|
")::input(vector)::wheelsPosIn"), |
64 |
|
|
dt_(-1), |
65 |
|
|
openLoop_(true), |
66 |
|
|
wheelSeparation_(0.0), |
67 |
|
|
wheelRadius_(0.0), |
68 |
|
|
wheelSeparationMultiplier_(1.0), |
69 |
|
|
leftWheelRadiusMultiplier_(1.0), |
70 |
|
|
rightWheelRadiusMultiplier_(1.0) { |
71 |
|
|
wheelsVelSOUT.setFunction( |
72 |
|
|
boost::bind(&DiffDriveController::computeControl, this, _1, _2)); |
73 |
|
|
basePoseSOUT.setFunction( |
74 |
|
|
boost::bind(&DiffDriveController::computeBasePose, this, _1, _2)); |
75 |
|
|
baseVelSOUT.setFunction( |
76 |
|
|
boost::bind(&DiffDriveController::computeBaseVel, this, _1, _2)); |
77 |
|
|
signalRegistration(baseVelSIN << wheelsVelSOUT << basePoseSOUT << baseVelSOUT |
78 |
|
|
<< wheelsPosSIN); |
79 |
|
|
|
80 |
|
|
wheelsVelSOUT.addDependency(baseVelSIN); |
81 |
|
|
wheelsVelSOUT.addDependency(baseVelSOUT); |
82 |
|
|
|
83 |
|
|
baseVelSOUT.addDependency(basePoseSOUT); |
84 |
|
|
|
85 |
|
|
basePoseSOUT.addDependency(baseVelSIN); |
86 |
|
|
|
87 |
|
|
// Add commands |
88 |
|
|
addCommand("setOpenLoop", |
89 |
|
|
command::makeDirectSetter( |
90 |
|
|
*this, &openLoop_, |
91 |
|
|
"Enable / disable closed loop control of wheels position.\n" |
92 |
|
|
"When closed loop control is enable, the true base position " |
93 |
|
|
"must be \n" |
94 |
|
|
"provided in signal wheelsPosSIN.")); |
95 |
|
|
addCommand("getOpenLoop", |
96 |
|
|
command::makeDirectGetter( |
97 |
|
|
*this, &openLoop_, |
98 |
|
|
"Enable / disable closed loop control of wheels position.\n" |
99 |
|
|
"When closed loop control is enable, the true base position " |
100 |
|
|
"must be \n" |
101 |
|
|
"provided in signal wheelsPosSIN.")); |
102 |
|
|
addCommand("setWheelSeparation", |
103 |
|
|
command::makeDirectSetter( |
104 |
|
|
*this, &wheelSeparation_, |
105 |
|
|
"Wheel separation, wrt the midpoint of the wheel width")); |
106 |
|
|
addCommand("getWheelSeparation", |
107 |
|
|
command::makeDirectGetter( |
108 |
|
|
*this, &wheelSeparation_, |
109 |
|
|
"Wheel separation, wrt the midpoint of the wheel width")); |
110 |
|
|
addCommand("setWheelRadius", |
111 |
|
|
command::makeDirectSetter(*this, &wheelRadius_, |
112 |
|
|
"Wheel radius (assuming it's the same " |
113 |
|
|
"for the left and right wheels")); |
114 |
|
|
addCommand("getWheelRadius", |
115 |
|
|
command::makeDirectGetter(*this, &wheelRadius_, |
116 |
|
|
"Wheel radius (assuming it's the same " |
117 |
|
|
"for the left and right wheels")); |
118 |
|
|
addCommand("setPeriod", command::makeCommandVoid1( |
119 |
|
|
*this, &DiffDriveController::setPeriod, |
120 |
|
|
command::docCommandVoid1("Set period.", |
121 |
|
|
" double: the period"))); |
122 |
|
|
addCommand("getPeriod", command::makeDirectGetter(*this, &dt_, "the period")); |
123 |
|
|
addCommand( |
124 |
|
|
"resetOdometryAccumulators", |
125 |
|
|
command::makeCommandVoid0( |
126 |
|
|
*this, &DiffDriveController::resetOdometryAccumulators, |
127 |
|
|
command::docCommandVoid0("Reset accumulators used for velocity " |
128 |
|
|
"estimation from encoders."))); |
129 |
|
|
|
130 |
|
|
addCommand( |
131 |
|
|
"setAngularLimits", |
132 |
|
|
command::makeCommandVoid4( |
133 |
|
|
*this, &DiffDriveController::setAngularLimits, |
134 |
|
|
command::docCommandVoid4( |
135 |
|
|
"Set angular limits.", |
136 |
|
|
" int (order): 1, 2 or 3 for velocity, acceleration or jerk", |
137 |
|
|
" bool: to enable/disable the limits", " double: the lower limit", |
138 |
|
|
" double: the upper limit"))); |
139 |
|
|
addCommand( |
140 |
|
|
"setLinearLimits", |
141 |
|
|
command::makeCommandVoid4( |
142 |
|
|
*this, &DiffDriveController::setLinearLimits, |
143 |
|
|
command::docCommandVoid4( |
144 |
|
|
"Set linear limits.", |
145 |
|
|
" int (order): 1, 2 or 3 for velocity, acceleration or jerk", |
146 |
|
|
" bool: to enable/disable the limits", " double: the lower limit", |
147 |
|
|
" double: the upper limit"))); |
148 |
|
|
} |
149 |
|
|
|
150 |
|
|
Vector& DiffDriveController::computeControl(Vector& control, const int& time) { |
151 |
|
|
control.resize(2); |
152 |
|
|
baseVelSOUT.recompute(time); |
153 |
|
|
|
154 |
|
|
// Apply (possibly new) multipliers: |
155 |
|
|
const double ws = wheelSeparationMultiplier_ * wheelSeparation_; |
156 |
|
|
const double lwr = leftWheelRadiusMultiplier_ * wheelRadius_; |
157 |
|
|
const double rwr = rightWheelRadiusMultiplier_ * wheelRadius_; |
158 |
|
|
|
159 |
|
|
// MOVE ROBOT |
160 |
|
|
// Retreive current velocity command: |
161 |
|
|
const Vector& baseVelVect = baseVelSIN.access(time); |
162 |
|
|
assert(baseVelVect.size() >= 6); |
163 |
|
|
// TODO check that baseVel is of form (lin_a, lin_b, 0, 0, 0, ang) |
164 |
|
|
// such that (lin_a, lin_b) is parallel to (odometry_.getX(), getY()) |
165 |
|
|
Command baseVel; |
166 |
|
|
// Project the linear velocity onto the current X axis |
167 |
|
|
// baseVel.lin = baseVelVect[0]; |
168 |
|
|
double heading = odometry_.getHeading(); |
169 |
|
|
Eigen::RowVector2d cs(cos(heading), sin(heading)); |
170 |
|
|
baseVel.lin = cs * baseVelVect.head<2>(); |
171 |
|
|
baseVel.ang = baseVelVect[5]; |
172 |
|
|
baseVel.time = time; |
173 |
|
|
|
174 |
|
|
// Limit velocities and accelerations: |
175 |
|
|
limiterLin_.limit(baseVel.lin, lastBaseVel_.lin, penultimateBaseVel_.lin, |
176 |
|
|
dt_); |
177 |
|
|
limiterAng_.limit(baseVel.ang, lastBaseVel_.ang, penultimateBaseVel_.ang, |
178 |
|
|
dt_); |
179 |
|
|
|
180 |
|
|
penultimateBaseVel_ = lastBaseVel_; |
181 |
|
|
lastBaseVel_ = baseVel; |
182 |
|
|
|
183 |
|
|
// Compute wheels velocities: |
184 |
|
|
control.resize(2); |
185 |
|
|
control[0] = (baseVel.lin - baseVel.ang * ws / 2.0) / lwr; // Left |
186 |
|
|
control[1] = (baseVel.lin + baseVel.ang * ws / 2.0) / rwr; // Right |
187 |
|
|
|
188 |
|
|
return control; |
189 |
|
|
} |
190 |
|
|
|
191 |
|
|
Vector& DiffDriveController::computeBaseVel(Vector& baseVel, const int& time) { |
192 |
|
|
baseVel.resize(2); |
193 |
|
|
basePoseSOUT.recompute(time); |
194 |
|
|
baseVel[0] = odometry_.getLinear(); |
195 |
|
|
baseVel[1] = odometry_.getAngular(); |
196 |
|
|
return baseVel; |
197 |
|
|
} |
198 |
|
|
|
199 |
|
|
Vector& DiffDriveController::computeBasePose(Vector& basePos, const int& time) { |
200 |
|
|
basePos.resize(3); |
201 |
|
|
if (!computeOdometry(time)) { |
202 |
|
|
basePos.setZero(); |
203 |
|
|
return basePos; |
204 |
|
|
} |
205 |
|
|
basePos[0] = odometry_.getX(); |
206 |
|
|
basePos[1] = odometry_.getY(); |
207 |
|
|
basePos[2] = odometry_.getHeading(); |
208 |
|
|
return basePos; |
209 |
|
|
} |
210 |
|
|
|
211 |
|
|
bool DiffDriveController::computeOdometry(const int& time) { |
212 |
|
|
if (dt_ < 0) { |
213 |
|
|
SEND_ERROR_STREAM_MSG("Period must be set to a position value."); |
214 |
|
|
return false; |
215 |
|
|
} |
216 |
|
|
// // Check if odometry has already been computed. |
217 |
|
|
// if ( wheelsVelSOUT.getTime() >= time |
218 |
|
|
// || basePoseSOUT .getTime() >= time |
219 |
|
|
// || baseVelSOUT .getTime() >= time) |
220 |
|
|
// return true; |
221 |
|
|
|
222 |
|
|
// Apply (possibly new) multipliers: |
223 |
|
|
const double ws = wheelSeparationMultiplier_ * wheelSeparation_; |
224 |
|
|
const double lwr = leftWheelRadiusMultiplier_ * wheelRadius_; |
225 |
|
|
const double rwr = rightWheelRadiusMultiplier_ * wheelRadius_; |
226 |
|
|
|
227 |
|
|
odometry_.setWheelParams(ws, lwr, rwr); |
228 |
|
|
|
229 |
|
|
// COMPUTE ODOMETRY |
230 |
|
|
bool openLoop = openLoop_; |
231 |
|
|
if (!openLoop_ && wheelsPosSIN.isPlugged()) { |
232 |
|
|
SEND_ERROR_STREAM_MSG( |
233 |
|
|
"wheelsPosSIN must be plugged for closed loop control."); |
234 |
|
|
openLoop = true; |
235 |
|
|
} |
236 |
|
|
// TODO lastBaseVel_ should be reset to zero when the velocity has not been |
237 |
|
|
// applied. |
238 |
|
|
// double dt = dt_ * (time - lastBaseVel_.time); |
239 |
|
|
double dt = dt_; |
240 |
|
|
if (openLoop) { |
241 |
|
|
odometry_.updateOpenLoop(lastBaseVel_.lin, lastBaseVel_.ang, dt); |
242 |
|
|
} else { |
243 |
|
|
const Vector& wheelsPos = wheelsPosSIN.access(time); |
244 |
|
|
// TODO |
245 |
|
|
// if (std::isnan(lp) || std::isnan(rp)) |
246 |
|
|
// return; |
247 |
|
|
|
248 |
|
|
// Estimate linear and angular velocity using joint information |
249 |
|
|
odometry_.update(wheelsPos[0], wheelsPos[1], dt); |
250 |
|
|
} |
251 |
|
|
return true; |
252 |
|
|
} |
253 |
|
|
|
254 |
|
|
void DiffDriveController::setPeriod(const double& dt) { |
255 |
|
|
if (dt <= 0) throw std::invalid_argument("Period should be positive."); |
256 |
|
|
dt_ = dt; |
257 |
|
|
} |
258 |
|
|
|
259 |
|
|
void DiffDriveController::resetOdometryAccumulators() { odometry_.init(); } |
260 |
|
|
|
261 |
|
|
void DiffDriveController::setAngularLimits(const int& order, const bool& enable, |
262 |
|
|
const double& min, |
263 |
|
|
const double& max) { |
264 |
|
|
switch (order) { |
265 |
|
|
case 1: |
266 |
|
|
limiterAng_.has_velocity_limits = enable; |
267 |
|
|
limiterAng_.max_velocity = max; |
268 |
|
|
limiterAng_.min_velocity = min; |
269 |
|
|
break; |
270 |
|
|
case 2: |
271 |
|
|
limiterAng_.has_acceleration_limits = enable; |
272 |
|
|
limiterAng_.max_acceleration = max; |
273 |
|
|
limiterAng_.min_acceleration = min; |
274 |
|
|
break; |
275 |
|
|
case 3: |
276 |
|
|
limiterAng_.has_jerk_limits = enable; |
277 |
|
|
limiterAng_.max_jerk = max; |
278 |
|
|
limiterAng_.min_jerk = min; |
279 |
|
|
break; |
280 |
|
|
default: |
281 |
|
|
throw std::invalid_argument("Order must be either 1, 2 or 3"); |
282 |
|
|
} |
283 |
|
|
} |
284 |
|
|
|
285 |
|
|
void DiffDriveController::setLinearLimits(const int& order, const bool& enable, |
286 |
|
|
const double& min, |
287 |
|
|
const double& max) { |
288 |
|
|
switch (order) { |
289 |
|
|
case 1: |
290 |
|
|
limiterLin_.has_velocity_limits = enable; |
291 |
|
|
limiterLin_.max_velocity = max; |
292 |
|
|
limiterLin_.min_velocity = min; |
293 |
|
|
break; |
294 |
|
|
case 2: |
295 |
|
|
limiterLin_.has_acceleration_limits = enable; |
296 |
|
|
limiterLin_.max_acceleration = max; |
297 |
|
|
limiterLin_.min_acceleration = min; |
298 |
|
|
break; |
299 |
|
|
case 3: |
300 |
|
|
limiterLin_.has_jerk_limits = enable; |
301 |
|
|
limiterLin_.max_jerk = max; |
302 |
|
|
limiterLin_.min_jerk = min; |
303 |
|
|
break; |
304 |
|
|
default: |
305 |
|
|
throw std::invalid_argument("Order must be either 1, 2 or 3"); |
306 |
|
|
} |
307 |
|
|
} |
308 |
|
|
|
309 |
|
|
Vector DiffDriveController::getAngularLimits(const int& order) { |
310 |
|
|
Vector res(2); |
311 |
|
|
switch (order) { |
312 |
|
|
case 1: |
313 |
|
|
if (!limiterAng_.has_velocity_limits) return Vector(); |
314 |
|
|
res << limiterAng_.min_velocity, limiterAng_.max_velocity; |
315 |
|
|
break; |
316 |
|
|
case 2: |
317 |
|
|
if (!limiterAng_.has_acceleration_limits) return Vector(); |
318 |
|
|
res << limiterAng_.min_acceleration, limiterAng_.max_acceleration; |
319 |
|
|
break; |
320 |
|
|
case 3: |
321 |
|
|
if (!limiterAng_.has_jerk_limits) return Vector(); |
322 |
|
|
res << limiterAng_.min_jerk, limiterAng_.max_jerk; |
323 |
|
|
break; |
324 |
|
|
default: |
325 |
|
|
throw std::invalid_argument("Order must be either 1, 2 or 3"); |
326 |
|
|
} |
327 |
|
|
return res; |
328 |
|
|
} |
329 |
|
|
|
330 |
|
|
Vector DiffDriveController::getLinearLimits(const int& order) { |
331 |
|
|
Vector res(2); |
332 |
|
|
switch (order) { |
333 |
|
|
case 1: |
334 |
|
|
if (!limiterLin_.has_velocity_limits) return Vector(); |
335 |
|
|
res << limiterLin_.min_velocity, limiterLin_.max_velocity; |
336 |
|
|
break; |
337 |
|
|
case 2: |
338 |
|
|
if (!limiterLin_.has_acceleration_limits) return Vector(); |
339 |
|
|
res << limiterLin_.min_acceleration, limiterLin_.max_acceleration; |
340 |
|
|
break; |
341 |
|
|
case 3: |
342 |
|
|
if (!limiterLin_.has_jerk_limits) return Vector(); |
343 |
|
|
res << limiterLin_.min_jerk, limiterLin_.max_jerk; |
344 |
|
|
break; |
345 |
|
|
default: |
346 |
|
|
throw std::invalid_argument("Order must be either 1, 2 or 3"); |
347 |
|
|
} |
348 |
|
|
return res; |
349 |
|
|
} |
350 |
|
|
|
351 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(DiffDriveController, "DiffDriveController"); |
352 |
|
|
} // namespace dynamicgraph |