trajectory.h

[詳解]

 
#pragma once
 
#include <ctrl/slalom/slalom.h>
 
namespace ctrl {
 
namespace slalom {
 
class Trajectory {
 public:
  Trajectory(const Shape& shape, const bool mirror_x = false) : shape(shape) {
    if (mirror_x) {
      this->shape.curve = shape.curve.mirror_x();
      this->shape.total = shape.total.mirror_x();
    }
  }
  void reset(const float velocity, const float th_start = 0,
             const float t_start = 0) {
    this->velocity = velocity;
    const float gain = velocity / shape.v_ref;
    ad.reset(gain * gain * gain * shape.dddth_max, gain * gain * shape.ddth_max,
             gain * shape.dth_max, 0, 0, shape.total.th, th_start, t_start);
  }
  void update(State& state, const float t, const float Ts,
              const float k_slip = 0) const {
    return Shape::integrate(ad, state, velocity, t, Ts, k_slip);
  }
  float getVelocity() const { return velocity; }
  float getTimeCurve() const { return ad.t_end(); }
  const Shape& getShape() const { return shape; }
  const AccelDesigner& getAccelDesigner() const { return ad; }
 
 protected:
  Shape shape;      
  AccelDesigner ad; 
  float velocity;   
};
 
}  // namespace slalom
}  // namespace ctrl