trajectory.hpp

Go to the documentation of this file.

#pragma once

#include "hebi.h"

#include <memory>
#include <vector>

#include "Eigen/Eigen"
#include "util.hpp"

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