Fawkes API Fawkes Development Version
forward_omni_drive_mode.cpp
1
2/***************************************************************************
3 * forward_omni_drive_mode.cpp - Implementation of drive-mode "forward"
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2014 Tobias Neumann
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "forward_omni_drive_mode.h"
23
24#include <utils/math/angle.h>
25#include <utils/math/common.h>
26
27#include <cmath>
28
29namespace fawkes {
30
31/** @class ForwardOmniDriveModule <plugins/colli/drive_modes/forward_drive_mode.h>
32 * This is the SlowForward drive-module, for slow forward only movements.
33 */
34
35/** Constructor.
36 * @param logger The fawkes logger
37 * @param config The fawkes configuration
38 */
40: AbstractDriveMode(logger, config)
41{
42 logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Entering...");
44
45 max_trans_ = config_->get_float("/plugins/colli/drive_mode/normal/max_trans");
46 max_rot_ = config_->get_float("/plugins/colli/drive_mode/normal/max_rot");
47
48 logger_->log_debug("ForwardOmniDriveModule", "(Constructor): Exiting...");
49}
50
51/** Descturctor. Destruct your local values here. */
53{
54 logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Entering...");
56 logger_->log_debug("ForwardOmniDriveModule", "(Destructor): Exiting...");
57}
58
59void
60ForwardOmniDriveModule::calculate_rotation(float ori_alpha_target,
61 float ori_alpha_next_target,
62 float dist_to_target,
63 float angle_allowed_to_next_target)
64{
65 // first calculate desired angle
66 float des_alpha;
67 if (!std::isfinite(ori_alpha_next_target)) {
68 des_alpha = ori_alpha_target;
69 } else {
70 float angle_min = ori_alpha_target - angle_allowed_to_next_target;
71 float angle_max = ori_alpha_target + angle_allowed_to_next_target;
72 des_alpha = normalize_mirror_rad(std::max(angle_min, std::min(ori_alpha_target, angle_max)));
73 }
74
75 // then choose rotation speed, depending on desired angle
76 const float _TURN_MAX_SPEED_LIMIT_ = M_PI_4;
77 if (des_alpha > _TURN_MAX_SPEED_LIMIT_) {
79 } else if (des_alpha < -_TURN_MAX_SPEED_LIMIT_) {
81 } else {
82 proposed_.rot = des_alpha * (max_rot_ / _TURN_MAX_SPEED_LIMIT_);
83 }
84}
85
86void
87ForwardOmniDriveModule::calculate_translation(float dist_to_target,
88 float ori_alpha_target,
89 float dec_factor)
90{
91 float part_x = 0;
92 float part_y = 0;
93 if (!(local_target_.x == 0 && local_target_.y == 0)) {
94 part_x = local_target_.x / (fabs(local_target_.x) + fabs(local_target_.y));
95 part_y = local_target_.y / (fabs(local_target_.x) + fabs(local_target_.y));
96 }
97 proposed_.x = part_x * max_trans_ * dec_factor;
98 proposed_.y = part_y * max_trans_ * dec_factor;
99
100 // Check translation limits
101 if (proposed_.x < 0. || fabs(ori_alpha_target) >= M_PI_2 - 0.2) {
102 proposed_.x = 0.;
103 proposed_.y = 0.;
104 }
105}
106
107/* ************************************************************************** */
108/* *********************** U P D A T E ************************* */
109/* ************************************************************************** */
110
111/** Calculate here your desired settings. What you desire is checked afterwards to the current
112 * settings of the physical boundaries, but take care also.
113 *
114 * How you do this is up to you, but be careful, our hardware is expensive!!!!
115 *
116 * Available are:
117 *
118 * target_ --> current target coordinates to drive to
119 * robot_ --> current robot coordinates
120 * robot_vel_ --> current Motor velocities
121 *
122 * local_target_ --> our local target found by the search component we want to reach
123 * local_trajec_ --> The point we would collide with, if we would drive WITHOUT Rotation
124 *
125 * orient_at_target_ --> Do we have to orient ourself at the target?
126 * stop_at_target_ --> Do we have to stop really ON the target?
127 *
128 * Afterwards filled should be:
129 *
130 * proposed_ --> Desired translation and rotation speed
131 *
132 * Those values are questioned after an update() was called.
133 */
134void
136{
137 proposed_.x = 0.f;
138 proposed_.rot = 0.f;
139
140 float dist_to_target = sqrt(sqr(local_target_.x) + sqr(local_target_.y));
141 float alpha_target = normalize_mirror_rad(atan2(local_target_.y, local_target_.x));
142 float alpha_next_target = angle_distance_signed(robot_.ori, target_.ori);
143
144 // last time border check............. IMPORTANT!!!
145 // because the motorinstructor just tests robots physical borders.
146 if (dist_to_target < 0.04) {
148
149 } else {
150 float angle_tollerance = M_PI_4;
151 calculate_rotation(alpha_target, alpha_next_target, dist_to_target, angle_tollerance / 2.);
152
153 float dec_factor = 1;
154 if (fabs(alpha_target) >= angle_tollerance) { // if we need to turn a lot => drive slower
155 dec_factor = 0.5;
156 }
157
158 calculate_translation(dist_to_target, alpha_target, dec_factor);
159
160 if (stop_at_target_) {
161 float target_rel = std::sqrt(sqr(target_.x - robot_.x) + sqr(target_.y - robot_.y));
162 float robo_trans = std::sqrt(sqr(robot_vel_.x) + sqr(robot_vel_.y));
163 float proposed_trans = std::sqrt(sqr(proposed_.x) + sqr(proposed_.y));
164 float target_trans = guarantee_trans_stop(target_rel, robo_trans, proposed_trans);
165
166 float des = fabs(target_trans / proposed_trans);
167 if (proposed_trans == 0) {
168 des = 0;
169 }
170
171 proposed_.x *= des;
172 proposed_.y *= des;
173 }
174 }
175}
176
177} // 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.
field_pos_t target_
current target
field_pos_t robot_
current robot pos
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 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.
ForwardOmniDriveModule(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 normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians.
Definition: angle.h:134
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
float y
y coordinate in meters
Definition: types.h:127
float ori
orientation
Definition: types.h:128
float x
x coordinate in meters
Definition: types.h:126