ParabolicSmoother.hpp

Go to the documentation of this file.

#ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_
#define AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_
 
#include <Eigen/Dense>
 
#include "aikido/planner/TrajectoryPostProcessor.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "aikido/trajectory/Spline.hpp"
 
namespace aikido {
namespace planner {
namespace parabolic {
 
constexpr double DEFAULT_TIMELIMT = 3.0;
constexpr double DEFAULT_BLEND_RADIUS = 0.5;
constexpr int DEFAULT_BLEND_ITERATIONS = 4;
constexpr double DEFAULT_CHECK_RESOLUTION = 1e-4;
constexpr double DEFAULT_TOLERANCE = 1e-3;
 
std::unique_ptr<trajectory::Spline> doShortcut(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    aikido::common::RNG& _rng,
    double _timelimit = DEFAULT_TIMELIMT,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);
 
std::unique_ptr<trajectory::Spline> doBlend(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    double _blendRadius = DEFAULT_BLEND_RADIUS,
    int _blendIterations = DEFAULT_BLEND_ITERATIONS,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);
 
std::unique_ptr<trajectory::Spline> doShortcutAndBlend(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    aikido::common::RNG& _rng,
    double _timelimit = DEFAULT_TIMELIMT,
    double _blendRadius = DEFAULT_BLEND_RADIUS,
    int _blendIterations = DEFAULT_BLEND_ITERATIONS,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);
 
class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
{
public:
  ParabolicSmoother(
      const Eigen::VectorXd& _velocityLimits,
      const Eigen::VectorXd& _accelerationLimits,
      bool _enableShortcut = true,
      bool _enableBlend = true,
      double _shortcutTimelimit = DEFAULT_TIMELIMT,
      double _blendRadius = DEFAULT_BLEND_RADIUS,
      int _blendIterations = DEFAULT_BLEND_ITERATIONS,
      double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
      double _feasibilityApproxTolerance = DEFAULT_TOLERANCE);
 
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Interpolated& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable) override;
 
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Spline& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable) override;
 
private:
  std::unique_ptr<aikido::trajectory::Spline> handleShortcutOrBlend(
      const aikido::trajectory::Spline& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable);
 
  double mFeasibilityCheckResolution;
 
  double mFeasibilityApproxTolerance;
 
  const Eigen::VectorXd mVelocityLimits;
 
  const Eigen::VectorXd mAccelerationLimits;
 
  bool mEnableShortcut;
 
  bool mEnableBlend;
 
  double mShortcutTimelimit;
 
  double mBlendRadius;
 
  int mBlendIterations;
};
 
} // namespace parabolic
} // namespace planner
} // namespace aikido
 
#endif // ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_