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_TIMELIMIT = 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_TIMELIMIT,
    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_TIMELIMIT,
    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:
  struct Params
  {
    Params(
        bool _enableShortcut = true,
        bool _enableBlend = true,
        double _shortcutTimelimit = DEFAULT_TIMELIMIT,
        double _blendRadius = DEFAULT_BLEND_RADIUS,
        int _blendIterations = DEFAULT_BLEND_ITERATIONS,
        double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
        double _feasibilityApproxTolerance = DEFAULT_TOLERANCE)
      : mEnableShortcut(_enableShortcut)
      , mEnableBlend(_enableBlend)
      , mShortcutTimelimit(_shortcutTimelimit)
      , mBlendRadius(_blendRadius)
      , mBlendIterations(_blendIterations)
      , mFeasibilityCheckResolution(_feasibilityCheckResolution)
      , mFeasibilityApproxTolerance(_feasibilityApproxTolerance)
    {
      // Do nothing.
    }
 
    bool mEnableShortcut;
    bool mEnableBlend;
    double mShortcutTimelimit;
    double mBlendRadius;
    int mBlendIterations;
    double mFeasibilityCheckResolution;
    double mFeasibilityApproxTolerance;
  };
 
  ParabolicSmoother(
      const Eigen::VectorXd& _velocityLimits,
      const Eigen::VectorXd& _accelerationLimits,
      bool _enableShortcut = true,
      bool _enableBlend = true,
      double _shortcutTimelimit = DEFAULT_TIMELIMIT,
      double _blendRadius = DEFAULT_BLEND_RADIUS,
      int _blendIterations = DEFAULT_BLEND_ITERATIONS,
      double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
      double _feasibilityApproxTolerance = DEFAULT_TOLERANCE);
 
  ParabolicSmoother(
      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& _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_