KunzRetimer.hpp

Go to the documentation of this file.

#ifndef AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_
#define AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_
 
#include <Eigen/Dense>
 
#include "aikido/planner/TrajectoryPostProcessor.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "aikido/trajectory/Spline.hpp"
 
namespace aikido {
namespace planner {
namespace kunzretimer {
 
constexpr double DEFAULT_MAX_DEVIATION = 1e-2;
constexpr double DEFAULT_TIME_STEP = 0.1;
 
std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
    const aikido::trajectory::Interpolated& inputTrajectory,
    const Eigen::VectorXd& maxVelocity,
    const Eigen::VectorXd& maxAcceleration,
    double maxDeviation = DEFAULT_MAX_DEVIATION,
    double timeStep = DEFAULT_TIME_STEP);
 
class KunzRetimer : public aikido::planner::TrajectoryPostProcessor
{
public:
  struct Params
  {
    Params(
        double _maxDeviation = DEFAULT_MAX_DEVIATION,
        double _timeStep = DEFAULT_TIME_STEP)
      : mMaxDeviation(_maxDeviation), mTimeStep(_timeStep)
    {
      // Do nothing.
    }
 
    double mMaxDeviation;
    double mTimeStep;
  };
 
  KunzRetimer(
      const Eigen::VectorXd& velocityLimits,
      const Eigen::VectorXd& accelerationLimits,
      double maxDeviation = DEFAULT_MAX_DEVIATION,
      double timeStep = DEFAULT_TIME_STEP);
 
  KunzRetimer(
      const Eigen::VectorXd& velocityLimits,
      const Eigen::VectorXd& accelerationLimits,
      const Params& params);
 
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Interpolated& inputTraj,
      const aikido::common::RNG& rng,
      const aikido::constraint::TestablePtr& constraint = nullptr) override;
 
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Spline& inputTraj,
      const aikido::common::RNG& rng,
      const aikido::constraint::TestablePtr& constraint = nullptr) override;
 
  const Eigen::VectorXd& getVelocityLimits() const;
 
  const Eigen::VectorXd& getAccelerationLimits() const;
 
  void setVelocityLimits(const Eigen::VectorXd& velocityLimits);
 
  void setAccelerationLimits(const Eigen::VectorXd& accelerationLimits);
 
  double getTimeStep() const;
 
  void setTimeStep(double timeStep);
 
  double getMaxDeviation() const;
 
  void setMaxDeviation(double maxDeviation);
 
private:
  Eigen::VectorXd mVelocityLimits;
 
  Eigen::VectorXd mAccelerationLimits;
 
  double mMaxDeviation;
 
  double mTimeStep;
};
 
} // namespace kunzretimer
} // namespace planner
} // namespace aikido
 
#endif // ifndef AIKIDO_PLANNER_KUNZRETIMER_KUNZRETIMER_HPP_