Fawkes API Fawkes Development Version
forward_drive_mode.cpp
1
2/***************************************************************************
3 * forward_drive_mode.cpp - Implementation of drive-mode "forward"
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013-2014 Bahram Maleki-Fard
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#include "forward_drive_mode.h"
24
25#include <utils/math/common.h>
26
27namespace fawkes {
28
29/** @class ForwardDriveModule <plugins/colli/drive_modes/forward_drive_mode.h>
30 * This is the Forward drive-module, for forward only movements.
31 */
32
33/** Constructor.
34 * @param logger The fawkes logger
35 * @param config The fawkes configuration
36 */
38: AbstractDriveMode(logger, config)
39{
40 logger_->log_debug("ForwardDriveModule", "(Constructor): Entering...");
42
43 max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
44 max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
45
46 logger_->log_debug("ForwardDriveModule", "(Constructor): Exiting...");
47}
48
49/** Destructor. Destruct your local values here. */
51{
52 logger_->log_debug("ForwardDriveModule", "(Destructor): Entering...");
54 logger_->log_debug("ForwardDriveModule", "(Destructor): Exiting...");
55}
56
57/** Calculate by given variables a new rotation to give for the motor to minimize curvature.
58 *
59 * DOC.: This here is the most complicated part in realizing the colli. By the given information
60 * I have to calculate a rotation I want to achieve in an optimal way.
61 * Here this is solved in an interesting way:
62 * First, check how long the curvature is, we want to drive to the target. This is done by
63 * approximating the size of the triangle around this curvature given by collision and
64 * and targetpoint and the angle between those both. Afterwards the time we have to drive
65 * with constant speed is calculated. Now we have the time we want to turn the angle. By
66 * multiplying this with a high constant (here 4), we rotate faster than we want to, but
67 * so the curvatures are not so extraordinary big. Afterwards there is an proportional
68 * part added, so we have a control factor here. (P-Regler ;-) )
69 *
70 * @return A desired rotation.
71 */
72float
73ForwardDriveModule::forward_curvature(float dist_to_target,
74 float dist_to_trajec,
75 float alpha,
76 float cur_trans,
77 float cur_rot)
78{
79 return 1.2f * alpha;
80}
81
82/** Calculate by given variables a new translation to give for the motor to
83 * minimize distance to the target.
84 *
85 * DOC.: This here is a fairly easy routine after the previous one ;-). It calculates
86 * to a proposed rotation the translation part. This means, in relation to
87 * distances to target and trajec and the current values we calculate a new one.
88 *
89 * @return A desired translation.
90 */
91float
92ForwardDriveModule::forward_translation(float dist_to_target,
93 float dist_to_front,
94 float alpha,
95 float cur_trans,
96 float cur_rot,
97 float des_rot)
98{
99 if (fabs(alpha) >= M_PI_2) {
100 // target is more than +-90° away. Turn without driving first
101 return 0.f;
102 }
103
104 /*
105 if ( fabs( des_rot ) >= 0.0 && fabs( des_rot ) <= 1.0 )
106 des_trans = lin_interpol( fabs( des_rot ), 1.0, 0.0, 0.7, max_trans_+0.1 );
107 else if ( fabs( des_rot ) > 1.0 )
108 des_trans = lin_interpol( fabs( des_rot ), M_PI, 1.0, 0.0, 0.7 );
109 */
110 /* We only translate if the target is in angle of +-90° (checked above!)
111 * With this interpolation: The higher the rotation, the lower the translation.
112 * Why? Because the amount of rotation is related to where the target lies. If it
113 * lies ahead, i.e. rotation is low, we can drive faster. If the rotation needs
114 * to be high to reach the target, we assume that it is better to drive slower.
115 */
116 float des_trans = lin_interpol(fabs(des_rot), 0.f, M_PI_2, max_trans_, 0.f);
117
118 // OLD STUFF!!!
119 // // check stopping on target and compare distances with choosen velocities
120 // if ( fabs( dist_to_target - dist_to_front ) < 0.2 )
121 // {
122 // if (stop_at_target_ == true)
123 // des_trans = min( des_trans, dist_to_target*1.5 );
124 // else
125 // ; // do not stop, so drive behind the target with full power
126 // }
127 // else
128 // {
129 // des_trans = min( des_trans, dist_to_front );
130 // }
131 // OLD STUFF END HERE
132
133 // NEW STUFF
134 float trans_target = 10000.f;
135 float trans_front = 10000.f;
136
137 if (stop_at_target_ == true)
138 trans_target = guarantee_trans_stop(dist_to_target, cur_trans, des_trans);
139
140 // And the next collision point
141 if (dist_to_front > 0.f && dist_to_front < dist_to_target)
142 trans_front = guarantee_trans_stop(dist_to_front, cur_trans, des_trans);
143 // NEW STUFF END HERE
144
145 des_trans = std::min(des_trans, std::min(trans_target, trans_front));
146
147 return des_trans;
148}
149
150/* ************************************************************************** */
151/* *********************** U P D A T E ************************* */
152/* ************************************************************************** */
153
154/** Calculate here your desired settings. What you desire is checked afterwards to the current
155 * settings of the physical boundaries, but take care also.
156 *
157 * How you do this is up to you, but be careful, our hardware is expensive!!!!
158 *
159 * Available are:
160 *
161 * target_ --> current target coordinates to drive to
162 * robot_ --> current robot coordinates
163 * robot_vel_ --> current Motor velocities
164 *
165 * local_target_ --> our local target found by the search component we want to reach
166 * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
167 *
168 * orient_at_target_ --> Do we have to orient ourself at the target?
169 * stop_at_target_ --> Do we have to stop really ON the target?
170 *
171 * Afterwards filled should be:
172 *
173 * proposed_ --> Desired translation and rotation speed
174 *
175 * Those values are questioned after an update() was called.
176 */
177void
179{
181
182 float dist_to_target = sqrt(sqr(local_target_.x) + sqr(local_target_.y));
183 float alpha = atan2(local_target_.y, local_target_.x);
184 float dist_to_trajec = sqrt(sqr(local_trajec_.x) + sqr(local_trajec_.y));
185
186 // last time border check............. IMPORTANT!!!
187 // because the motorinstructor just tests robots physical borders.
188 if (dist_to_target >= 0.04) {
189 // Calculate ideal rotation and translation
190 proposed_.rot =
191 forward_curvature(dist_to_target, dist_to_trajec, alpha, robot_speed_, robot_vel_.rot);
192
193 proposed_.x = forward_translation(
194 dist_to_target, dist_to_trajec, alpha, robot_speed_, robot_vel_.rot, proposed_.rot);
195
196 // Track relation between proposed-rotation and max-rotation. Use this to adjust the
197 // proposed-translation. Only required if the value is smaller than 1, otherwise we are not
198 // reducing the proposed-rotation, because it is smaller than max-rotaion
199 float trans_correction = fabs(max_rot_ / proposed_.rot);
200 if (trans_correction < 1.f) {
201 // for now we simply reduce the translation quadratically to how much the rotation has been reduced
202 proposed_.x *= trans_correction * trans_correction;
203 }
204
205 // Check rotation limits.
206 // Remember, possible reduction of rotation has been considered already (trans_correction)
207 if (proposed_.rot > max_rot_)
209 else if (proposed_.rot < -max_rot_)
211
212 // Check translation limits
213 proposed_.x = std::max(0.f, std::min(proposed_.x, max_trans_));
214 // maybe consider adjusting rotation again, in case we had to reduce the translation
215 }
216}
217
218} // namespace fawkes
This is the base class which calculates drive modes.
bool stop_at_target_
flag if stopping on or after target
float max_trans_
The maximum translation speed.
cart_coord_2d_t local_trajec_
local trajectory
float robot_speed_
current robo translation velocity
colli_trans_rot_t robot_vel_
current robot velocity
float max_rot_
The maximum rotation speed.
Configuration * config_
The fawkes configuration.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Logger * logger_
The fawkes logger.
cart_coord_2d_t local_target_
local target
Interface for configuration handling.
Definition: config.h:68
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
ForwardDriveModule(Logger *logger, Configuration *config)
Constructor.
virtual void update()
Calculate here your desired settings.
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
@ Forward
Moving forward constant.
@ MovingNotAllowed
Moving not allowed constant.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
float x
Translation in x-direction.
Definition: types.h:61
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63