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 
27 namespace 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  */
37 ForwardDriveModule::ForwardDriveModule(Logger *logger, Configuration *config)
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  */
72 float
73 ForwardDriveModule::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  */
91 float
92 ForwardDriveModule::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  */
177 void
179 {
180  proposed_.x = proposed_.y = proposed_.rot = 0.f;
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
fawkes::AbstractDriveMode::max_rot_
float max_rot_
The maximum rotation speed.
Definition: abstract_drive_mode.h:112
fawkes::ForwardDriveModule::update
virtual void update()
Calculate here your desired settings.
Definition: forward_drive_mode.cpp:184
fawkes::AbstractDriveMode::drive_mode_
NavigatorInterface::DriveMode drive_mode_
the drive mode name
Definition: abstract_drive_mode.h:106
fawkes::AbstractDriveMode::max_trans_
float max_trans_
The maximum translation speed.
Definition: abstract_drive_mode.h:111
fawkes::AbstractDriveMode::proposed_
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
Definition: abstract_drive_mode.h:104
fawkes::colli_trans_rot_t::rot
float rot
Rotation around z-axis.
Definition: types.h:70
fawkes::AbstractDriveMode::stop_at_target_
bool stop_at_target_
flag if stopping on or after target
Definition: abstract_drive_mode.h:102
fawkes::NavigatorInterface::Forward
@ Forward
Moving forward constant.
Definition: NavigatorInterface.h:68
fawkes::colli_trans_rot_t::y
float y
Translation in y-direction.
Definition: types.h:69
fawkes::cart_coord_2d_struct::y
float y
y coordinate
Definition: types.h:67
fawkes::AbstractDriveMode::robot_speed_
float robot_speed_
current robo translation velocity
Definition: abstract_drive_mode.h:96
fawkes::NavigatorInterface::MovingNotAllowed
@ MovingNotAllowed
Moving not allowed constant.
Definition: NavigatorInterface.h:67
fawkes::ForwardDriveModule::ForwardDriveModule
ForwardDriveModule(Logger *logger, Configuration *config)
Constructor.
Definition: forward_drive_mode.cpp:43
fawkes::colli_trans_rot_t::x
float x
Translation in x-direction.
Definition: types.h:68
fawkes::AbstractDriveMode::local_target_
cart_coord_2d_t local_target_
local target
Definition: abstract_drive_mode.h:98
fawkes
fawkes::AbstractDriveMode::config_
Configuration * config_
The fawkes configuration.
Definition: abstract_drive_mode.h:109
fawkes::AbstractDriveMode::guarantee_trans_stop
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
Definition: abstract_drive_mode.h:306
fawkes::ForwardDriveModule::~ForwardDriveModule
~ForwardDriveModule()
Destructor.
Definition: forward_drive_mode.cpp:56
fawkes::AbstractDriveMode::logger_
Logger * logger_
The fawkes logger.
Definition: abstract_drive_mode.h:108
fawkes::cart_coord_2d_struct::x
float x
x coordinate
Definition: types.h:66
fawkes::AbstractDriveMode::local_trajec_
cart_coord_2d_t local_trajec_
local trajectory
Definition: abstract_drive_mode.h:99
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::AbstractDriveMode::lin_interpol
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Definition: abstract_drive_mode.h:294
fawkes::AbstractDriveMode::robot_vel_
colli_trans_rot_t robot_vel_
current robot velocity
Definition: abstract_drive_mode.h:95
fawkes::sqr
double sqr(double x)
Fast square multiplication.
Definition: common.h:43
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0