YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoardTrajectory.h
Go to the documentation of this file.
1#pragma once
2
3#include <chrono>
4#include <memory>
5#include <mutex>
6
7#include <yarp/os/Log.h>
8#include <yarp/os/LogStream.h>
9
16
17//------------------------------------------------------------------------------------------------------------------
18// Watchdog
19//------------------------------------------------------------------------------------------------------------------
20
22{
23 double m_duration;
24 double m_lastUpdate;
25
26public:
27 void reset();
28 bool isExpired();
29 void modifyDuration(double expireTime);
30 double getDuration();
31
32 Watchdog(double expireTime);
33};
34
35//------------------------------------------------------------------------------------------------------------------
36// RampFilter
37//------------------------------------------------------------------------------------------------------------------
38
40{
41private:
42 std::mutex m_mutex;
43 double m_final_reference;
44 double m_current_value;
45 double m_step;
46
47public:
48 RampFilter();
49
50 void setReference(double ref, double step);
51 void update();
52 double getCurrentValue();
53 void stop();
54};
55
56//------------------------------------------------------------------------------------------------------------------
57// TrajectoryGenerator
58//------------------------------------------------------------------------------------------------------------------
59
61{
62protected:
63 std::mutex m_mutex;
65 double m_x0;
66 double m_xf;
67 double m_speed;
74
75public:
76 virtual ~TrajectoryGenerator();
77 virtual bool initTrajectory(double current_pos,
78 double final_pos,
79 double speed,
80 double acceleration,
81 std::chrono::milliseconds controller_period) = 0;
82
83 virtual bool abortTrajectory(double limit) = 0;
84 virtual double computeTrajectory() = 0;
85 virtual double computeTrajectoryStep() = 0;
87 bool setLimits(double min, double max);
88 bool isMotionDone();
89};
90
91//------------------------------------------------------------------------------------------------------------------
92// ConstSpeedTrajectoryGenerator
93//------------------------------------------------------------------------------------------------------------------
94
96{
97public:
100
101private:
102 double p_computeTrajectory();
103 double p_computeTrajectoryStep();
104 bool p_abortTrajectory(double limit);
105
106public:
107 bool initTrajectory(double current_pos,
108 double final_pos,
109 double speed,
110 double acceleration,
111 std::chrono::milliseconds controller_period);
112
113 bool abortTrajectory(double limit) override;
114 double computeTrajectory() override;
115 double computeTrajectoryStep() override;
117};
118
120{
121public:
124
125private:
126 double m_ta;
127 double m_tb;
128 double m_tf;
129 double m_tick;
130 double m_v0;
131 double m_computed_reference_velocity;
132
133 double p_computeTrajectoryStep();
134
135public:
136 bool initTrajectory(double current_pos,
137 double final_pos,
138 double speed,
139 double acceleration,
140 std::chrono::milliseconds controller_period);
141
142 bool abortTrajectory(double limit) override;
143 double computeTrajectory() override;
144 double computeTrajectoryStep() override;
146};
147
148//------------------------------------------------------------------------------------------------------------------
149// MinJerkTrajectoryGenerator
150//------------------------------------------------------------------------------------------------------------------
151
153{
154public:
157
158private:
159 double m_trajectory_coeff_c1;
160 double m_trajectory_coeff_c2;
161 double m_trajectory_coeff_c3;
162 double m_dx0;
163 double m_tf;
164 double m_prev_a;
165 double m_cur_t;
166 double m_cur_step;
167 double m_step;
168
169 double p_computeTrajectory();
170 double p_computeTrajectoryStep();
171 bool p_abortTrajectory(double limit);
172
173 double p_compute_p5f(double t);
174 double p_compute_p5f_vel(double t);
175 double p_compute_current_vel();
176
177public:
178 bool initTrajectory(double current_pos,
179 double final_pos,
180 double speed,
181 double acceleration,
182 std::chrono::milliseconds controller_period);
183
184 bool abortTrajectory(double limit) override;
185 double computeTrajectory() override;
186 double computeTrajectoryStep() override;
188};
189
190//------------------------------------------------------------------------------------------------------------------
191// TrajectoryGeneratorFactory
192//------------------------------------------------------------------------------------------------------------------
193
195{
196public:
197 static std::unique_ptr<TrajectoryGenerator> create(TrajectoryType type)
198 {
199 switch (type)
200 {
202 return std::make_unique<MinJerkTrajectoryGenerator>();
204 return std::make_unique<ConstSpeedTrajectoryGenerator>();
206 return std::make_unique<TrapezoidalSpeedTrajectoryGenerator>();
207 default:
208 yError() << "Trajectory type not supported";
209 return nullptr;
210 }
211 }
212};
213
@ TRAJECTORY_TYPE_TRAP_SPEED
@ TRAJECTORY_TYPE_CONST_SPEED
#define yError(...)
Definition Log.h:361
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
TrajectoryType getTrajectoryType() override
bool abortTrajectory(double limit) override
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
TrajectoryType getTrajectoryType() override
bool abortTrajectory(double limit) override
void setReference(double ref, double step)
static std::unique_ptr< TrajectoryGenerator > create(TrajectoryType type)
virtual double computeTrajectory()=0
virtual bool abortTrajectory(double limit)=0
virtual double computeTrajectoryStep()=0
bool setLimits(double min, double max)
virtual TrajectoryType getTrajectoryType()=0
virtual bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)=0
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
void modifyDuration(double expireTime)