21 m_duration = expireTime;
27 m_duration = expireTime;
43 m_final_reference = 0;
50 std::lock_guard<std::mutex> lock(m_mutex);
51 m_final_reference = ref;
57 std::lock_guard<std::mutex> lock(m_mutex);
58 m_final_reference = 0.0;
59 m_current_value = 0.0;
65 std::lock_guard<std::mutex> lock(m_mutex);
67 double error_abs = fabs(m_final_reference - m_current_value);
69 if (m_final_reference > m_current_value)
71 tmp = +(m_step / 1000.0) * 1.0;
74 tmp = -(m_step / 1000.0) * 1.0;
77 if (error_abs < fabs(tmp))
79 tmp = (m_final_reference - m_current_value);
82 m_current_value = m_current_value + tmp;
87 return m_current_value;
95 : m_trajectory_complete(true)
131double MinJerkTrajectoryGenerator::p_compute_p5f(
double t)
133 double accum = m_dx0 * t;
134 double tmp = t * t * t;
135 accum = accum + tmp * m_trajectory_coeff_c1;
137 accum = accum - tmp * m_trajectory_coeff_c2;
139 accum = accum + tmp * m_trajectory_coeff_c3;
143double MinJerkTrajectoryGenerator::p_compute_p5f_vel(
double t)
145 double accum = -2 * m_dx0 * t - m_dx0;
146 accum = accum + (30 *
m_x0 + 15 * m_dx0 - 30 *
m_xf) * (t * t);
147 accum = -accum / m_tf * (t - 1) * (t - 1);
151double MinJerkTrajectoryGenerator::p_compute_current_vel()
161 }
else if (m_cur_t < 1.0 - m_step)
164 a = p_compute_p5f_vel(m_cur_t);
170bool MinJerkTrajectoryGenerator::p_abortTrajectory(
double limit)
189 std::lock_guard<std::mutex> lock(
m_mutex);
190 bool ret = p_abortTrajectory(limit);
198 std::chrono::milliseconds controller_period)
200 std::lock_guard<std::mutex> lock(
m_mutex);
203 double speedf = fabs(speed);
212 m_dx0 = p_compute_current_vel();
214 m_prev_a = current_pos;
227 m_dx0 = m_dx0 * m_tf;
230 m_trajectory_coeff_c1 = (10 * (
m_xf -
m_x0) - 6 * dx0);
231 m_trajectory_coeff_c2 = (15 * (
m_xf -
m_x0) - 8 * dx0);
232 m_trajectory_coeff_c3 = (6 * (
m_xf -
m_x0) - 3 * dx0);
234 if (m_tf < 1 || m_tf == 0)
236 p_abortTrajectory(final_pos);
250double MinJerkTrajectoryGenerator::p_computeTrajectoryStep()
253 double delta_target = 0;
259 delta_target = target - m_prev_a;
271 delta_target = target - m_prev_a;
275 }
else if (m_cur_t < 1.0 - m_step)
278 target = p_compute_p5f(m_cur_t);
285 delta_target = target - m_prev_a;
301 std::lock_guard<std::mutex> lock(
m_mutex);
302 double ret = p_computeTrajectoryStep();
306double MinJerkTrajectoryGenerator::p_computeTrajectory()
328 }
else if (m_cur_t < 1.0 - m_step)
331 target = p_compute_p5f(m_cur_t);
351 std::lock_guard<std::mutex> lock(
m_mutex);
352 double ret = p_computeTrajectory();
377 std::chrono::milliseconds controller_period)
379 std::lock_guard<std::mutex> lock(
m_mutex);
394bool ConstSpeedTrajectoryGenerator::p_abortTrajectory(
double limit)
402 std::lock_guard<std::mutex> lock(
m_mutex);
403 bool ret = p_abortTrajectory(limit);
407double ConstSpeedTrajectoryGenerator::p_computeTrajectoryStep()
420 if (error_abs < fabs(step))
431 std::lock_guard<std::mutex> lock(
m_mutex);
432 double ret = p_computeTrajectoryStep();
438 std::lock_guard<std::mutex> lock(
m_mutex);
439 double ret = p_computeTrajectory();
443double ConstSpeedTrajectoryGenerator::p_computeTrajectory()
451 if (error_abs < step)
474 : m_computed_reference_velocity(0.0)
486 std::chrono::milliseconds controller_period)
488 std::lock_guard<std::mutex> lock(
m_mutex);
490 if (speed <= 0.0 || acceleration <= 0.0)
497 m_v0 = m_computed_reference_velocity;
504 double sign = (
m_xf >=
m_x0) ? 1.0 : -1.0;
513 if (std::abs(deltaTotal) > std::abs(deltaTriMax))
519 m_tb = m_ta + (deltaTotal - deltaRamp1 - deltaRamp2) /
m_speed;
525 = sign * std::sqrt(0.5 * (2 *
m_acceleration * deltaTotal + m_v0 * m_v0));
541 std::lock_guard<std::mutex> lock(
m_mutex);
543 m_computed_reference_velocity = 0.0;
549 std::lock_guard<std::mutex> lock(
m_mutex);
556 std::lock_guard<std::mutex> lock(
m_mutex);
557 return p_computeTrajectoryStep();
560double TrapezoidalSpeedTrajectoryGenerator::p_computeTrajectoryStep()
568 double instant = m_tick * period;
573 step = 0.5 *
m_acceleration * period * period * (2 * m_tick + 1) + m_v0 * period;
575 }
else if (instant > m_tb)
577 step =
m_acceleration * (m_tf * period - 0.5 * period * period * (2 * m_tick + 1));
578 m_computed_reference_velocity =
m_acceleration * (m_tf - instant);
582 m_computed_reference_velocity =
m_speed;
589 m_computed_reference_velocity = 0.0;
@ TRAJECTORY_TYPE_TRAP_SPEED
@ TRAJECTORY_TYPE_CONST_SPEED
@ TRAJECTORY_TYPE_MIN_JERK
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
virtual ~ConstSpeedTrajectoryGenerator()
ConstSpeedTrajectoryGenerator()
TrajectoryType getTrajectoryType() override
double computeTrajectory() override
bool abortTrajectory(double limit) override
double computeTrajectoryStep() override
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
TrajectoryType getTrajectoryType() override
MinJerkTrajectoryGenerator()
double computeTrajectoryStep() override
virtual ~MinJerkTrajectoryGenerator()
bool abortTrajectory(double limit) override
double computeTrajectory() override
void setReference(double ref, double step)
virtual ~TrajectoryGenerator()
bool m_trajectory_complete
double m_controllerPeriodMilliseconds
double m_computed_reference
bool setLimits(double min, double max)
TrapezoidalSpeedTrajectoryGenerator()
bool abortTrajectory(double limit) override
double computeTrajectoryStep() override
bool initTrajectory(double current_pos, double final_pos, double speed, double acceleration, std::chrono::milliseconds controller_period)
double computeTrajectory() override
virtual ~TrapezoidalSpeedTrajectoryGenerator()
TrajectoryType getTrajectoryType() override
void modifyDuration(double expireTime)
Watchdog(double expireTime)
double now()
Return the current time in seconds, relative to an arbitrary starting point.