From c66c1940f289d4a616aa473cfe02e26f92a10790 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Jun 2020 17:58:22 -0700 Subject: [PATCH 01/27] Add post-processor params header. --- include/aikido/robot.hpp | 1 + include/aikido/robot/RobotParams.hpp | 78 ++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+) create mode 100644 include/aikido/robot/RobotParams.hpp diff --git a/include/aikido/robot.hpp b/include/aikido/robot.hpp index 196d6e22fd..a0a002e241 100644 --- a/include/aikido/robot.hpp +++ b/include/aikido/robot.hpp @@ -4,4 +4,5 @@ #include "aikido/robot/Hand.hpp" #include "aikido/robot/Manipulator.hpp" #include "aikido/robot/Robot.hpp" +#include "aikido/robot/RobotParams.hpp" #include "aikido/robot/util.hpp" diff --git a/include/aikido/robot/RobotParams.hpp b/include/aikido/robot/RobotParams.hpp new file mode 100644 index 0000000000..70b93f97c8 --- /dev/null +++ b/include/aikido/robot/RobotParams.hpp @@ -0,0 +1,78 @@ +#ifndef AIKIDO_ROBOT_ROBOTPARAMS_HPP_ +#define AIKIDO_ROBOT_ROBOTPARAMS_HPP_ + +#include + +namespace aikido { +namespace robot { + +/// Enum for which postprocessor is being used. +enum class PostProcessorType +{ + HAUSER, + KUNZ +}; + +/// Hauser postprocessor parameters. +struct HauserParams +{ + bool mEnableShortcut; + bool mEnableBlend; + double mShortcutTimelimit; + double mBlendRadius; + int mBlendIterations; + double mFeasibilityCheckResolution; + double mFeasibilityApproxTolerance; +}; + +/// Kunz postprocessor parameters. +struct KunzParams +{ + double mMaxDeviation; + double mTimeStep; +}; + +/// Trajectory postprocessing parameters. +struct PostProcessorParams +{ + /// Convenience constructor for when using Hauser. + PostProcessorParams( + bool enableShortcut = true, + bool enableBlend = true, + double shortcutTimelimit = 0.6, + double blendRadius = 0.4, + int blendIterations = 1, + double feasibilityCheckResolution = 1e-3, + double feasibilityApproxTolerance = 1e-3) + : mPostProcessorType(PostProcessorType::HAUSER) + , mHauserParams{enableShortcut, + enableBlend, + shortcutTimelimit, + blendRadius, + blendIterations, + feasibilityCheckResolution, + feasibilityApproxTolerance} + { + // Do nothing. + } + + /// Convenience constructor for when using Kunz. + PostProcessorParams(double maxDeviation = 0.1, double timeStep = 0.01) + : mPostProcessorType(PostProcessorType::KUNZ) + , mKunzParams{maxDeviation, timeStep} + { + // Do nothing. + } + + /// Which postprocessor is being used. + PostProcessorType mPostProcessorType; + /// Params used if `mPostProcessorType` indicates we are using Hauser. + HauserParams mHauserParams; + /// Params used if `mPostProcessorType` indicates we are using Kunz. + KunzParams mKunzParams; +}; + +} // namespace robot +} // namespace aikido + +#endif // AIKIDO_ROBOT_ROBOTPARAMS_HPP_ From 59698ee792b181f1c59062e851ab988b2bf70b2b Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Jun 2020 18:14:47 -0700 Subject: [PATCH 02/27] Add getTrajectoryPostProcessor() method to ConcreteRobot. --- include/aikido/robot/ConcreteRobot.hpp | 11 ++++++++ src/robot/ConcreteRobot.cpp | 35 ++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 2fdaefe5ae..f5f9922258 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -17,6 +17,7 @@ #include "aikido/planner/parabolic/ParabolicSmoother.hpp" #include "aikido/planner/parabolic/ParabolicTimer.hpp" #include "aikido/robot/Robot.hpp" +#include "aikido/robot/RobotParams.hpp" #include "aikido/robot/util.hpp" #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/trajectory/Trajectory.hpp" @@ -126,6 +127,16 @@ class ConcreteRobot : public Robot double feasibilityCheckResolution, double feasibilityApproxTolerance) const; + // Get a postprocessor that respects velocity and acceleration limits, as well + // as the passed constraint. However, unlike the above, the specific + // postprocessor returned is controlled by `postProcessorParams`. + /// \param[in] metaSkeleton The MetaSkeleton whose limits must be respected. + /// \param[in] postProcessorParams Postprocessor parameters. + std::shared_ptr + getTrajectoryPostProcessor( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const PostProcessorParams& postProcessorParams) const; + /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its /// initial configuration after planning. diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 29eedc17cf..a2658d461e 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -331,6 +331,41 @@ ConcreteRobot::getTrajectoryPostProcessor( feasibilityApproxTolerance); } +//============================================================================== +std::shared_ptr +ConcreteRobot::getTrajectoryPostProcessor( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const PostProcessorParams& postProcessorParams) const +{ + // TODO (sniyaz):Should we delete `smoothPath`, `retimePath`, and + // `retimePathWithKunz` in a subsequent PR? + switch (postProcessorParams.mPostProcessorType) + { + case PostProcessorType::HAUSER: + return std::make_shared( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams.mHauserParams.mEnableShortcut, + postProcessorParams.mHauserParams.mEnableBlend, + postProcessorParams.mHauserParams.mShortcutTimelimit, + postProcessorParams.mHauserParams.mBlendRadius, + postProcessorParams.mHauserParams.mBlendIterations, + postProcessorParams.mHauserParams.mFeasibilityCheckResolution, + postProcessorParams.mHauserParams.mFeasibilityApproxTolerance); + case PostProcessorType::KUNZ: + return std::make_shared( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams.mKunzParams.mMaxDeviation, + postProcessorParams.mKunzParams.mTimeStep); + default: + // TODO (sniyaz): Can this be made a compile-time check instead? + std::stringstream message; + message << "PostProcessor type not recognized in `postProcessorParams`!"; + throw std::runtime_error(message.str()); + } +} + //============================================================================== TrajectoryPtr ConcreteRobot::planToConfiguration( const MetaSkeletonStateSpacePtr& stateSpace, From 95243e7f774a1522455ad61610a43fd68e5661a6 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Jun 2020 18:52:49 -0700 Subject: [PATCH 03/27] Add postProcessPath() to ConcreteRobot. --- include/aikido/robot/ConcreteRobot.hpp | 13 +++++++++++++ src/robot/ConcreteRobot.cpp | 23 +++++++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index f5f9922258..8bea68770b 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -137,6 +137,19 @@ class ConcreteRobot : public Robot const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const PostProcessorParams& postProcessorParams) const; + /// Returns a post-processed trajectory that can be executed by the robot, but + /// allows the user to control the exact params used. + /// \param[in] metaSkeleton Metaskeleton of the path. + /// \param[in] path Geometric path to execute. + /// \param[in] constraint Must be satisfied after postprocessing. Typically + /// collision constraint is passed. + /// \param[in] postProcessorParams Postprocessor parameters. + aikido::trajectory::UniqueSplinePtr postProcessPath( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const PostProcessorParams& postProcessorParams); + /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its /// initial configuration after planning. diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index a2658d461e..066fdc2e13 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -339,6 +339,7 @@ ConcreteRobot::getTrajectoryPostProcessor( { // TODO (sniyaz):Should we delete `smoothPath`, `retimePath`, and // `retimePathWithKunz` in a subsequent PR? + // TODO: (sniyaz): Include custom limits in params. switch (postProcessorParams.mPostProcessorType) { case PostProcessorType::HAUSER: @@ -366,6 +367,28 @@ ConcreteRobot::getTrajectoryPostProcessor( } } +//============================================================================== +UniqueSplinePtr ConcreteRobot::postProcessPath( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const PostProcessorParams& postProcessorParams) +{ + auto postProcessor + = getTrajectoryPostProcessor(metaSkeleton, postProcessorParams); + + auto interpolated = dynamic_cast(path); + if (interpolated) + return postProcessor->postprocess( + *interpolated, *(cloneRNG().get()), constraint); + + auto spline = dynamic_cast(path); + if (spline) + return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); + + throw std::invalid_argument("Path should be either Spline or Interpolated."); +} + //============================================================================== TrajectoryPtr ConcreteRobot::planToConfiguration( const MetaSkeletonStateSpacePtr& stateSpace, From c6c3e050c35bfb7e6c1f41da8ca97d4b0326d8c8 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 9 Jul 2020 19:30:25 -0700 Subject: [PATCH 04/27] Re-org post-processor params like Brian suggested. --- include/aikido/planner.hpp | 1 + .../PostProcessorParams.hpp} | 36 ++++++------------- .../planner/kunzretimer/KunzRetimer.hpp | 8 +++++ .../planner/parabolic/ParabolicSmoother.hpp | 13 +++++++ include/aikido/robot.hpp | 1 - include/aikido/robot/ConcreteRobot.hpp | 6 ++-- src/robot/ConcreteRobot.cpp | 8 ++--- 7 files changed, 39 insertions(+), 34 deletions(-) rename include/aikido/{robot/RobotParams.hpp => planner/PostProcessorParams.hpp} (70%) diff --git a/include/aikido/planner.hpp b/include/aikido/planner.hpp index 5ff14ce2f2..465ffe0cff 100644 --- a/include/aikido/planner.hpp +++ b/include/aikido/planner.hpp @@ -1,4 +1,5 @@ #include "aikido/planner/PlanningResult.hpp" +#include "aikido/planner/PostProcessorParams.hpp" #include "aikido/planner/SnapPlanner.hpp" #include "aikido/planner/TrajectoryPostProcessor.hpp" #include "aikido/planner/World.hpp" diff --git a/include/aikido/robot/RobotParams.hpp b/include/aikido/planner/PostProcessorParams.hpp similarity index 70% rename from include/aikido/robot/RobotParams.hpp rename to include/aikido/planner/PostProcessorParams.hpp index 70b93f97c8..9313a415b6 100644 --- a/include/aikido/robot/RobotParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -1,10 +1,13 @@ -#ifndef AIKIDO_ROBOT_ROBOTPARAMS_HPP_ -#define AIKIDO_ROBOT_ROBOTPARAMS_HPP_ +#ifndef AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ +#define AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ #include +#include "aikido/planner/kunzretimer/KunzRetimer.hpp" +#include "aikido/planner/parabolic/ParabolicSmoother.hpp" + namespace aikido { -namespace robot { +namespace planner { /// Enum for which postprocessor is being used. enum class PostProcessorType @@ -13,25 +16,6 @@ enum class PostProcessorType KUNZ }; -/// Hauser postprocessor parameters. -struct HauserParams -{ - bool mEnableShortcut; - bool mEnableBlend; - double mShortcutTimelimit; - double mBlendRadius; - int mBlendIterations; - double mFeasibilityCheckResolution; - double mFeasibilityApproxTolerance; -}; - -/// Kunz postprocessor parameters. -struct KunzParams -{ - double mMaxDeviation; - double mTimeStep; -}; - /// Trajectory postprocessing parameters. struct PostProcessorParams { @@ -67,12 +51,12 @@ struct PostProcessorParams /// Which postprocessor is being used. PostProcessorType mPostProcessorType; /// Params used if `mPostProcessorType` indicates we are using Hauser. - HauserParams mHauserParams; + parabolic::HauserParams mHauserParams; /// Params used if `mPostProcessorType` indicates we are using Kunz. - KunzParams mKunzParams; + kunzretimer::KunzParams mKunzParams; }; -} // namespace robot +} // namespace planner } // namespace aikido -#endif // AIKIDO_ROBOT_ROBOTPARAMS_HPP_ +#endif // AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 379f5a80fc..249a4a9223 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -11,6 +11,14 @@ namespace aikido { namespace planner { namespace kunzretimer { +/// Kunz postprocessor parameters used with the main `PostProcessorParams` +/// struct. +struct KunzParams +{ + double mMaxDeviation; + double mTimeStep; +}; + /// Computes the time-optimal timing of a trajectory consisting of a sequence /// Geodesic interpolations between states under velocity and acceleration /// bounds. The output is a parabolic spline, encoded in cubic polynomials. diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index 02c69aca8a..4cbd9de29f 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -17,6 +17,19 @@ constexpr int DEFAULT_BLEND_ITERATIONS = 4; constexpr double DEFAULT_CHECK_RESOLUTION = 1e-4; constexpr double DEFAULT_TOLERANCE = 1e-3; +/// Hauser postprocessor parameters used with the main `PostProcessorParams` +/// struct. +struct HauserParams +{ + bool mEnableShortcut; + bool mEnableBlend; + double mShortcutTimelimit; + double mBlendRadius; + int mBlendIterations; + double mFeasibilityCheckResolution; + double mFeasibilityApproxTolerance; +}; + /// Shortcut waypoints in a trajectory using parabolic splines. /// /// This function smooths `_inputTrajectory' by iteratively sampling diff --git a/include/aikido/robot.hpp b/include/aikido/robot.hpp index a0a002e241..196d6e22fd 100644 --- a/include/aikido/robot.hpp +++ b/include/aikido/robot.hpp @@ -4,5 +4,4 @@ #include "aikido/robot/Hand.hpp" #include "aikido/robot/Manipulator.hpp" #include "aikido/robot/Robot.hpp" -#include "aikido/robot/RobotParams.hpp" #include "aikido/robot/util.hpp" diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 8bea68770b..81af2f01d1 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -14,10 +14,10 @@ #include "aikido/constraint/dart/TSR.hpp" #include "aikido/control/TrajectoryExecutor.hpp" #include "aikido/distance/ConfigurationRanker.hpp" +#include "aikido/planner/PostProcessorParams.hpp" #include "aikido/planner/parabolic/ParabolicSmoother.hpp" #include "aikido/planner/parabolic/ParabolicTimer.hpp" #include "aikido/robot/Robot.hpp" -#include "aikido/robot/RobotParams.hpp" #include "aikido/robot/util.hpp" #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/trajectory/Trajectory.hpp" @@ -135,7 +135,7 @@ class ConcreteRobot : public Robot std::shared_ptr getTrajectoryPostProcessor( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, - const PostProcessorParams& postProcessorParams) const; + const aikido::planner::PostProcessorParams& postProcessorParams) const; /// Returns a post-processed trajectory that can be executed by the robot, but /// allows the user to control the exact params used. @@ -148,7 +148,7 @@ class ConcreteRobot : public Robot const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const PostProcessorParams& postProcessorParams); + const aikido::planner::PostProcessorParams& postProcessorParams); /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 066fdc2e13..e1570faa5d 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -335,14 +335,14 @@ ConcreteRobot::getTrajectoryPostProcessor( std::shared_ptr ConcreteRobot::getTrajectoryPostProcessor( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, - const PostProcessorParams& postProcessorParams) const + const aikido::planner::PostProcessorParams& postProcessorParams) const { // TODO (sniyaz):Should we delete `smoothPath`, `retimePath`, and // `retimePathWithKunz` in a subsequent PR? // TODO: (sniyaz): Include custom limits in params. switch (postProcessorParams.mPostProcessorType) { - case PostProcessorType::HAUSER: + case aikido::planner::PostProcessorType::HAUSER: return std::make_shared( getVelocityLimits(*metaSkeleton), getAccelerationLimits(*metaSkeleton), @@ -353,7 +353,7 @@ ConcreteRobot::getTrajectoryPostProcessor( postProcessorParams.mHauserParams.mBlendIterations, postProcessorParams.mHauserParams.mFeasibilityCheckResolution, postProcessorParams.mHauserParams.mFeasibilityApproxTolerance); - case PostProcessorType::KUNZ: + case aikido::planner::PostProcessorType::KUNZ: return std::make_shared( getVelocityLimits(*metaSkeleton), getAccelerationLimits(*metaSkeleton), @@ -372,7 +372,7 @@ UniqueSplinePtr ConcreteRobot::postProcessPath( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const PostProcessorParams& postProcessorParams) + const aikido::planner::PostProcessorParams& postProcessorParams) { auto postProcessor = getTrajectoryPostProcessor(metaSkeleton, postProcessorParams); From 9c9353b14659dd4ad6237ed426ec1152ad7cf3ff Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 9 Jul 2020 19:33:45 -0700 Subject: [PATCH 05/27] Leave better comment on PostProcessorParams struct. --- include/aikido/planner/PostProcessorParams.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp index 9313a415b6..260f6a0069 100644 --- a/include/aikido/planner/PostProcessorParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -16,7 +16,9 @@ enum class PostProcessorType KUNZ }; -/// Trajectory postprocessing parameters. +/// Trajectory postprocessing parameters. This struct can be used with the +/// `ConcreteRobot` class, and let's the user specify the exact processor and +// and parameters to use. struct PostProcessorParams { /// Convenience constructor for when using Hauser. From ccc88b9aa85b1f0a6b907923cc69b1ec828c8107 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 18:44:19 -0700 Subject: [PATCH 06/27] Rename Hauser/Kunz param structs to just `Params`, set default args. --- include/aikido/planner/PostProcessorParams.hpp | 4 ++-- .../aikido/planner/kunzretimer/KunzRetimer.hpp | 6 +++--- .../planner/parabolic/ParabolicSmoother.hpp | 16 ++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp index 260f6a0069..1a66ef2be6 100644 --- a/include/aikido/planner/PostProcessorParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -53,9 +53,9 @@ struct PostProcessorParams /// Which postprocessor is being used. PostProcessorType mPostProcessorType; /// Params used if `mPostProcessorType` indicates we are using Hauser. - parabolic::HauserParams mHauserParams; + parabolic::Params mHauserParams; /// Params used if `mPostProcessorType` indicates we are using Kunz. - kunzretimer::KunzParams mKunzParams; + kunzretimer::Params mKunzParams; }; } // namespace planner diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 249a4a9223..b25c04303f 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -13,10 +13,10 @@ namespace kunzretimer { /// Kunz postprocessor parameters used with the main `PostProcessorParams` /// struct. -struct KunzParams +struct Params { - double mMaxDeviation; - double mTimeStep; + double mMaxDeviation = 1e-2; + double mTimeStep = 0.1; }; /// Computes the time-optimal timing of a trajectory consisting of a sequence diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index 4cbd9de29f..cba07078fb 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -19,15 +19,15 @@ constexpr double DEFAULT_TOLERANCE = 1e-3; /// Hauser postprocessor parameters used with the main `PostProcessorParams` /// struct. -struct HauserParams +struct Params { - bool mEnableShortcut; - bool mEnableBlend; - double mShortcutTimelimit; - double mBlendRadius; - int mBlendIterations; - double mFeasibilityCheckResolution; - double mFeasibilityApproxTolerance; + bool mEnableShortcut = true; + bool mEnableBlend = true; + double mShortcutTimelimit = DEFAULT_TIMELIMT; + double mBlendRadius = DEFAULT_BLEND_RADIUS; + int mBlendIterations = DEFAULT_BLEND_ITERATIONS; + double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION; + double mFeasibilityApproxTolerance = DEFAULT_TOLERANCE; }; /// Shortcut waypoints in a trajectory using parabolic splines. From 3cb622ea6531c1e7bed25af72ece4480b3b3554a Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 18:59:16 -0700 Subject: [PATCH 07/27] [WIP] Add new Kunz/Hauser constructors. --- .../aikido/planner/kunzretimer/KunzRetimer.hpp | 8 ++++++++ .../planner/parabolic/ParabolicSmoother.hpp | 8 ++++++++ src/planner/kunzretimer/KunzRetimer.cpp | 13 +++++++++++++ src/planner/parabolic/ParabolicSmoother.cpp | 18 ++++++++++++++++++ 4 files changed, 47 insertions(+) diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index b25c04303f..7b7261ab2e 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -62,6 +62,14 @@ class KunzRetimer : public aikido::planner::TrajectoryPostProcessor double maxDeviation, double timeStep); + /// \param[in] velocityLimits Maximum velocity for each dimension. + /// \param[in] accelerationLimits Maximum acceleration for each dimension. + /// \param[in] params Postprocessor parameters. + KunzRetimer( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const Params& params); + /// Performs parabolic retiming on an input trajectory. /// \copydoc TrajectoryPostProcessor::postprocess std::unique_ptr postprocess( diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index cba07078fb..aaeaa04dff 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -175,6 +175,14 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION, double _feasibilityApproxTolerance = DEFAULT_TOLERANCE); + /// \param _velocityLimits Maximum velocity for each dimension. + /// \param _accelerationLimits Maximum acceleration for each dimension. + /// \param _params Postprocessor parameters. + ParabolicSmoother( + const Eigen::VectorXd& _velocityLimits, + const Eigen::VectorXd& _accelerationLimits, + const Params& _params); + /// Performs parabolic smoothing on an input trajectory. /// \param _inputTraj The untimed trajectory for the arm to process. /// \param _rng Random number generator. diff --git a/src/planner/kunzretimer/KunzRetimer.cpp b/src/planner/kunzretimer/KunzRetimer.cpp index f9a1d38154..e8d364c02c 100644 --- a/src/planner/kunzretimer/KunzRetimer.cpp +++ b/src/planner/kunzretimer/KunzRetimer.cpp @@ -160,6 +160,19 @@ KunzRetimer::KunzRetimer( // Do nothing } +//============================================================================== +KunzRetimer::KunzRetimer( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const Params& params) + : mVelocityLimits{velocityLimits} + , mAccelerationLimits{accelerationLimits} + , mMaxDeviation(params.mMaxDeviation) + , mTimeStep(params.mTimeStep) +{ + // Do nothing +} + //============================================================================== std::unique_ptr KunzRetimer::postprocess( const aikido::trajectory::Interpolated& inputTraj, diff --git a/src/planner/parabolic/ParabolicSmoother.cpp b/src/planner/parabolic/ParabolicSmoother.cpp index 0f1551cf6a..3c0075703a 100644 --- a/src/planner/parabolic/ParabolicSmoother.cpp +++ b/src/planner/parabolic/ParabolicSmoother.cpp @@ -139,6 +139,24 @@ ParabolicSmoother::ParabolicSmoother( // Do nothing } +//============================================================================== +ParabolicSmoother::ParabolicSmoother( + const Eigen::VectorXd& _velocityLimits, + const Eigen::VectorXd& _accelerationLimits, + const Params& _params) + : mFeasibilityCheckResolution{_params.mFeasibilityCheckResolution} + , mFeasibilityApproxTolerance{_params.mFeasibilityApproxTolerance} + , mVelocityLimits{_velocityLimits} + , mAccelerationLimits{_accelerationLimits} + , mEnableShortcut{_params.mEnableShortcut} + , mEnableBlend{_params.mEnableBlend} + , mShortcutTimelimit{_params.mShortcutTimelimit} + , mBlendRadius{_params.mBlendRadius} + , mBlendIterations{_params.mBlendIterations} +{ + // Do nothing +} + //============================================================================== std::unique_ptr ParabolicSmoother::postprocess( const aikido::trajectory::Interpolated& _inputTraj, From f9dbb29f74f5e4e2402be5457bb95d6bf7233904 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 19:06:03 -0700 Subject: [PATCH 08/27] Change getTrajectoryPostProcessor() to take limits. --- include/aikido/robot/ConcreteRobot.hpp | 6 ++++-- src/robot/ConcreteRobot.cpp | 16 ++++++++++------ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 81af2f01d1..16ef4eb7e1 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -130,11 +130,13 @@ class ConcreteRobot : public Robot // Get a postprocessor that respects velocity and acceleration limits, as well // as the passed constraint. However, unlike the above, the specific // postprocessor returned is controlled by `postProcessorParams`. - /// \param[in] metaSkeleton The MetaSkeleton whose limits must be respected. + /// \param[in] velocityLimits Maximum velocity for each dimension. + /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] postProcessorParams Postprocessor parameters. std::shared_ptr getTrajectoryPostProcessor( - const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, const aikido::planner::PostProcessorParams& postProcessorParams) const; /// Returns a post-processed trajectory that can be executed by the robot, but diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index e1570faa5d..a1c142e974 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -334,7 +334,8 @@ ConcreteRobot::getTrajectoryPostProcessor( //============================================================================== std::shared_ptr ConcreteRobot::getTrajectoryPostProcessor( - const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, const aikido::planner::PostProcessorParams& postProcessorParams) const { // TODO (sniyaz):Should we delete `smoothPath`, `retimePath`, and @@ -344,8 +345,8 @@ ConcreteRobot::getTrajectoryPostProcessor( { case aikido::planner::PostProcessorType::HAUSER: return std::make_shared( - getVelocityLimits(*metaSkeleton), - getAccelerationLimits(*metaSkeleton), + velocityLimits, + accelerationLimits, postProcessorParams.mHauserParams.mEnableShortcut, postProcessorParams.mHauserParams.mEnableBlend, postProcessorParams.mHauserParams.mShortcutTimelimit, @@ -355,8 +356,8 @@ ConcreteRobot::getTrajectoryPostProcessor( postProcessorParams.mHauserParams.mFeasibilityApproxTolerance); case aikido::planner::PostProcessorType::KUNZ: return std::make_shared( - getVelocityLimits(*metaSkeleton), - getAccelerationLimits(*metaSkeleton), + velocityLimits, + accelerationLimits, postProcessorParams.mKunzParams.mMaxDeviation, postProcessorParams.mKunzParams.mTimeStep); default: @@ -375,7 +376,10 @@ UniqueSplinePtr ConcreteRobot::postProcessPath( const aikido::planner::PostProcessorParams& postProcessorParams) { auto postProcessor - = getTrajectoryPostProcessor(metaSkeleton, postProcessorParams); + = getTrajectoryPostProcessor( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams); auto interpolated = dynamic_cast(path); if (interpolated) From bc808aaef69c4eea89fb993446229950e762cb79 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 19:07:20 -0700 Subject: [PATCH 09/27] Make format. --- src/robot/ConcreteRobot.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index a1c142e974..420ebd7b32 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -375,11 +375,10 @@ UniqueSplinePtr ConcreteRobot::postProcessPath( const constraint::TestablePtr& constraint, const aikido::planner::PostProcessorParams& postProcessorParams) { - auto postProcessor - = getTrajectoryPostProcessor( - getVelocityLimits(*metaSkeleton), - getAccelerationLimits(*metaSkeleton), - postProcessorParams); + auto postProcessor = getTrajectoryPostProcessor( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams); auto interpolated = dynamic_cast(path); if (interpolated) From 84207066be0e65c1856bf6112e4f962bb69f738d Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 19:19:02 -0700 Subject: [PATCH 10/27] Add new postProcessPath() to ConcreteRobot that takes limits. --- include/aikido/robot/ConcreteRobot.hpp | 15 +++++++++++++++ src/robot/ConcreteRobot.cpp | 23 +++++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 16ef4eb7e1..9c6f45e25c 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -152,6 +152,21 @@ class ConcreteRobot : public Robot const constraint::TestablePtr& constraint, const aikido::planner::PostProcessorParams& postProcessorParams); + /// Returns a post-processed trajectory that can be executed by the robot, but + /// allows the user to control the exact params a limits used. + /// \param[in] velocityLimits Maximum velocity for each dimension. + /// \param[in] accelerationLimits Maximum acceleration for each dimension. + /// \param[in] path Geometric path to execute. + /// \param[in] constraint Must be satisfied after postprocessing. Typically + /// collision constraint is passed. + /// \param[in] postProcessorParams Postprocessor parameters. + aikido::trajectory::UniqueSplinePtr postProcessPath( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const aikido::planner::PostProcessorParams& postProcessorParams); + /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its /// initial configuration after planning. diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index 420ebd7b32..afe221d8bc 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -392,6 +392,29 @@ UniqueSplinePtr ConcreteRobot::postProcessPath( throw std::invalid_argument("Path should be either Spline or Interpolated."); } +//============================================================================== +UniqueSplinePtr ConcreteRobot::postProcessPath( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const aikido::planner::PostProcessorParams& postProcessorParams) +{ + auto postProcessor = getTrajectoryPostProcessor( + velocityLimits, accelerationLimits, postProcessorParams); + + auto interpolated = dynamic_cast(path); + if (interpolated) + return postProcessor->postprocess( + *interpolated, *(cloneRNG().get()), constraint); + + auto spline = dynamic_cast(path); + if (spline) + return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); + + throw std::invalid_argument("Path should be either Spline or Interpolated."); +} + //============================================================================== TrajectoryPtr ConcreteRobot::planToConfiguration( const MetaSkeletonStateSpacePtr& stateSpace, From 6ca023020c6e3be04219ce5cc7889de9441c9ce4 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 21 Jul 2020 23:58:21 -0700 Subject: [PATCH 11/27] [WIP] Think I re-wrote PostProcessorParams.hpp to work but holy cow a lot broke. --- .../aikido/planner/PostProcessorParams.hpp | 48 +++++-------------- 1 file changed, 11 insertions(+), 37 deletions(-) diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp index 1a66ef2be6..398ff08e92 100644 --- a/include/aikido/planner/PostProcessorParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -9,53 +9,27 @@ namespace aikido { namespace planner { -/// Enum for which postprocessor is being used. -enum class PostProcessorType -{ - HAUSER, - KUNZ -}; - /// Trajectory postprocessing parameters. This struct can be used with the /// `ConcreteRobot` class, and let's the user specify the exact processor and // and parameters to use. +template struct PostProcessorParams { - /// Convenience constructor for when using Hauser. - PostProcessorParams( - bool enableShortcut = true, - bool enableBlend = true, - double shortcutTimelimit = 0.6, - double blendRadius = 0.4, - int blendIterations = 1, - double feasibilityCheckResolution = 1e-3, - double feasibilityApproxTolerance = 1e-3) - : mPostProcessorType(PostProcessorType::HAUSER) - , mHauserParams{enableShortcut, - enableBlend, - shortcutTimelimit, - blendRadius, - blendIterations, - feasibilityCheckResolution, - feasibilityApproxTolerance} - { - // Do nothing. - } + static_assert( + std::is_base_of::value, + "T must derive from aikido::planner::TrajectoryPostProcessor"); - /// Convenience constructor for when using Kunz. - PostProcessorParams(double maxDeviation = 0.1, double timeStep = 0.01) - : mPostProcessorType(PostProcessorType::KUNZ) - , mKunzParams{maxDeviation, timeStep} +public: + /// Constructor. + PostProcessorParams(typename T::Params params = T::Params()) : mParams(params) { // Do nothing. } - /// Which postprocessor is being used. - PostProcessorType mPostProcessorType; - /// Params used if `mPostProcessorType` indicates we are using Hauser. - parabolic::Params mHauserParams; - /// Params used if `mPostProcessorType` indicates we are using Kunz. - kunzretimer::Params mKunzParams; + typename T::Params getParams(){return mParams;}; + +private: + typename T::Params mParams; }; } // namespace planner From 20d880edd1261bd998dda6ece56d0cdccadfc017 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Wed, 22 Jul 2020 11:55:17 -0700 Subject: [PATCH 12/27] Move ConcreteRobot post-process stuff to -impl files. --- .../aikido/planner/PostProcessorParams.hpp | 5 +- include/aikido/robot/ConcreteRobot.hpp | 11 ++- .../robot/detail/ConcreteRobot-impl.hpp | 68 +++++++++++++++ src/robot/ConcreteRobot.cpp | 84 ------------------- 4 files changed, 80 insertions(+), 88 deletions(-) create mode 100644 include/aikido/robot/detail/ConcreteRobot-impl.hpp diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp index 398ff08e92..f897ee1f6d 100644 --- a/include/aikido/planner/PostProcessorParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -26,7 +26,10 @@ struct PostProcessorParams // Do nothing. } - typename T::Params getParams(){return mParams;}; + typename T::Params getParams() + { + return mParams; + }; private: typename T::Params mParams; diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 9c6f45e25c..8e15ae2f36 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -133,11 +133,12 @@ class ConcreteRobot : public Robot /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] postProcessorParams Postprocessor parameters. + template std::shared_ptr getTrajectoryPostProcessor( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - const aikido::planner::PostProcessorParams& postProcessorParams) const; + const aikido::planner::PostProcessorParams& postProcessorParams) const; /// Returns a post-processed trajectory that can be executed by the robot, but /// allows the user to control the exact params used. @@ -146,11 +147,12 @@ class ConcreteRobot : public Robot /// \param[in] constraint Must be satisfied after postprocessing. Typically /// collision constraint is passed. /// \param[in] postProcessorParams Postprocessor parameters. + template aikido::trajectory::UniqueSplinePtr postProcessPath( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams); + const aikido::planner::PostProcessorParams& postProcessorParams); /// Returns a post-processed trajectory that can be executed by the robot, but /// allows the user to control the exact params a limits used. @@ -160,12 +162,13 @@ class ConcreteRobot : public Robot /// \param[in] constraint Must be satisfied after postprocessing. Typically /// collision constraint is passed. /// \param[in] postProcessorParams Postprocessor parameters. + template aikido::trajectory::UniqueSplinePtr postProcessPath( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams); + const aikido::planner::PostProcessorParams& postProcessorParams); /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its @@ -351,4 +354,6 @@ class ConcreteRobot : public Robot } // namespace robot } // namespace aikido +#include "detail/ConcreteRobot-impl.hpp" + #endif // AIKIDO_ROBOT_CONCRETEROBOT_HPP_ diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp new file mode 100644 index 0000000000..b546fad80e --- /dev/null +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -0,0 +1,68 @@ +namespace aikido { +namespace robot { + +//============================================================================== +template +std::shared_ptr +ConcreteRobot::getTrajectoryPostProcessor( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const aikido::planner::PostProcessorParams& postProcessorParams) const +{ + return std::make_shared( + velocityLimits, accelerationLimits, postProcessorParams); +} + +//============================================================================== +template +aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const aikido::planner::PostProcessorParams& postProcessorParams) +{ + auto postProcessor = getTrajectoryPostProcessor( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams); + + auto interpolated + = dynamic_cast(path); + if (interpolated) + return postProcessor->postprocess( + *interpolated, *(cloneRNG().get()), constraint); + + auto spline = dynamic_cast(path); + if (spline) + return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); + + throw std::invalid_argument("Path should be either Spline or Interpolated."); +} + +//============================================================================== +template +aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( + const Eigen::VectorXd& velocityLimits, + const Eigen::VectorXd& accelerationLimits, + const aikido::trajectory::Trajectory* path, + const constraint::TestablePtr& constraint, + const aikido::planner::PostProcessorParams& postProcessorParams) +{ + auto postProcessor = getTrajectoryPostProcessor( + velocityLimits, accelerationLimits, postProcessorParams); + + auto interpolated + = dynamic_cast(path); + if (interpolated) + return postProcessor->postprocess( + *interpolated, *(cloneRNG().get()), constraint); + + auto spline = dynamic_cast(path); + if (spline) + return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); + + throw std::invalid_argument("Path should be either Spline or Interpolated."); +} + +} // namespace robot +} // namespace aikido diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index afe221d8bc..29eedc17cf 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -331,90 +331,6 @@ ConcreteRobot::getTrajectoryPostProcessor( feasibilityApproxTolerance); } -//============================================================================== -std::shared_ptr -ConcreteRobot::getTrajectoryPostProcessor( - const Eigen::VectorXd& velocityLimits, - const Eigen::VectorXd& accelerationLimits, - const aikido::planner::PostProcessorParams& postProcessorParams) const -{ - // TODO (sniyaz):Should we delete `smoothPath`, `retimePath`, and - // `retimePathWithKunz` in a subsequent PR? - // TODO: (sniyaz): Include custom limits in params. - switch (postProcessorParams.mPostProcessorType) - { - case aikido::planner::PostProcessorType::HAUSER: - return std::make_shared( - velocityLimits, - accelerationLimits, - postProcessorParams.mHauserParams.mEnableShortcut, - postProcessorParams.mHauserParams.mEnableBlend, - postProcessorParams.mHauserParams.mShortcutTimelimit, - postProcessorParams.mHauserParams.mBlendRadius, - postProcessorParams.mHauserParams.mBlendIterations, - postProcessorParams.mHauserParams.mFeasibilityCheckResolution, - postProcessorParams.mHauserParams.mFeasibilityApproxTolerance); - case aikido::planner::PostProcessorType::KUNZ: - return std::make_shared( - velocityLimits, - accelerationLimits, - postProcessorParams.mKunzParams.mMaxDeviation, - postProcessorParams.mKunzParams.mTimeStep); - default: - // TODO (sniyaz): Can this be made a compile-time check instead? - std::stringstream message; - message << "PostProcessor type not recognized in `postProcessorParams`!"; - throw std::runtime_error(message.str()); - } -} - -//============================================================================== -UniqueSplinePtr ConcreteRobot::postProcessPath( - const dart::dynamics::MetaSkeletonPtr& metaSkeleton, - const aikido::trajectory::Trajectory* path, - const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams) -{ - auto postProcessor = getTrajectoryPostProcessor( - getVelocityLimits(*metaSkeleton), - getAccelerationLimits(*metaSkeleton), - postProcessorParams); - - auto interpolated = dynamic_cast(path); - if (interpolated) - return postProcessor->postprocess( - *interpolated, *(cloneRNG().get()), constraint); - - auto spline = dynamic_cast(path); - if (spline) - return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); - - throw std::invalid_argument("Path should be either Spline or Interpolated."); -} - -//============================================================================== -UniqueSplinePtr ConcreteRobot::postProcessPath( - const Eigen::VectorXd& velocityLimits, - const Eigen::VectorXd& accelerationLimits, - const aikido::trajectory::Trajectory* path, - const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams) -{ - auto postProcessor = getTrajectoryPostProcessor( - velocityLimits, accelerationLimits, postProcessorParams); - - auto interpolated = dynamic_cast(path); - if (interpolated) - return postProcessor->postprocess( - *interpolated, *(cloneRNG().get()), constraint); - - auto spline = dynamic_cast(path); - if (spline) - return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); - - throw std::invalid_argument("Path should be either Spline or Interpolated."); -} - //============================================================================== TrajectoryPtr ConcreteRobot::planToConfiguration( const MetaSkeletonStateSpacePtr& stateSpace, From dacf6f9c1ea755a1a8aea29b49f83ba3c3b80b5f Mon Sep 17 00:00:00 2001 From: sniyaz Date: Wed, 22 Jul 2020 12:04:35 -0700 Subject: [PATCH 13/27] Actually move Params structs into their PP classes. --- .../planner/kunzretimer/KunzRetimer.hpp | 16 ++++++------ .../planner/parabolic/ParabolicSmoother.hpp | 26 +++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 7b7261ab2e..0ab8475d97 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -11,14 +11,6 @@ namespace aikido { namespace planner { namespace kunzretimer { -/// Kunz postprocessor parameters used with the main `PostProcessorParams` -/// struct. -struct Params -{ - double mMaxDeviation = 1e-2; - double mTimeStep = 0.1; -}; - /// Computes the time-optimal timing of a trajectory consisting of a sequence /// Geodesic interpolations between states under velocity and acceleration /// bounds. The output is a parabolic spline, encoded in cubic polynomials. @@ -52,6 +44,14 @@ std::unique_ptr computeKunzTiming( class KunzRetimer : public aikido::planner::TrajectoryPostProcessor { public: + /// Kunz postprocessor parameters used with the main `PostProcessorParams` + /// struct. + struct Params + { + double mMaxDeviation = 1e-2; + double mTimeStep = 0.1; + }; + /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] maxDeviation Maximum deviation in circular blending diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index aaeaa04dff..e0c6390800 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -17,19 +17,6 @@ constexpr int DEFAULT_BLEND_ITERATIONS = 4; constexpr double DEFAULT_CHECK_RESOLUTION = 1e-4; constexpr double DEFAULT_TOLERANCE = 1e-3; -/// Hauser postprocessor parameters used with the main `PostProcessorParams` -/// struct. -struct Params -{ - bool mEnableShortcut = true; - bool mEnableBlend = true; - double mShortcutTimelimit = DEFAULT_TIMELIMT; - double mBlendRadius = DEFAULT_BLEND_RADIUS; - int mBlendIterations = DEFAULT_BLEND_ITERATIONS; - double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION; - double mFeasibilityApproxTolerance = DEFAULT_TOLERANCE; -}; - /// Shortcut waypoints in a trajectory using parabolic splines. /// /// This function smooths `_inputTrajectory' by iteratively sampling @@ -147,6 +134,19 @@ std::unique_ptr doShortcutAndBlend( class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor { public: + /// Hauser postprocessor parameters used with the main `PostProcessorParams` + /// struct. + struct Params + { + bool mEnableShortcut = true; + bool mEnableBlend = true; + double mShortcutTimelimit = DEFAULT_TIMELIMT; + double mBlendRadius = DEFAULT_BLEND_RADIUS; + int mBlendIterations = DEFAULT_BLEND_ITERATIONS; + double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION; + double mFeasibilityApproxTolerance = DEFAULT_TOLERANCE; + }; + /// \param _velocityLimits Maximum velocity for each dimension. /// \param _accelerationLimits Maximum acceleration for each dimension. /// \param _enableShortcut Whether shortcutting is used in smoothing. From 304d95dab902e47f830d79993fc0c20a91c282bd Mon Sep 17 00:00:00 2001 From: sniyaz Date: Wed, 22 Jul 2020 12:09:17 -0700 Subject: [PATCH 14/27] In ConcreteRobot -> postProcessPath(), print and return null instead of throwing. --- include/aikido/robot/detail/ConcreteRobot-impl.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index b546fad80e..5c01b5c672 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -36,7 +36,10 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( if (spline) return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); - throw std::invalid_argument("Path should be either Spline or Interpolated."); + std::cerr + << "[postProcessPath]: Path should be either Spline or Interpolated." + << std::endl; + return nullptr; } //============================================================================== @@ -61,7 +64,10 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( if (spline) return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); - throw std::invalid_argument("Path should be either Spline or Interpolated."); + std::cerr + << "[postProcessPath]: Path should be either Spline or Interpolated." + << std::endl; + return nullptr; } } // namespace robot From c3696b451ead493de52932d7fbf5c0e2c95ef5a1 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Wed, 22 Jul 2020 12:23:37 -0700 Subject: [PATCH 15/27] Whoops, tiny bug fix. --- include/aikido/robot/detail/ConcreteRobot-impl.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index 5c01b5c672..1ce3f98bf8 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -10,7 +10,7 @@ ConcreteRobot::getTrajectoryPostProcessor( const aikido::planner::PostProcessorParams& postProcessorParams) const { return std::make_shared( - velocityLimits, accelerationLimits, postProcessorParams); + velocityLimits, accelerationLimits, postProcessorParams.getParams()); } //============================================================================== From a13922a4fda55badbc57f8bc101ce999cfbfbdfc Mon Sep 17 00:00:00 2001 From: sniyaz Date: Wed, 22 Jul 2020 18:15:59 -0700 Subject: [PATCH 16/27] Mark PostProcessorParams -> getParams() as const. --- include/aikido/planner/PostProcessorParams.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp index f897ee1f6d..d23414dd9d 100644 --- a/include/aikido/planner/PostProcessorParams.hpp +++ b/include/aikido/planner/PostProcessorParams.hpp @@ -26,7 +26,7 @@ struct PostProcessorParams // Do nothing. } - typename T::Params getParams() + typename T::Params getParams() const { return mParams; }; From b307153d7879e059634f4989bbc8735ffa982b6d Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 24 Jul 2020 15:49:20 -0700 Subject: [PATCH 17/27] Respond to nits. --- .../planner/kunzretimer/KunzRetimer.hpp | 22 ++++++++++++------- .../planner/parabolic/ParabolicSmoother.hpp | 10 ++++----- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 0ab8475d97..4c9ab207d0 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -11,6 +11,9 @@ namespace aikido { namespace planner { namespace kunzretimer { +constexpr double DEFAULT_MAX_DEVIATION = 1e-2; +constexpr double DEFAULT_TIME_STEP = 0.1; + /// Computes the time-optimal timing of a trajectory consisting of a sequence /// Geodesic interpolations between states under velocity and acceleration /// bounds. The output is a parabolic spline, encoded in cubic polynomials. @@ -36,8 +39,8 @@ std::unique_ptr computeKunzTiming( const aikido::trajectory::Interpolated& inputTrajectory, const Eigen::VectorXd& maxVelocity, const Eigen::VectorXd& maxAcceleration, - double maxDeviation = 1e-2, - double timeStep = 0.1); + double maxDeviation = DEFAULT_MAX_DEVIATION, + double timeStep = DEFAULT_TIME_STEP); /// Class for performing time-optimal trajectory retiming following subject to /// velocity and acceleration limits. @@ -48,19 +51,22 @@ class KunzRetimer : public aikido::planner::TrajectoryPostProcessor /// struct. struct Params { - double mMaxDeviation = 1e-2; - double mTimeStep = 0.1; + /// Maximum deviation in circular blending (in configuration space). + double mMaxDeviation = DEFAULT_MAX_DEVIATION; + /// Time step in following the path (in seconds). + double mTimeStep = DEFAULT_TIME_STEP; }; /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. - /// \param[in] maxDeviation Maximum deviation in circular blending - /// \param[in] timeStep Time step in following the path + /// \param[in] maxDeviation Maximum deviation in circular blending (in + /// configuration space). + /// \param[in] timeStep Time step in following the path (in seconds). KunzRetimer( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - double maxDeviation, - double timeStep); + double maxDeviation = DEFAULT_MAX_DEVIATION, + double timeStep = DEFAULT_TIME_STEP); /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index e0c6390800..97195dcf86 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -11,7 +11,7 @@ namespace aikido { namespace planner { namespace parabolic { -constexpr double DEFAULT_TIMELIMT = 3.0; +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; @@ -50,7 +50,7 @@ std::unique_ptr doShortcut( const Eigen::VectorXd& _maxVelocity, const Eigen::VectorXd& _maxAcceleration, aikido::common::RNG& _rng, - double _timelimit = DEFAULT_TIMELIMT, + double _timelimit = DEFAULT_TIMELIMIT, double _checkResolution = DEFAULT_CHECK_RESOLUTION, double _tolerance = DEFAULT_TOLERANCE); @@ -124,7 +124,7 @@ std::unique_ptr doShortcutAndBlend( const Eigen::VectorXd& _maxVelocity, const Eigen::VectorXd& _maxAcceleration, aikido::common::RNG& _rng, - double _timelimit = DEFAULT_TIMELIMT, + double _timelimit = DEFAULT_TIMELIMIT, double _blendRadius = DEFAULT_BLEND_RADIUS, int _blendIterations = DEFAULT_BLEND_ITERATIONS, double _checkResolution = DEFAULT_CHECK_RESOLUTION, @@ -140,7 +140,7 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor { bool mEnableShortcut = true; bool mEnableBlend = true; - double mShortcutTimelimit = DEFAULT_TIMELIMT; + double mShortcutTimelimit = DEFAULT_TIMELIMIT; double mBlendRadius = DEFAULT_BLEND_RADIUS; int mBlendIterations = DEFAULT_BLEND_ITERATIONS; double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION; @@ -169,7 +169,7 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor const Eigen::VectorXd& _accelerationLimits, bool _enableShortcut = true, bool _enableBlend = true, - double _shortcutTimelimit = DEFAULT_TIMELIMT, + double _shortcutTimelimit = DEFAULT_TIMELIMIT, double _blendRadius = DEFAULT_BLEND_RADIUS, int _blendIterations = DEFAULT_BLEND_ITERATIONS, double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION, From ad267062da7aad3a72c37877d5141ecca60d456e Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 24 Jul 2020 15:55:15 -0700 Subject: [PATCH 18/27] Update CHANGELOG. --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 83f4e78ebc..aa5e194097 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,10 @@ ### 0.4.0 (2020-XX-XX) +* Planner + + * Moved post-processor params into AIKIDO: [#579](https://github.com/personalrobotics/aikido/pull/579) + ### 0.3.0 (2020-05-22) * Common From de4a12961300322043580bd36f43b9c48c642308 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 01:52:57 -0700 Subject: [PATCH 19/27] Update CHANGELOG. --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index aa5e194097..64c393bc0b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,7 @@ * Planner - * Moved post-processor params into AIKIDO: [#579](https://github.com/personalrobotics/aikido/pull/579) + * Defined post-processor parameter structs in AIKIDO: [#579](https://github.com/personalrobotics/aikido/pull/579) ### 0.3.0 (2020-05-22) From 238fe01cfb0fcb1a6d26dbfff726efc3428299f0 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 02:01:20 -0700 Subject: [PATCH 20/27] Respond to some nits. --- include/aikido/robot/ConcreteRobot.hpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 8e15ae2f36..4be289eb0c 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -127,9 +127,8 @@ class ConcreteRobot : public Robot double feasibilityCheckResolution, double feasibilityApproxTolerance) const; - // Get a postprocessor that respects velocity and acceleration limits, as well - // as the passed constraint. However, unlike the above, the specific - // postprocessor returned is controlled by `postProcessorParams`. + /// Get a postprocessor that respects velocity and acceleration limits. The + /// specific postprocessor returned is controlled by `postProcessorParams`. /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] postProcessorParams Postprocessor parameters. @@ -140,8 +139,7 @@ class ConcreteRobot : public Robot const Eigen::VectorXd& accelerationLimits, const aikido::planner::PostProcessorParams& postProcessorParams) const; - /// Returns a post-processed trajectory that can be executed by the robot, but - /// allows the user to control the exact params used. + /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] metaSkeleton Metaskeleton of the path. /// \param[in] path Geometric path to execute. /// \param[in] constraint Must be satisfied after postprocessing. Typically @@ -154,8 +152,7 @@ class ConcreteRobot : public Robot const constraint::TestablePtr& constraint, const aikido::planner::PostProcessorParams& postProcessorParams); - /// Returns a post-processed trajectory that can be executed by the robot, but - /// allows the user to control the exact params a limits used. + /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] path Geometric path to execute. From 8fdef1d824cf8b24ad2e0075a8af1ecfc9284b18 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 02:12:24 -0700 Subject: [PATCH 21/27] Don't dupe postProcessPath() logic. --- .../robot/detail/ConcreteRobot-impl.hpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index 1ce3f98bf8..05236244b3 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -21,25 +21,12 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const constraint::TestablePtr& constraint, const aikido::planner::PostProcessorParams& postProcessorParams) { - auto postProcessor = getTrajectoryPostProcessor( + return postProcessPath( getVelocityLimits(*metaSkeleton), getAccelerationLimits(*metaSkeleton), + path, + constraint, postProcessorParams); - - auto interpolated - = dynamic_cast(path); - if (interpolated) - return postProcessor->postprocess( - *interpolated, *(cloneRNG().get()), constraint); - - auto spline = dynamic_cast(path); - if (spline) - return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint); - - std::cerr - << "[postProcessPath]: Path should be either Spline or Interpolated." - << std::endl; - return nullptr; } //============================================================================== From 2bad0b11a9cf88ffe4e94591bc8cc39423ed27b1 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 02:55:56 -0700 Subject: [PATCH 22/27] Kill PostProcessorParams.hpp. --- .../aikido/planner/PostProcessorParams.hpp | 41 ------------------- include/aikido/robot/ConcreteRobot.hpp | 7 ++-- .../robot/detail/ConcreteRobot-impl.hpp | 12 ++++-- 3 files changed, 11 insertions(+), 49 deletions(-) delete mode 100644 include/aikido/planner/PostProcessorParams.hpp diff --git a/include/aikido/planner/PostProcessorParams.hpp b/include/aikido/planner/PostProcessorParams.hpp deleted file mode 100644 index d23414dd9d..0000000000 --- a/include/aikido/planner/PostProcessorParams.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ -#define AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ - -#include - -#include "aikido/planner/kunzretimer/KunzRetimer.hpp" -#include "aikido/planner/parabolic/ParabolicSmoother.hpp" - -namespace aikido { -namespace planner { - -/// Trajectory postprocessing parameters. This struct can be used with the -/// `ConcreteRobot` class, and let's the user specify the exact processor and -// and parameters to use. -template -struct PostProcessorParams -{ - static_assert( - std::is_base_of::value, - "T must derive from aikido::planner::TrajectoryPostProcessor"); - -public: - /// Constructor. - PostProcessorParams(typename T::Params params = T::Params()) : mParams(params) - { - // Do nothing. - } - - typename T::Params getParams() const - { - return mParams; - }; - -private: - typename T::Params mParams; -}; - -} // namespace planner -} // namespace aikido - -#endif // AIKIDO_PLANNER_POSTPROCESSORPARAMS_HPP_ diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 4be289eb0c..a8db42cd78 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -14,7 +14,6 @@ #include "aikido/constraint/dart/TSR.hpp" #include "aikido/control/TrajectoryExecutor.hpp" #include "aikido/distance/ConfigurationRanker.hpp" -#include "aikido/planner/PostProcessorParams.hpp" #include "aikido/planner/parabolic/ParabolicSmoother.hpp" #include "aikido/planner/parabolic/ParabolicTimer.hpp" #include "aikido/robot/Robot.hpp" @@ -137,7 +136,7 @@ class ConcreteRobot : public Robot getTrajectoryPostProcessor( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - const aikido::planner::PostProcessorParams& postProcessorParams) const; + const typename T::Params& postProcessorParams) const; /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] metaSkeleton Metaskeleton of the path. @@ -150,7 +149,7 @@ class ConcreteRobot : public Robot const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams); + const typename T::Params& postProcessorParams); /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] velocityLimits Maximum velocity for each dimension. @@ -165,7 +164,7 @@ class ConcreteRobot : public Robot const Eigen::VectorXd& accelerationLimits, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams); + const typename T::Params& postProcessorParams); /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index 05236244b3..ac2fb229de 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -7,10 +7,14 @@ std::shared_ptr ConcreteRobot::getTrajectoryPostProcessor( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - const aikido::planner::PostProcessorParams& postProcessorParams) const + const typename T::Params& postProcessorParams) const { + static_assert( + std::is_base_of::value, + "T must derive from aikido::planner::TrajectoryPostProcessor"); + return std::make_shared( - velocityLimits, accelerationLimits, postProcessorParams.getParams()); + velocityLimits, accelerationLimits, postProcessorParams); } //============================================================================== @@ -19,7 +23,7 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams) + const typename T::Params& postProcessorParams) { return postProcessPath( getVelocityLimits(*metaSkeleton), @@ -36,7 +40,7 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const Eigen::VectorXd& accelerationLimits, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const aikido::planner::PostProcessorParams& postProcessorParams) + const typename T::Params& postProcessorParams) { auto postProcessor = getTrajectoryPostProcessor( velocityLimits, accelerationLimits, postProcessorParams); From d3c824dcb58de113bfed20a0cae85f4cd62742be Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 03:44:52 -0700 Subject: [PATCH 23/27] Last nits, I think. --- include/aikido/robot/ConcreteRobot.hpp | 15 ++++++++------ .../robot/detail/ConcreteRobot-impl.hpp | 20 ++++++++++--------- 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index a8db42cd78..83cda67346 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -131,12 +131,13 @@ class ConcreteRobot : public Robot /// \param[in] velocityLimits Maximum velocity for each dimension. /// \param[in] accelerationLimits Maximum acceleration for each dimension. /// \param[in] postProcessorParams Postprocessor parameters. - template + /// \tparam PostProcessor The trajectory postprocessor to use. + template std::shared_ptr getTrajectoryPostProcessor( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - const typename T::Params& postProcessorParams) const; + const typename PostProcessor::Params& postProcessorParams) const; /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] metaSkeleton Metaskeleton of the path. @@ -144,12 +145,13 @@ class ConcreteRobot : public Robot /// \param[in] constraint Must be satisfied after postprocessing. Typically /// collision constraint is passed. /// \param[in] postProcessorParams Postprocessor parameters. - template + /// \tparam PostProcessor The trajectory postprocessor to use. + template aikido::trajectory::UniqueSplinePtr postProcessPath( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const typename T::Params& postProcessorParams); + const typename PostProcessor::Params& postProcessorParams); /// Returns a post-processed trajectory that can be executed by the robot. /// \param[in] velocityLimits Maximum velocity for each dimension. @@ -158,13 +160,14 @@ class ConcreteRobot : public Robot /// \param[in] constraint Must be satisfied after postprocessing. Typically /// collision constraint is passed. /// \param[in] postProcessorParams Postprocessor parameters. - template + /// \tparam PostProcessor The trajectory postprocessor to use. + template aikido::trajectory::UniqueSplinePtr postProcessPath( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const typename T::Params& postProcessorParams); + const typename PostProcessor::Params& postProcessorParams); /// TODO: Replace this with Problem interface. /// Plan the robot to a specific configuration. Restores the robot to its diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index ac2fb229de..ace9e7033f 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -2,28 +2,30 @@ namespace aikido { namespace robot { //============================================================================== -template +template std::shared_ptr ConcreteRobot::getTrajectoryPostProcessor( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, - const typename T::Params& postProcessorParams) const + const typename PostProcessor::Params& postProcessorParams) const { static_assert( - std::is_base_of::value, - "T must derive from aikido::planner::TrajectoryPostProcessor"); + std::is_base_of:: + value, + "PostProcessor must derive from " + "aikido::planner::TrajectoryPostProcessor"); - return std::make_shared( + return std::make_shared( velocityLimits, accelerationLimits, postProcessorParams); } //============================================================================== -template +template aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const typename T::Params& postProcessorParams) + const typename PostProcessor::Params& postProcessorParams) { return postProcessPath( getVelocityLimits(*metaSkeleton), @@ -34,13 +36,13 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( } //============================================================================== -template +template aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits, const aikido::trajectory::Trajectory* path, const constraint::TestablePtr& constraint, - const typename T::Params& postProcessorParams) + const typename PostProcessor::Params& postProcessorParams) { auto postProcessor = getTrajectoryPostProcessor( velocityLimits, accelerationLimits, postProcessorParams); From 67c7fc7fc722d974d24dc493417b6052db72a3b8 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 12:35:38 -0700 Subject: [PATCH 24/27] Final final edits? --- include/aikido/planner.hpp | 1 - include/aikido/robot/ConcreteRobot.hpp | 11 +++++++++++ include/aikido/robot/detail/ConcreteRobot-impl.hpp | 13 +++++++++++++ 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/include/aikido/planner.hpp b/include/aikido/planner.hpp index 465ffe0cff..5ff14ce2f2 100644 --- a/include/aikido/planner.hpp +++ b/include/aikido/planner.hpp @@ -1,5 +1,4 @@ #include "aikido/planner/PlanningResult.hpp" -#include "aikido/planner/PostProcessorParams.hpp" #include "aikido/planner/SnapPlanner.hpp" #include "aikido/planner/TrajectoryPostProcessor.hpp" #include "aikido/planner/World.hpp" diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index 83cda67346..fddac85dfd 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -126,6 +126,17 @@ class ConcreteRobot : public Robot double feasibilityCheckResolution, double feasibilityApproxTolerance) const; + /// Get a postprocessor that respects velocity and acceleration limits. The + /// specific postprocessor returned is controlled by `postProcessorParams`. + /// \param[in] metaSkeleton Metaskeleton of the path. + /// \param[in] postProcessorParams Postprocessor parameters. + /// \tparam PostProcessor The trajectory postprocessor to use. + template + std::shared_ptr + getTrajectoryPostProcessor( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const typename PostProcessor::Params& postProcessorParams) const; + /// Get a postprocessor that respects velocity and acceleration limits. The /// specific postprocessor returned is controlled by `postProcessorParams`. /// \param[in] velocityLimits Maximum velocity for each dimension. diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index ace9e7033f..7acf5a5c57 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -1,6 +1,19 @@ namespace aikido { namespace robot { +//============================================================================== +template +std::shared_ptr +ConcreteRobot::getTrajectoryPostProcessor( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const typename PostProcessor::Params& postProcessorParams) const +{ + return getTrajectoryPostProcessor( + getVelocityLimits(*metaSkeleton), + getAccelerationLimits(*metaSkeleton), + postProcessorParams); +} + //============================================================================== template std::shared_ptr From 923cea2a4795eee632888a7a4e6558accab6a065 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 31 Jul 2020 12:59:08 -0700 Subject: [PATCH 25/27] More nits. --- include/aikido/planner/kunzretimer/KunzRetimer.hpp | 3 +-- include/aikido/planner/parabolic/ParabolicSmoother.hpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 4c9ab207d0..17af1f3425 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -47,8 +47,7 @@ std::unique_ptr computeKunzTiming( class KunzRetimer : public aikido::planner::TrajectoryPostProcessor { public: - /// Kunz postprocessor parameters used with the main `PostProcessorParams` - /// struct. + /// Kunz postprocessor parameters. struct Params { /// Maximum deviation in circular blending (in configuration space). diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index 97195dcf86..da97eed865 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -134,8 +134,7 @@ std::unique_ptr doShortcutAndBlend( class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor { public: - /// Hauser postprocessor parameters used with the main `PostProcessorParams` - /// struct. + /// Hauser postprocessor parameters. struct Params { bool mEnableShortcut = true; From d1aa4860bf84aed85f9c2e4353a332ffbc37097a Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 1 Aug 2020 15:10:08 -0700 Subject: [PATCH 26/27] Add constructors to param structs. --- .../planner/kunzretimer/KunzRetimer.hpp | 17 +++++-- .../planner/parabolic/ParabolicSmoother.hpp | 48 ++++++++++++++++--- 2 files changed, 54 insertions(+), 11 deletions(-) diff --git a/include/aikido/planner/kunzretimer/KunzRetimer.hpp b/include/aikido/planner/kunzretimer/KunzRetimer.hpp index 17af1f3425..7420b194a9 100644 --- a/include/aikido/planner/kunzretimer/KunzRetimer.hpp +++ b/include/aikido/planner/kunzretimer/KunzRetimer.hpp @@ -50,10 +50,19 @@ class KunzRetimer : public aikido::planner::TrajectoryPostProcessor /// Kunz postprocessor parameters. struct Params { - /// Maximum deviation in circular blending (in configuration space). - double mMaxDeviation = DEFAULT_MAX_DEVIATION; - /// Time step in following the path (in seconds). - double mTimeStep = DEFAULT_TIME_STEP; + /// \param[in] _maxDeviation Maximum deviation in circular blending (in + /// configuration space). + /// \param[in] _timeStep Time step in following the path (in seconds). + Params( + double _maxDeviation = DEFAULT_MAX_DEVIATION, + double _timeStep = DEFAULT_TIME_STEP) + : mMaxDeviation(_maxDeviation), mTimeStep(_timeStep) + { + // Do nothing. + } + + double mMaxDeviation; + double mTimeStep; }; /// \param[in] velocityLimits Maximum velocity for each dimension. diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp index da97eed865..92b10b89d9 100644 --- a/include/aikido/planner/parabolic/ParabolicSmoother.hpp +++ b/include/aikido/planner/parabolic/ParabolicSmoother.hpp @@ -137,13 +137,47 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor /// Hauser postprocessor parameters. struct Params { - bool mEnableShortcut = true; - bool mEnableBlend = true; - double mShortcutTimelimit = DEFAULT_TIMELIMIT; - double mBlendRadius = DEFAULT_BLEND_RADIUS; - int mBlendIterations = DEFAULT_BLEND_ITERATIONS; - double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION; - double mFeasibilityApproxTolerance = DEFAULT_TOLERANCE; + /// \param _enableShortcut Whether shortcutting is used in smoothing. + /// \param _enableBlend Whether blending is used in smoothing. + /// \param _shortcutTimelimit Timelimit for shortcutting. It is ineffective + /// when _enableShortcut is false. + /// \param _blendRadius Blend radius for blending. It is ineffective + /// when _enableBlend is false. + /// \param _blendIterations Blend iterations for blending. It is + /// ineffective when _enableBlend is false. + /// \param _feasibilityCheckResolution The resolution in discretizing + /// a segment in checking the feasibility of the segment. + /// \param _feasibilityApproxTolerance This tolerance is used in a + /// piecewise linear discretization that deviates no more than + /// \c _feasibilityApproxTolerance from the parabolic ramp along any + /// axis, and then checks for configuration and segment feasibility along + /// that piecewise linear path. + 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; }; /// \param _velocityLimits Maximum velocity for each dimension. From 9c13c0d9b46e7b9b99a7f3c3b0c61425ef9fd522 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Tue, 4 Aug 2020 18:06:47 -0700 Subject: [PATCH 27/27] Fix bug with missing template params. --- include/aikido/robot/detail/ConcreteRobot-impl.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/aikido/robot/detail/ConcreteRobot-impl.hpp b/include/aikido/robot/detail/ConcreteRobot-impl.hpp index 7acf5a5c57..6c3b061859 100644 --- a/include/aikido/robot/detail/ConcreteRobot-impl.hpp +++ b/include/aikido/robot/detail/ConcreteRobot-impl.hpp @@ -8,7 +8,7 @@ ConcreteRobot::getTrajectoryPostProcessor( const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const typename PostProcessor::Params& postProcessorParams) const { - return getTrajectoryPostProcessor( + return getTrajectoryPostProcessor( getVelocityLimits(*metaSkeleton), getAccelerationLimits(*metaSkeleton), postProcessorParams); @@ -40,7 +40,7 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const constraint::TestablePtr& constraint, const typename PostProcessor::Params& postProcessorParams) { - return postProcessPath( + return postProcessPath( getVelocityLimits(*metaSkeleton), getAccelerationLimits(*metaSkeleton), path, @@ -57,7 +57,7 @@ aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath( const constraint::TestablePtr& constraint, const typename PostProcessor::Params& postProcessorParams) { - auto postProcessor = getTrajectoryPostProcessor( + auto postProcessor = getTrajectoryPostProcessor( velocityLimits, accelerationLimits, postProcessorParams); auto interpolated