diff --git a/CHANGELOG.md b/CHANGELOG.md index b1cdeeab7d..d6bce8b007 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -67,6 +67,7 @@ * Robot * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes: [#325](https://github.com/personalrobotics/aikido/pull/325), [#392](https://github.com/personalrobotics/aikido/pull/392) + * Added Kunz timer to Robot class: [#505](https://github.com/personalrobotics/aikido/pull/505) * RViz diff --git a/include/aikido/robot/ConcreteManipulator.hpp b/include/aikido/robot/ConcreteManipulator.hpp index 19f54c75a9..463f5faa61 100644 --- a/include/aikido/robot/ConcreteManipulator.hpp +++ b/include/aikido/robot/ConcreteManipulator.hpp @@ -42,6 +42,13 @@ class ConcreteManipulator : public Manipulator const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path) override; + // Documentation inherited. + virtual std::unique_ptr retimePathWithKunz( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + double maxDeviation, + double timestep) override; + // Documentation inherited. virtual std::future executeTrajectory( const trajectory::TrajectoryPtr& trajectory) const override; diff --git a/include/aikido/robot/ConcreteRobot.hpp b/include/aikido/robot/ConcreteRobot.hpp index db4e1c6740..1d63e21659 100644 --- a/include/aikido/robot/ConcreteRobot.hpp +++ b/include/aikido/robot/ConcreteRobot.hpp @@ -62,6 +62,13 @@ class ConcreteRobot : public Robot const dart::dynamics::MetaSkeletonPtr& metaSkeleton, const aikido::trajectory::Trajectory* path) override; + // Documentation inherited. + virtual std::unique_ptr retimePathWithKunz( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + double maxDeviation, + double timestep) override; + // Documentation inherited. virtual std::future executeTrajectory( const trajectory::TrajectoryPtr& trajectory) const override; diff --git a/include/aikido/robot/Robot.hpp b/include/aikido/robot/Robot.hpp index 53dbfa59cf..58ee093fd9 100644 --- a/include/aikido/robot/Robot.hpp +++ b/include/aikido/robot/Robot.hpp @@ -45,6 +45,18 @@ class Robot const aikido::trajectory::Trajectory* path) = 0; + /// Returns a timed trajectory computed with KunzRetimer + /// \param[in] metaSkeleton Metaskeleton of the path. + /// \param[in] path Geometric path to execute. + /// \param[in] maxDeviation Maximum deviation allowed from original path. + /// \param[in] timestep Time step between trajectory points. + virtual std::unique_ptr retimePathWithKunz( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + double maxDeviation, + double timestep) + = 0; + /// Executes a trajectory /// \param[in] trajectory Timed trajectory to execute virtual std::future executeTrajectory( diff --git a/src/robot/CMakeLists.txt b/src/robot/CMakeLists.txt index b3d1dc7a0a..6fae8b1637 100644 --- a/src/robot/CMakeLists.txt +++ b/src/robot/CMakeLists.txt @@ -32,6 +32,7 @@ target_link_libraries("${PROJECT_NAME}_robot" "${PROJECT_NAME}_planner" "${PROJECT_NAME}_planner_ompl" "${PROJECT_NAME}_planner_parabolic" + "${PROJECT_NAME}_planner_kunzretimer" "${PROJECT_NAME}_planner_vectorfield" "${PROJECT_NAME}_constraint" "${PROJECT_NAME}_distance" diff --git a/src/robot/ConcreteManipulator.cpp b/src/robot/ConcreteManipulator.cpp index 496f7ca037..3e790e4655 100644 --- a/src/robot/ConcreteManipulator.cpp +++ b/src/robot/ConcreteManipulator.cpp @@ -35,6 +35,17 @@ std::unique_ptr ConcreteManipulator::retimePath( return mRobot->retimePath(metaSkeleton, path); } +//============================================================================== +std::unique_ptr +ConcreteManipulator::retimePathWithKunz( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + double maxDeviation, + double timestep) +{ + return mRobot->retimePathWithKunz(metaSkeleton, path, maxDeviation, timestep); +} + //============================================================================== std::future ConcreteManipulator::executeTrajectory( const trajectory::TrajectoryPtr& trajectory) const diff --git a/src/robot/ConcreteRobot.cpp b/src/robot/ConcreteRobot.cpp index bb9790538e..11f0ef1833 100644 --- a/src/robot/ConcreteRobot.cpp +++ b/src/robot/ConcreteRobot.cpp @@ -1,5 +1,6 @@ #include "aikido/robot/ConcreteRobot.hpp" #include "aikido/constraint/TestableIntersection.hpp" +#include "aikido/planner/kunzretimer/KunzRetimer.hpp" #include "aikido/robot/util.hpp" #include "aikido/statespace/StateSpace.hpp" @@ -11,6 +12,7 @@ using constraint::dart::TSRPtr; using constraint::TestablePtr; using constraint::ConstTestablePtr; using planner::TrajectoryPostProcessor; +using planner::kunzretimer::KunzRetimer; using planner::parabolic::ParabolicSmoother; using planner::parabolic::ParabolicTimer; using statespace::dart::MetaSkeletonStateSpace; @@ -161,6 +163,29 @@ UniqueSplinePtr ConcreteRobot::retimePath( throw std::invalid_argument("Path should be either Spline or Interpolated."); } +//============================================================================== +UniqueSplinePtr ConcreteRobot::retimePathWithKunz( + const dart::dynamics::MetaSkeletonPtr& metaSkeleton, + const aikido::trajectory::Trajectory* path, + double maxDeviation, + double timestep) +{ + Eigen::VectorXd velocityLimits = getVelocityLimits(*metaSkeleton); + Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton); + auto retimer = std::make_shared( + velocityLimits, accelerationLimits, maxDeviation, timestep); + + auto interpolated = dynamic_cast(path); + if (interpolated) + return retimer->postprocess(*interpolated, *(cloneRNG().get())); + + auto spline = dynamic_cast(path); + if (spline) + return retimer->postprocess(*spline, *(cloneRNG().get())); + + throw std::invalid_argument("Path should be either Spline or Interpolated."); +} + //============================================================================== std::future ConcreteRobot::executeTrajectory( const TrajectoryPtr& trajectory) const