trajectory.hpp

Go to the documentation of this file.

#pragma once

#include "hebi.h"

#include <memory>
#include <vector>

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

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);

  static Eigen::VectorXd estimateSegmentTimes(const Eigen::MatrixXd& positions,
                                              const Eigen::VectorXd& max_velocities,
                                              const Eigen::VectorXd& max_accelerations,
                                              const HebiTimeEstimationParams& params,
                                              double min_segment_time = 0.01);

public:
  static std::shared_ptr<Trajectory> createUnconstrainedQp(const Eigen::VectorXd& time_vector, const Eigen::MatrixXd& positions,
                                                           const Eigen::MatrixXd* velocities = nullptr,
                                                           const Eigen::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, Eigen::VectorXd* position, Eigen::VectorXd* velocity, Eigen::VectorXd* acceleration) const;

  bool getMinMaxPosition(Eigen::VectorXd& min_position, Eigen::VectorXd& max_position);

  bool getMaxVelocity(Eigen::VectorXd& max_velocity);

  bool getMaxAcceleration(Eigen::VectorXd& max_acceleration);

  static Eigen::VectorXd segmentTimesToWaypointTimes(const Eigen::VectorXd& segment_times);

  static Eigen::VectorXd waypointTimesToSegmentTimes(const Eigen::VectorXd& waypoint_times);

  static Eigen::VectorXd estimateSegmentTimesNFabian(const Eigen::MatrixXd& positions,
                                                     const Eigen::VectorXd& max_velocities,
                                                     const Eigen::VectorXd& max_accelerations,
                                                     double fabian_constant = 6.5,
                                                     double min_segment_time = 0.01) {
    HebiTimeEstimationParams params;
    params.method = HebiTimeEstimationMethod::HebiTimeEstimationNFabian;
    params.params.n_fabian_params.magic_fabian_constant = fabian_constant;
    return estimateSegmentTimes(positions, max_velocities, max_accelerations, params, min_segment_time);
  }

  static Eigen::VectorXd estimateSegmentTimesTrapezoidal(const Eigen::MatrixXd& positions,
                                                     const Eigen::VectorXd& max_velocities,
                                                     const Eigen::VectorXd& max_accelerations,
                                                     double min_segment_time = 0.01) {
    HebiTimeEstimationParams params;
    params.method = HebiTimeEstimationMethod::HebiTimeEstimationVelocityRamp;
    return estimateSegmentTimes(positions, max_velocities, max_accelerations, params, min_segment_time);
  }

private:
  HEBI_DISABLE_COPY_MOVE(Trajectory)
};

} // namespace trajectory
} // namespace hebi