Kobuki 1.0.0
C++ API for the Kobuki research robot
Loading...
Searching...
No Matches
acceleration_limiter.hpp
1
9/*****************************************************************************
10** Ifdefs
11*****************************************************************************/
12
13#ifndef KOBUKI_CORE_ACCELERATION_LIMITER_HPP_
14#define KOBUKI_CORE_ACCELERATION_LIMITER_HPP_
15
16/*****************************************************************************
17** Includes
18*****************************************************************************/
19
20#include <vector>
21#include <iomanip>
22#include <sstream>
23#include <iostream>
24#include <stdint.h>
25#include <ecl/time.hpp>
26
27/*****************************************************************************
28** Namespaces
29*****************************************************************************/
30
31namespace kobuki {
32
33/*****************************************************************************
34** Interfaces
35*****************************************************************************/
36
50class AccelerationLimiter {
51public:
52 AccelerationLimiter() :
53 is_enabled(true),
54 last_timestamp(ecl::TimeStamp()),
55 last_vx(0.0),
56 last_wz(0.0)
57 {}
58 void init(bool enable_acceleration_limiter
59 , double linear_acceleration_max_= 0.5, double angular_acceleration_max_= 3.5
60 , double linear_deceleration_max_=-0.5*1.2, double angular_deceleration_max_=-3.5*1.2)
61 {
62 is_enabled = enable_acceleration_limiter;
63 linear_acceleration_max = linear_acceleration_max_ ;
64 linear_deceleration_max = linear_deceleration_max_ ;
65 angular_acceleration_max = angular_acceleration_max_;
66 angular_deceleration_max = angular_deceleration_max_;
67 }
68
69 bool isEnabled() const { return is_enabled; }
70
78 std::vector<double> limit(const std::vector<double> &command) { return limit(command[0], command[1]); }
79
80 std::vector<double> limit(const double &vx, const double &wz)
81 {
82 std::vector<double> ret_val;
83 if( is_enabled ) {
84 //get current time
85 ecl::TimeStamp curr_timestamp;
86 //get time difference
87 ecl::TimeStamp duration = curr_timestamp - last_timestamp;
88 //calculate acceleration
89 double linear_acceleration = ((double)(vx - last_vx)) / duration; // in [m/s^2]
90 double angular_acceleration = ((double)(wz - last_wz)) / duration; // in [rad/s^2]
91
92 //std::ostringstream oss;
93 //oss << std::fixed << std::setprecision(4);
94 //oss << "[" << std::setw(6) << (double)duration << "]";
95 //oss << "[" << std::setw(6) << last_vx << ", " << std::setw(6) << last_wz << "]";
96 //oss << "[" << std::setw(6) << vx << ", " << std::setw(6) << wz << "]";
97 //oss << "[" << std::setw(6) << linear_acceleration << ", " << std::setw(6) << angular_acceleration << "]";
98
99 if( linear_acceleration > linear_acceleration_max )
100 command_vx = last_vx + linear_acceleration_max * duration;
101 else if( linear_acceleration < linear_deceleration_max )
102 command_vx = last_vx + linear_deceleration_max * duration;
103 else
104 command_vx = vx;
105 last_vx = command_vx;
106
107 if( angular_acceleration > angular_acceleration_max )
108 command_wz = last_wz + angular_acceleration_max * duration;
109 else if( angular_acceleration < angular_deceleration_max )
110 command_wz = last_wz + angular_deceleration_max * duration;
111 else
112 command_wz = wz;
113 last_wz = command_wz;
114
115 last_timestamp = curr_timestamp;
116
117 //oss << "[" << std::setw(6) << command_vx << ", " << std::setw(6) << command_wz << "]";
118 //std::cout << oss.str() << std::endl;
119
120 ret_val.push_back(command_vx);
121 ret_val.push_back(command_wz);
122 } else {
123 ret_val.push_back(0.0);
124 ret_val.push_back(0.0);
125 }
126 return ret_val;
127 }
128
129private:
130 bool is_enabled;
131 ecl::TimeStamp last_timestamp;
132
133 double last_vx, last_wz; // In [m/s] and [rad/s]
134 double command_vx, command_wz; // In [m/s] and [rad/s]
135 double linear_acceleration_max, linear_deceleration_max; // In [m/s^2]
136 double angular_acceleration_max, angular_deceleration_max; // In [rad/s^2]
137};
138
139} // namespace kobuki
140
141#endif /* KOBUKI_ACCELERATION_LIMITER__HPP_ */
kobuki::AccelerationLimiter
An acceleration limiter for the kobuki.
Definition acceleration_limiter.hpp:50
kobuki::AccelerationLimiter::limit
std::vector< double > limit(const std::vector< double > &command)
Limits the input velocity commands if gatekeeper is enabled.
Definition acceleration_limiter.hpp:78