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 #include <utils/math/common.h>
25 
26 namespace fawkes
27 {
28 #if 0 /* just to make Emacs auto-indent happy */
29 }
30 #endif
31 
32 /** @class ForwardDriveModule <plugins/colli/drive_modes/forward_drive_mode.h>
33  * This is the Forward drive-module, for forward only movements.
34  */
35 
36 /** Constructor.
37  * @param logger The fawkes logger
38  * @param config The fawkes configuration
39  */
41  : AbstractDriveMode(logger, config)
42 {
43  logger_->log_debug("ForwardDriveModule", "(Constructor): Entering...");
45 
46  max_trans_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_trans" );
47  max_rot_ = config_->get_float( "/plugins/colli/drive_mode/normal/max_rot" );
48 
49  logger_->log_debug("ForwardDriveModule", "(Constructor): Exiting...");
50 }
51 
52 
53 /** Destructor. Destruct your local values here. */
55 {
56  logger_->log_debug("ForwardDriveModule", "(Destructor): Entering...");
58  logger_->log_debug("ForwardDriveModule", "(Destructor): Exiting...");
59 }
60 
61 
62 /** Calculate by given variables a new rotation to give for the motor to minimize curvature.
63  *
64  * DOC.: This here is the most complicated part in realizing the colli. By the given information
65  * I have to calculate a rotation I want to achieve in an optimal way.
66  * Here this is solved in an interesting way:
67  * First, check how long the curvature is, we want to drive to the target. This is done by
68  * approximating the size of the triangle around this curvature given by collision and
69  * and targetpoint and the angle between those both. Afterwards the time we have to drive
70  * with constant speed is calculated. Now we have the time we want to turn the angle. By
71  * multiplying this with a high constant (here 4), we rotate faster than we want to, but
72  * so the curvatures are not so extraordinary big. Afterwards there is an proportional
73  * part added, so we have a control factor here. (P-Regler ;-) )
74  *
75  * @return A desired rotation.
76  */
77 float
78 ForwardDriveModule::forward_curvature( float dist_to_target, float dist_to_trajec, float alpha,
79  float cur_trans, float cur_rot )
80 {
81  return 1.2f*alpha;
82 }
83 
84 
85 /** Calculate by given variables a new translation to give for the motor to
86  * minimize distance to the target.
87  *
88  * DOC.: This here is a fairly easy routine after the previous one ;-). It calculates
89  * to a proposed rotation the translation part. This means, in relation to
90  * distances to target and trajec and the current values we calculate a new one.
91  *
92  * @return A desired translation.
93  */
94 float
95 ForwardDriveModule::forward_translation( float dist_to_target, float dist_to_front, float alpha,
96  float cur_trans, float cur_rot, float des_rot )
97 {
98  if( fabs(alpha) >= M_PI_2 ) {
99  // target is more than +-90° away. Turn without driving first
100  return 0.f;
101  }
102 
103  float des_trans = 0.f;
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  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 
134  // NEW STUFF
135  float trans_target = 10000.f;
136  float trans_front = 10000.f;
137 
138  if ( stop_at_target_ == true )
139  trans_target = guarantee_trans_stop( dist_to_target, cur_trans, des_trans );
140 
141  // And the next collision point
142  if ( dist_to_front > 0.f && dist_to_front < dist_to_target )
143  trans_front = guarantee_trans_stop( dist_to_front, cur_trans, des_trans );
144  // NEW STUFF END HERE
145 
146  des_trans = std::min( des_trans, std::min( trans_target, trans_front ) );
147 
148  return des_trans;
149 }
150 
151 
152 /* ************************************************************************** */
153 /* *********************** U P D A T E ************************* */
154 /* ************************************************************************** */
155 
156 /** Calculate here your desired settings. What you desire is checked afterwards to the current
157  * settings of the physical boundaries, but take care also.
158  *
159  * How you do this is up to you, but be careful, our hardware is expensive!!!!
160  *
161  * Available are:
162  *
163  * target_ --> current target coordinates to drive to
164  * robot_ --> current robot coordinates
165  * robot_vel_ --> current Motor velocities
166  *
167  * local_target_ --> our local target found by the search component we want to reach
168  * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
169  *
170  * orient_at_target_ --> Do we have to orient ourself at the target?
171  * stop_at_target_ --> Do we have to stop really ON the target?
172  *
173  * Afterwards filled should be:
174  *
175  * proposed_ --> Desired translation and rotation speed
176  *
177  * Those values are questioned after an update() was called.
178  */
179 void
181 {
182  proposed_.x = proposed_.y = proposed_.rot = 0.f;
183 
184  float dist_to_target = sqrt( sqr(local_target_.x) + sqr(local_target_.y) );
185  float alpha = atan2( local_target_.y, local_target_.x );
186  float dist_to_trajec = sqrt( sqr(local_trajec_.x) + sqr(local_trajec_.y) );
187 
188  // last time border check............. IMPORTANT!!!
189  // because the motorinstructor just tests robots physical borders.
190  if ( dist_to_target >= 0.04 ) {
191  // Calculate ideal rotation and translation
192  proposed_.rot = forward_curvature( dist_to_target, dist_to_trajec, alpha,
194 
195  proposed_.x = forward_translation( dist_to_target, dist_to_trajec, alpha,
197 
198  // Track relation between proposed-rotation and max-rotation. Use this to adjust the
199  // proposed-translation. Only required if the value is smaller than 1, otherwise we are not
200  // reducing the proposed-rotation, because it is smaller than max-rotaion
201  float trans_correction = fabs( max_rot_ / proposed_.rot );
202  if( trans_correction < 1.f ) {
203  // for now we simply reduce the translation quadratically to how much the rotation has been reduced
204  proposed_.x *= trans_correction * trans_correction;
205  }
206 
207  // Check rotation limits.
208  // Remember, possible reduction of rotation has been considered already (trans_correction)
209  if (proposed_.rot > max_rot_)
211  else if (proposed_.rot < -max_rot_)
213 
214  // Check translation limits
215  proposed_.x = std::max( 0.f, std::min (proposed_.x, max_trans_) );
216  // maybe consider adjusting rotation again, in case we had to reduce the translation
217  }
218 }
219 
220 } // namespace fawkes
float x
Translation in x-direction.
Definition: types.h:61
cart_coord_2d_t local_target_
local target
Logger * logger_
The fawkes logger.
Fawkes library namespace.
colli_trans_rot_t proposed_
proposed translation and rotation for next timestep
bool stop_at_target_
flag if stopping on or after target
virtual void update()
Calculate here your desired settings.
float max_trans_
The maximum translation speed.
float y
Translation in y-direction.
Definition: types.h:62
float rot
Rotation around z-axis.
Definition: types.h:63
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
float max_rot_
The maximum rotation speed.
NavigatorInterface::DriveMode drive_mode_
the drive mode name
colli_trans_rot_t robot_vel_
current robot velocity
float guarantee_trans_stop(float distance, float current_trans, float desired_trans)
Get velocity that guarantees a stop for a given distance.
float lin_interpol(float x, float left, float right, float bot, float top)
Perform linear interpolation.
Configuration * config_
The fawkes configuration.
This is the base class which calculates drive modes.
ForwardDriveModule(Logger *logger, Configuration *config)
Constructor.
float y
y coordinate
Definition: types.h:61
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
float robot_speed_
current robo translation velocity
Interface for configuration handling.
Definition: config.h:67
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
cart_coord_2d_t local_trajec_
local trajectory
Interface for logging.
Definition: logger.h:34
float x
x coordinate
Definition: types.h:60