TrapezoidalTrajectory.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __TRAPEZOIDAL_TRAJECTORY_HPP__
#define __TRAPEZOIDAL_TRAJECTORY_HPP__
 
#include <mutex>
 
namespace roboticslab
{
 
class TrapezoidalTrajectory
{
public:
    struct Reference
    {
        double position; // units
        double velocity; // units/seconds
        double acceleration; // units/seconds^2
    };
 
    TrapezoidalTrajectory();
 
    void setTargetPosition(double startTimestamp, double initialPosition, double initialVelocity,
                           double targetPosition, double refSpeed, double refAcceleration);
 
    void setTargetVelocity(double startTimestamp, double initialPosition, double initialVelocity,
                           double targetVelocity, double refAcceleration);
 
    void reset(double currentPosition);
 
    Reference update(double timestamp);
 
    double getTargetPosition() const;
 
    double queryPosition() const;
 
    double queryVelocity() const;
 
    double queryAcceleration() const;
 
    double queryTime() const;
 
    bool isActive() const;
 
private:
    void configure(double startTimestamp, double initialPosition, double initialVelocity,
                   double targetPosition, double refSpeed, double refAcceleration);
 
    double ta, a1, a2, a3;
    double tb, b2, b3;
    double tc, c1, c2, c3;
 
    double startTimestamp;
    double targetPosition;
 
    double positionReference;
    double velocityReference;
    double accelerationReference;
    double elapsedTime;
 
    bool active;
    mutable std::mutex mutex;
};
 
} // namespace roboticslab
 
#endif // __TRAPEZOIDAL_TRAJECTORY_HPP__