src/ai/planner/smoother/SmootherHelpers.cpp
/*
 * Smoothing with Kris Hauser's parabolic smoother
 */
Smoothing with Kris Hauser's parabolic smoother
Smoothing with Kris Hauser ' s parabolic smoother
aikido::trajectory::UniqueSplinePtr hauserSmoothPath(const std::shared_ptr<ada::Ada> &ada,
                                                     const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace,
                                                     const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr,
                                                     const aikido::trajectory::Trajectory *path,
                                                     const aikido::constraint::TestablePtr &constraint) {
  Eigen::VectorXd velocityLimits = ada->getVelocityLimits();
  Eigen::VectorXd accelerationLimits = ada->getAccelerationLimits();
  auto interpolated = dynamic_cast<const aikido::trajectory::Interpolated *>(path);
  if (!interpolated) {
    auto spline = dynamic_cast<const aikido::trajectory::Spline *>(path);
    if (!spline) {
      throw std::invalid_argument("Path should be either spline or Interpolated.");
    }
    interpolated = detail::SplineToInterpolated(*spline);
  }
  return hauserDoShortcut(metaStateSpace,
                          metaSkeletonPtr,
                          *interpolated,
                          *(ada->cloneRNG().get()),
                          constraint,
                          velocityLimits,
                          accelerationLimits);
}
aikido :: trajectory :: UniqueSplinePtr hauserSmoothPath ( const std :: shared_ptr < ada :: Ada > & ada , const aikido :: statespace :: dart :: MetaSkeletonStateSpacePtr & metaStateSpace , const dart :: dynamics :: MetaSkeletonPtr & metaSkeletonPtr , const aikido :: trajectory :: Trajectory * path , const aikido :: constraint :: TestablePtr & constraint ) { Eigen :: VectorXd velocityLimits = ada -> getVelocityLimits ( ) ; Eigen :: VectorXd accelerationLimits = ada -> getAccelerationLimits ( ) ; auto interpolated = dynamic_cast < const aikido :: trajectory :: Interpolated * > ( path ) ; if ( ! interpolated ) { auto spline = dynamic_cast < const aikido :: trajectory :: Spline * > ( path ) ; if ( ! spline ) { throw std :: invalid_argument ( " " ) ; } interpolated = detail :: SplineToInterpolated ( * spline ) ; } return hauserDoShortcut ( metaStateSpace , metaSkeletonPtr , * interpolated , * ( ada -> cloneRNG ( ) . get ( ) ) , constraint , velocityLimits , accelerationLimits ) ; }
Smoothing with Kris Hauser's parabolic smoother
Smoothing with Kris Hauser ' s parabolic smoother