trajectory.hpp

Go to the documentation of this file.

#pragma once

#include "hebi.h"
#include "Eigen/Eigen"
#include "util.hpp"
#include <vector>
#include <memory>

using namespace Eigen;

namespace hebi {
namespace trajectory {

class Trajectory final
{
  private:
    std::vector<HebiTrajectoryPtr> trajectories_;

    const size_t number_of_joints_;

    const size_t number_of_waypoints_;

    const double start_time_;

    const double end_time_;
 
    Trajectory(std::vector<HebiTrajectoryPtr> trajectories, size_t number_of_waypoints, double start_time, double end_time);

  public:

    static std::shared_ptr<Trajectory> createUnconstrainedQp(
      const VectorXd& time_vector,
      const MatrixXd& positions,
      const MatrixXd* velocities = nullptr,
      const MatrixXd* accelerations = nullptr);

    ~Trajectory() noexcept;

    size_t getJointCount() const { return number_of_joints_; }

    size_t getWaypointCount() const { return number_of_waypoints_; }

    double getStartTime() const { return start_time_; }

    double getEndTime() const { return end_time_; }

    double getDuration() const;

    bool getState(double time, VectorXd* position, VectorXd* velocity, VectorXd* acceleration) const;

  private:
    HEBI_DISABLE_COPY_MOVE(Trajectory)
};

} // namespace trajectory
} // namespace hebi