From b42800bb943485c6887d3d3eed986d8ebbaf6538 Mon Sep 17 00:00:00 2001 From: Hejia Zhang Date: Sat, 21 Sep 2019 23:15:40 -0700 Subject: [PATCH] fix simpleSmoother; kris hauser's smoother --- CMakeLists.txt | 36 +- include/wecook/Action.h | 21 +- include/wecook/ActionNode.h | 6 + include/wecook/ContainingMap.h | 10 + include/wecook/MetaActuateInfo.h | 30 + include/wecook/ObjectMgr.h | 3 +- include/wecook/PrimitiveActionNode.h | 6 + include/wecook/PrimitiveActuateNode.h | 8 +- include/wecook/PrimitivePlaceNode.h | 9 +- include/wecook/TSRMotionNode.h | 5 + include/wecook/TaskGraph.h | 11 +- include/wecook/planner/SmootherHelpers.h | 77 + package.xml | 1 - scripts/fruit_jelly.py | 34 +- src/ActionPlanner.cpp | 82 +- src/ObjectMgr.cpp | 20 +- src/PrimitiveActuateNode.cpp | 121 +- src/PrimitiveEngageNode.cpp | 18 +- src/PrimitiveGraspNode.cpp | 21 +- src/PrimitivePlaceNode.cpp | 21 +- src/RelativeIKMotionNode.cpp | 4 +- src/TSRMotionNode.cpp | 47 +- src/TaskExecutorThread.cpp | 10 +- src/TaskGraph.cpp | 18 +- .../hauser_parabolic_smoother/CMakeLists.txt | 15 + .../hauser_parabolic_smoother/Config.h | 49 + .../hauser_parabolic_smoother/DynamicPath.cpp | 868 +++++ .../hauser_parabolic_smoother/DynamicPath.h | 151 + .../hauser_parabolic_smoother/HauserMath.h | 33 + .../ParabolicRamp.cpp | 2852 +++++++++++++++++ .../hauser_parabolic_smoother/ParabolicRamp.h | 172 + .../hauser_parabolic_smoother/Timer.cpp | 103 + .../hauser_parabolic_smoother/Timer.h | 43 + src/planner/SimpleDynamicPath.cpp | 40 + src/planner/SimpleDynamicPath.h | 38 + src/planner/SmootherHelpers.cpp | 174 + src/planner/SmootherUtil.cpp | 484 +++ src/planner/SmootherUtil.h | 93 + wecook.rosinstall | 35 +- 39 files changed, 5598 insertions(+), 171 deletions(-) create mode 100644 include/wecook/MetaActuateInfo.h create mode 100644 include/wecook/planner/SmootherHelpers.h create mode 100644 src/external/hauser_parabolic_smoother/CMakeLists.txt create mode 100644 src/external/hauser_parabolic_smoother/Config.h create mode 100644 src/external/hauser_parabolic_smoother/DynamicPath.cpp create mode 100644 src/external/hauser_parabolic_smoother/DynamicPath.h create mode 100644 src/external/hauser_parabolic_smoother/HauserMath.h create mode 100644 src/external/hauser_parabolic_smoother/ParabolicRamp.cpp create mode 100644 src/external/hauser_parabolic_smoother/ParabolicRamp.h create mode 100644 src/external/hauser_parabolic_smoother/Timer.cpp create mode 100644 src/external/hauser_parabolic_smoother/Timer.h create mode 100644 src/planner/SimpleDynamicPath.cpp create mode 100644 src/planner/SimpleDynamicPath.h create mode 100644 src/planner/SmootherHelpers.cpp create mode 100644 src/planner/SmootherUtil.cpp create mode 100644 src/planner/SmootherUtil.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 88c034a..d4c65a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,8 +52,6 @@ find_package(aikido 0.3.0 REQUIRED include_directories(${aikido_INCLUDE_DIRS}) find_package(libada REQUIRED) include_directories(${libada_INCLUDE_DIRS}) -find_package(pr_assets REQUIRED) -include_directories(${pr_assets_INCLUDE_DIRS}) find_package(pr_tsr REQUIRED) include_directories(${pr_tsr_INCLUDE_DIRS}) find_package(Boost COMPONENTS program_options REQUIRED) @@ -61,6 +59,8 @@ include_directories(${Boost_INCLUDE_DIRS}) include_directories(include) +add_subdirectory("src/external/hauser_parabolic_smoother") + add_executable(wecook src/main.cpp src/TaskManager.cpp @@ -121,18 +121,42 @@ add_executable(wecook src/TSRIKNode.cpp include/wecook/TaskGraph.h src/TaskGraph.cpp - include/wecook/ActionNode.h include/wecook/PrimitiveTaskGraph.h include/wecook/PrimitiveActionNode.h include/wecook/PrimitiveGraspNode.h include/wecook/Human.h include/wecook/Robot.h include/wecook/PrimitiveEngageNode.h include/wecook/PrimitiveActuateNode.h include/wecook/PrimitivePlaceNode.h include/wecook/RelativeIKMotionNode.h src/RelativeIKMotionNode.cpp src/PrimitiveTaskGraph.cpp src/PrimitiveActionNode.cpp include/wecook/PrimitiveActionExecutor.h src/PrimitiveActionExecutor.cpp src/PrimitiveGraspNode.cpp src/PrimitiveEngageNode.cpp src/PrimitivePlaceNode.cpp src/PrimitiveActuateNode.cpp src/Robot.cpp src/Human.cpp include/wecook/ObjectMgr.h src/ObjectMgr.cpp include/wecook/TaskExecutorThread.h src/TaskExecutorThread.cpp) + include/wecook/ActionNode.h + include/wecook/PrimitiveTaskGraph.h + include/wecook/PrimitiveActionNode.h + include/wecook/PrimitiveGraspNode.h + include/wecook/Human.h + include/wecook/Robot.h + include/wecook/PrimitiveEngageNode.h + include/wecook/PrimitiveActuateNode.h + include/wecook/PrimitivePlaceNode.h + include/wecook/RelativeIKMotionNode.h + src/RelativeIKMotionNode.cpp + src/PrimitiveTaskGraph.cpp + src/PrimitiveActionNode.cpp + include/wecook/PrimitiveActionExecutor.h + src/PrimitiveActionExecutor.cpp + src/PrimitiveGraspNode.cpp + src/PrimitiveEngageNode.cpp + src/PrimitivePlaceNode.cpp + src/PrimitiveActuateNode.cpp + src/Robot.cpp src/Human.cpp + include/wecook/ObjectMgr.h + src/ObjectMgr.cpp + include/wecook/TaskExecutorThread.h + src/TaskExecutorThread.cpp + include/wecook/planner/SmootherHelpers.h + src/planner/SmootherHelpers.cpp src/planner/SmootherUtil.h src/planner/SmootherUtil.cpp src/planner/SimpleDynamicPath.h src/planner/SimpleDynamicPath.cpp include/wecook/MetaActuateInfo.h) add_dependencies(wecook wecook_generate_messages_cpp) target_link_libraries(wecook ${DART_LIBRARIES} ${aikido_LIBRARIES} ${Boost_LIBRARIES} - libada) + libada + ${PROJECT_NAME}_external_hauserparabolicsmoother) install(TARGETS wecook RUNTIME DESTINATION bin) -#catkin_install_python(PROGRAMS scripts/ex_task1.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - #include(ClangFormat) #clang_format_setup(VERSION 3.8) # diff --git a/include/wecook/Action.h b/include/wecook/Action.h index dce928b..21d9369 100644 --- a/include/wecook/Action.h +++ b/include/wecook/Action.h @@ -16,16 +16,29 @@ class Action { const std::vector &location, const std::vector &ingredients, const std::string &verb, - const std::string &tool) : m_pids(pids), m_ingredients(ingredients), m_location(location), m_tool(tool), m_verb(verb) { + const std::string &tool) + : m_pids(pids), m_ingredients(ingredients), m_locations(location), m_tool(tool), m_verb(verb) { } + Action() { + + } + + Action(const Action &other) { + m_pids = other.get_pids(); + m_ingredients = other.get_ingredients(); + m_locations = other.get_locations(); + m_tool = other.get_tool(); + m_verb = other.get_verb(); + } + std::string get_verb() const { return m_verb; } - std::vector get_location() const { - return m_location; + std::vector get_locations() const { + return m_locations; } std::string get_tool() const { @@ -42,7 +55,7 @@ class Action { private: std::vector m_pids; - std::vector m_location; + std::vector m_locations; std::string m_tool; std::vector m_ingredients; std::string m_verb; diff --git a/include/wecook/ActionNode.h b/include/wecook/ActionNode.h index e2e5e2f..f10bfc2 100644 --- a/include/wecook/ActionNode.h +++ b/include/wecook/ActionNode.h @@ -24,6 +24,10 @@ class ActionNode { return m_children; } + inline std::vector getFathers() { + return m_fathers; + } + inline void addChild(ActionNode *actionNode) { m_children.emplace_back(actionNode); } @@ -52,12 +56,14 @@ class ActionNode { std::map>> m_motionSeqMap; PrimitiveTaskGraph m_primitiveTaskGraph; + bool m_ifExecuted = false; private: Action m_action; bool m_ifHead; std::vector m_children; std::vector m_fathers; std::vector m_pids; + }; } diff --git a/include/wecook/ContainingMap.h b/include/wecook/ContainingMap.h index 7a8365d..e1b3b99 100644 --- a/include/wecook/ContainingMap.h +++ b/include/wecook/ContainingMap.h @@ -47,6 +47,16 @@ class ContainingMap { return nullptr; } + inline std::vector getContainedBodyNodes(const std::string &containerName) { + std::vector ret; + for (const auto &container : m_containers) { + if (container.getContainerName() == containerName) { + ret.emplace_back(container.getContainedBodyNode()); + } + } + return ret; + } + inline bool hasTuple(const std::string &containerName, const std::string &containedName) { for (const auto &container : m_containers) { if (container.getContainerName() == containerName && container.getContainedName() == containedName) { diff --git a/include/wecook/MetaActuateInfo.h b/include/wecook/MetaActuateInfo.h new file mode 100644 index 0000000..b07b66d --- /dev/null +++ b/include/wecook/MetaActuateInfo.h @@ -0,0 +1,30 @@ +// +// Created by hejia on 9/19/19. +// + +#ifndef WECOOK_METAACTUATEINFO_H +#define WECOOK_METAACTUATEINFO_H + +#include "Action.h" + +namespace wecook { +class MetaActuateInfo { + public: + MetaActuateInfo(const Action &action) : m_action(action) { + + } + + MetaActuateInfo(const MetaActuateInfo& other) { + m_action = other.getAction(); + } + + Action getAction() const { + return m_action; + } + + private: + Action m_action; +}; +} + +#endif //WECOOK_METAACTUATEINFO_H diff --git a/include/wecook/ObjectMgr.h b/include/wecook/ObjectMgr.h index 7074e0b..7532e32 100644 --- a/include/wecook/ObjectMgr.h +++ b/include/wecook/ObjectMgr.h @@ -70,7 +70,8 @@ class ObjectMgr { } dart::collision::CollisionGroupPtr createCollisionGroupExceptFoodAndMovingObj(const std::string &toMove, - dart::collision::FCLCollisionDetectorPtr &collisionDetector); + dart::collision::FCLCollisionDetectorPtr &collisionDetector, + dart::dynamics::BodyNode *blackNode); private: std::map m_objects; diff --git a/include/wecook/PrimitiveActionNode.h b/include/wecook/PrimitiveActionNode.h index 738bdf5..d14e938 100644 --- a/include/wecook/PrimitiveActionNode.h +++ b/include/wecook/PrimitiveActionNode.h @@ -98,6 +98,12 @@ class PrimitiveActionNode { void removeFather(std::shared_ptr &father); + double m_timeStep = 0.02; + + void setTimeStep(double timeStep) { + m_timeStep = timeStep; + } + protected: bool m_ifTail; bool m_ifHead; diff --git a/include/wecook/PrimitiveActuateNode.h b/include/wecook/PrimitiveActuateNode.h index ce19d37..5c68a9b 100644 --- a/include/wecook/PrimitiveActuateNode.h +++ b/include/wecook/PrimitiveActuateNode.h @@ -10,6 +10,7 @@ #include "PrimitiveActionNode.h" #include "ContainingMap.h" +#include "MetaActuateInfo.h" namespace wecook { @@ -20,6 +21,7 @@ class PrimitiveActuateNode : public PrimitiveActionNode { const std::string &motionType, const std::string &grabbingObj, const std::string &placingObj, + const MetaActuateInfo &metaActuateInfo, bool ifHead = false, bool ifTail = false) : PrimitiveActionNode(pid, @@ -27,7 +29,10 @@ class PrimitiveActuateNode : public PrimitiveActionNode { grabbingObj, placingObj, ifHead, - ifTail), m_motionType(motionType), m_manipulatedObj(manipulatedObj) { + ifTail), + m_motionType(motionType), + m_manipulatedObj(manipulatedObj), + m_metaActuateInfo(metaActuateInfo) { } @@ -36,6 +41,7 @@ class PrimitiveActuateNode : public PrimitiveActionNode { std::shared_ptr &containingMap); private: + MetaActuateInfo m_metaActuateInfo; std::string m_motionType; std::string m_manipulatedObj; }; diff --git a/include/wecook/PrimitivePlaceNode.h b/include/wecook/PrimitivePlaceNode.h index 5b28bab..4c78b45 100644 --- a/include/wecook/PrimitivePlaceNode.h +++ b/include/wecook/PrimitivePlaceNode.h @@ -18,12 +18,14 @@ class PrimitivePlaceNode : public PrimitiveActionNode { const std::string &pid, const std::string &grabbingObj, const std::string &placingObj, - bool ifHead = false, - bool ifTail = false) + bool ifHead, + bool ifTail, + bool ifDebug = false) : PrimitiveActionNode(pid, "place", grabbingObj, placingObj, ifHead, ifTail), m_refObject(refObject), m_targetPose(targetPose), - m_toPlace(toPlace) { + m_toPlace(toPlace), + m_ifDebug(ifDebug) { } @@ -32,6 +34,7 @@ class PrimitivePlaceNode : public PrimitiveActionNode { std::shared_ptr &containingMap); private: + bool m_ifDebug; std::string m_toPlace; aikido::constraint::dart::TSRPtr m_targetPose; std::string m_refObject; diff --git a/include/wecook/TSRMotionNode.h b/include/wecook/TSRMotionNode.h index ded4cdc..8619f12 100644 --- a/include/wecook/TSRMotionNode.h +++ b/include/wecook/TSRMotionNode.h @@ -27,11 +27,16 @@ class TSRMotionNode : public MotionNode { void plan(const std::shared_ptr &ada); + void setTimeStep(double timeStep) { + m_retimeTimeStep = timeStep; + } + private: aikido::constraint::dart::CollisionFreePtr m_collisionFree = nullptr; dart::dynamics::BodyNode *m_bn = nullptr; aikido::constraint::dart::TSRPtr m_goalTSR; bool m_debug; + double m_retimeTimeStep = 0.02; }; } diff --git a/include/wecook/TaskGraph.h b/include/wecook/TaskGraph.h index a81d0d0..a9f607d 100644 --- a/include/wecook/TaskGraph.h +++ b/include/wecook/TaskGraph.h @@ -13,8 +13,15 @@ namespace wecook { class TaskGraph { public: TaskGraph(const std::vector &actionSeq) { + int i = 0; for (auto &action : actionSeq) { - addNode(action); + i += 1; + if (i == 6) { + addNode(action, true); + } else { + addNode(action); + } + } } @@ -24,7 +31,7 @@ class TaskGraph { } } - void addNode(const Action& action); + void addNode(const Action& action, bool ifEnd=false); void addArc(); diff --git a/include/wecook/planner/SmootherHelpers.h b/include/wecook/planner/SmootherHelpers.h new file mode 100644 index 0000000..87d3557 --- /dev/null +++ b/include/wecook/planner/SmootherHelpers.h @@ -0,0 +1,77 @@ +// +// Created by hejia on 9/16/19. +// + +#ifndef WECOOK_SMOOTHERHELPERS_H +#define WECOOK_SMOOTHERHELPERS_H + +#include +#include +#include +#include +#include +#include +#include + +namespace wecook { +namespace planner { +/* + * Simple short cutting + */ +aikido::trajectory::InterpolatedPtr simpleSmoothPath(const std::shared_ptr &ada, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Trajectory *path, + const aikido::constraint::TestablePtr &constraint); + +aikido::trajectory::InterpolatedPtr simpleDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit = 6.0, + double checkResolution = 5e-4, + double tolerance = 1e-3); + +/* + * Smoothing with Kris Hauser's parabolic smoother + */ +aikido::trajectory::UniqueSplinePtr hauserSmoothPath(const std::shared_ptr &ada, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Trajectory *path, + const aikido::constraint::TestablePtr &constraint); + +aikido::trajectory::UniqueSplinePtr hauserDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit = 6.0, + double checkResolution = 5e-4, + double tolerance = 1e-3); + +aikido::trajectory::InterpolatedPtr hauserSmoothPathInterpolated(const std::shared_ptr &ada, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Trajectory *path, + const aikido::constraint::TestablePtr &constraint); + +aikido::trajectory::InterpolatedPtr hauserDoShortcutInterpolated(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit = 8.0, + double checkResolution = 5e-4, + double tolerance = 1e-3); +} +} + +#endif //WECOOK_SMOOTHERHELPERS_H diff --git a/package.xml b/package.xml index 91451a6..79b1b3e 100644 --- a/package.xml +++ b/package.xml @@ -12,7 +12,6 @@ libada tf_conversions ada_description - pr_assets pr_tsr cv_bridge image_transport diff --git a/scripts/fruit_jelly.py b/scripts/fruit_jelly.py index d470a4a..8f9d03d 100755 --- a/scripts/fruit_jelly.py +++ b/scripts/fruit_jelly.py @@ -11,51 +11,57 @@ def talker(): scene_msg = SceneMsg([ObjectMsg('table0', 'package://wecook_assets/data/furniture/table.urdf', [0.5, 0.0, 0.0, 0., 0., 0., 1.]), + ObjectMsg('oil0', + 'package://wecook_assets/data/objects/oil.urdf', + [0.2, -0.95, 0.87, 0., 0., 0., 1.]), + ObjectMsg('plant0', + 'package://wecook_assets/data/objects/plant.urdf', + [0.65, -0.75, 0.73, 0., 0., 0., 1.]), ObjectMsg('plate1', 'package://wecook_assets/data/objects/plate.urdf', - [0.2, -0.10, 0.73, 0., 0., 0., 1.]), + [0.2, -0.15, 0.73, 0., 0., 0., 1.]), ObjectMsg('plate0', 'package://wecook_assets/data/objects/plate.urdf', - [0.2, 0.25, 0.73, 0., 0., 0., 1.]), + [0.2, 0.2, 0.73, 0., 0., 0., 1.]), ObjectMsg('bowl0', 'package://wecook_assets/data/objects/bowl.urdf', - [0.4, 0.05, 0.73, 0., 0., 0., 1.]), + [0.4, 0.0, 0.73, 0., 0., 0., 1.]), ObjectMsg('shelf0', 'package://wecook_assets/data/furniture/shelf.urdf', [0., -1.10, 0.65, 0., 0., 0., 1.]), ObjectMsg('knife1', 'package://wecook_assets/data/objects/knife.urdf', - [0.15, -0.6, 0.78, 0., 0, 0., 1.]), + [0.18, -0.55, 0.78, 0., 0, 0., 1.]), ObjectMsg('knifeHolder1', 'package://wecook_assets/data/objects/holder.urdf', - [0.25, -0.6, 0.73, 0., 0., 0., 1]), + [0.25, -0.55, 0.73, 0., 0., 0., 1]), ObjectMsg('knife0', 'package://wecook_assets/data/objects/knife.urdf', - [0.15, 0.6, 0.78, 0., 0, 0., 1.]), + [0.18, 0.65, 0.78, 0., 0, 0., 1.]), ObjectMsg('knifeHolder0', 'package://wecook_assets/data/objects/holder.urdf', - [0.25, 0.6, 0.73, 0., 0., 0., 1]), + [0.25, 0.65, 0.73, 0., 0., 0., 1]), ObjectMsg('cooker0', 'package://wecook_assets/data/objects/cooker.urdf', [-0.8, 0., 0.86, 0., 0., 0., 1.]), ObjectMsg('spoon1', 'package://wecook_assets/data/objects/spoon.urdf', - [0.3, -0.50, 0.78, 0, -0.7073, 0, 0.7073883]), + [0.3, -0.4, 0.78, 0, -0.7073, 0, 0.7073883]), ObjectMsg('food_item1', 'package://wecook_assets/data/food/food_item0.urdf', - [0.2, -0.10, 0.75, 0., 0., 0., 1.]), + [0.2, -0.15, 0.75, 0., 0., 0., 1.]), ObjectMsg('food_item0', 'package://wecook_assets/data/food/food_item1.urdf', - [0.2, 0.25, 0.75, 0., 0., 0., 1.]), + [0.2, 0.2, 0.75, 0., 0., 0., 1.]), ObjectMsg('spoonHolder1', 'package://wecook_assets/data/objects/holder.urdf', - [0.25, -0.50, 0.73, 0., 0., 0., 1]), + [0.25, -0.4, 0.73, 0., 0., 0., 1]), ObjectMsg('spoon0', 'package://wecook_assets/data/objects/spoon.urdf', - [0.30, 0.5, 0.78, 0, -0.7073, 0, 0.7073883]), + [0.3, 0.45, 0.78, 0, -0.7073, 0, 0.7073883]), ObjectMsg('spoonHolder0', 'package://wecook_assets/data/objects/holder.urdf', - [0.25, 0.5, 0.73, 0., 0., 0., 1])], + [0.25, 0.45, 0.73, 0., 0., 0., 1])], [ContainingMsg(['plate1', 'food_item1']), ContainingMsg(['plate0', 'food_item0'])]) @@ -65,7 +71,7 @@ def talker(): ActionMsg(['p2'], 'stir', ['bowl0'], 'spoon1', ['food_item0']), ActionMsg(['p1', 'p2'], 'holding_plate1_transfer', ['plate1', 'bowl0'], 'spoon1', ['food_item1']), ActionMsg(['p2'], 'stir', ['bowl0'], 'spoon1', ['food_item0', 'food_item1']), - ActionMsg(['p1'], 'stir', ['bowl0'], 'spoon0', ['food_item0', 'food_item1'])]) + ActionMsg(['p1'], 'stir', ['bowl0'], 'spoon0', ['food_item0', 'food_item1'])]) # task_msg = TaskMsg(scene_msg, [ActionMsg(['p1', 'p2'], 'holding_plate0_transfer', ['plate0', 'bowl0'], 'spoon1', ['food_item0'])]) diff --git a/src/ActionPlanner.cpp b/src/ActionPlanner.cpp index 43da128..973f717 100644 --- a/src/ActionPlanner.cpp +++ b/src/ActionPlanner.cpp @@ -96,7 +96,7 @@ void ActionPlanner::planRoll(ActionNode *actionNode, // 3) create predefined rolling node auto predefinedNode = - std::make_shared(pid, "end-effector", "roll", toolName, "", false, false); + std::make_shared(pid, "end-effector", "roll", toolName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 4) create place back node auto placePose = std::make_shared(); @@ -170,7 +170,7 @@ void ActionPlanner::planHeat(ActionNode *actionNode, // 3) create predefined heating node auto predefinedNode = - std::make_shared(pid, "end-effector", "heat", toolName, "", false, false); + std::make_shared(pid, "end-effector", "heat", toolName, "", MetaActuateInfo(Action(actionNode->getAction())), false, false); // 4) create place back node auto placePose = std::make_shared(); @@ -224,7 +224,7 @@ void ActionPlanner::planCut(ActionNode *actionNode, 0., -1., 0., 0., 0., -1; grabPose->mTw_e.linear() = rot; - auto epsilon = 0.02; + auto epsilon = 0.005; grabPose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; auto toolName = actionNode->getAction().get_tool(); @@ -237,7 +237,7 @@ void ActionPlanner::planCut(ActionNode *actionNode, // 2) create move to node // create start pose auto targetPose = std::make_shared(); - targetPose->mTw_e.translation() = Eigen::Vector3d(-0.12, 0., 0.06); + targetPose->mTw_e.translation() = Eigen::Vector3d(-0.08, 0., 0.06); targetPose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; auto toCutName = actionNode->getAction().get_ingredients()[0]; @@ -253,13 +253,13 @@ void ActionPlanner::planCut(ActionNode *actionNode, // 3) create predefined cutting node auto predefinedNode = - std::make_shared(pid, "end-effector", "cut", toolName, "", false, false); + std::make_shared(pid, "end-effector", "cut", toolName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 4) create place back node auto placePose = std::make_shared(); // get the original place of the tool auto translation = objectMgr->getObjTransform(toolName).translation(); - epsilon = 0.01; + epsilon = 0.005; placePose->mTw_e.translation() = translation - Eigen::Vector3d(0.5, 0.0, 0.); placePose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; @@ -301,7 +301,7 @@ void ActionPlanner::planStir(ActionNode *actionNode, // 1) create grab node // create grab pose auto grabPose = std::make_shared(); - grabPose->mTw_e.translation() = Eigen::Vector3d(0.0, 0., 0.15); + grabPose->mTw_e.translation() = Eigen::Vector3d(0.0, 0., 0.12); Eigen::Matrix3d rot; rot << 1., 0., 0., @@ -322,7 +322,7 @@ void ActionPlanner::planStir(ActionNode *actionNode, targetPose->mTw_e.translation() = Eigen::Vector3d(0., 0., 0.06); targetPose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -M_PI, M_PI; - auto toStirName = actionNode->getAction().get_location()[0]; + auto toStirName = actionNode->getAction().get_locations()[0]; auto moveToNode = std::make_shared(targetPose, toolName, @@ -335,7 +335,7 @@ void ActionPlanner::planStir(ActionNode *actionNode, // 3) create predefined stirring node auto predefinedNode = - std::make_shared(pid, "end-effector", "stir", toolName, "", false, false); + std::make_shared(pid, "end-effector", "stir", toolName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 4) create place back node auto placePose = std::make_shared(); @@ -400,7 +400,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, // 2) create place node auto placePose = std::make_shared(); - auto newLocationName = actionNode->getAction().get_location()[1]; + auto newLocationName = actionNode->getAction().get_locations()[1]; placePose->mTw_e.translation() = Eigen::Vector3d(0, 0, 0.05); placePose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -M_PI / 8, M_PI / 8, -M_PI / 8, M_PI @@ -427,7 +427,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, actionNode->setPrimitiveTaskGraph(ptg); return; - } else if (actionNode->getAction().get_tool() == actionNode->getAction().get_location()[0]) { + } else if (actionNode->getAction().get_tool() == actionNode->getAction().get_locations()[0]) { // To do transfer object by old container, we have 2 steps: grab old container, move container to start position, // do predefined motion // 1) create grab node @@ -446,7 +446,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, grabPose->mTw_e.linear() = rot2 * rot; grabPose->mTw_e.translation() = Eigen::Vector3d(0.0, 0.06, 0.08); grabPose->mBw << -0.01, 0.01, 0., 0., -0.01, 0.01, -M_PI / 8, M_PI / 8, -M_PI / 8, M_PI / 8, -0.02, 0.02; - auto objectName = actionNode->getAction().get_location()[0]; + auto objectName = actionNode->getAction().get_locations()[0]; auto pid = actionNode->getAction().get_pids()[0]; auto grabNode = std::make_shared(grabPose, objectName, objectName, pid, objectName, "", true, false); @@ -457,7 +457,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, auto epsilon = 0.02; targetPose->mBw << -epsilon / 2, epsilon / 2, -epsilon / 2, epsilon / 2, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; - auto newLocationName = actionNode->getAction().get_location()[1]; + auto newLocationName = actionNode->getAction().get_locations()[1]; auto moveToNode = std::make_shared(targetPose, objectName, @@ -470,7 +470,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, // 3) create predefined pouring node auto predefinedNode = - std::make_shared(pid, objectName, "transfer1", objectName, "", false, false); + std::make_shared(pid, objectName, "transfer1", objectName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 4) create place back node auto placePose = std::make_shared(); @@ -533,7 +533,7 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, targetPose->mTw_e.translation() = Eigen::Vector3d(0., 0., 0.03); targetPose->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; - auto oldLocationName = actionNode->getAction().get_location()[0]; + auto oldLocationName = actionNode->getAction().get_locations()[0]; auto ingredientName = actionNode->getAction().get_ingredients()[0]; auto moveToNode = std::make_shared(targetPose, @@ -547,20 +547,20 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, // 3) create predefined motion node auto predefinedNode = - std::make_shared(pid, toolName, "transfer2", toolName, "", false, false); + std::make_shared(pid, toolName, "transfer2", toolName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 4) move food to new location auto targetPose2 = std::make_shared(); Eigen::Matrix3d rot2; rot2 << - 1., 0., 0., + 1., 0., 0., 0., 0.7071055, -0.7071081, 0., 0.7071081, 0.7071055; targetPose2->mTw_e.linear() = rot2; targetPose2->mTw_e.translation() = Eigen::Vector3d(0, 0, 0.06); targetPose2->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -M_PI / 8, M_PI / 8, -M_PI / 8, M_PI / 8, -M_PI / 8, M_PI / 8; - auto newLocationName = actionNode->getAction().get_location()[1]; + auto newLocationName = actionNode->getAction().get_locations()[1]; auto moveToNode2 = std::make_shared(targetPose2, toolName, @@ -570,11 +570,12 @@ void ActionPlanner::planTransfer(ActionNode *actionNode, "", false, false); + moveToNode2->setTimeStep(0.02); // 5) create predefined pouring node auto predefinedNode2 = - std::make_shared(pid, toolName, "transfer3", toolName, "", false, false); + std::make_shared(pid, toolName, "transfer3", toolName, "", MetaActuateInfo(Action(actionNode->getAction())),false, false); // 6) create place back node auto placePose = std::make_shared(); @@ -727,7 +728,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, std::cout << collaborativeAction << std::endl; auto holdedObjectName = actionName.substr(occurances[0] + 1, occurances.back() - occurances[0] - 1); if (collaborativeAction == "transfer") { - if (holdedObjectName == actionNode->getAction().get_location()[1]) { + if (holdedObjectName == actionNode->getAction().get_locations()[1]) { // To do collaborative transferring, we have 9 steps // 1) create robotM grab node auto grabPoseM = std::make_shared(); @@ -738,7 +739,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, 0, 0, -1; Eigen::Matrix3d rot2; rot2 << - 1., 0., 0., + 1., 0., 0., 0., 0.8678, 0.4969, 0., -0.4969, 0.86781; @@ -793,7 +794,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, true); // build an new action node and plan it to get the other sub primitiveTaskGraph - Action action{std::vector{pidS}, actionNode->getAction().get_location(), + Action action{std::vector{pidS}, actionNode->getAction().get_locations(), actionNode->getAction().get_ingredients(), "transfer", actionNode->getAction().get_tool()}; auto collaborativeActionNode = new ActionNode(action, std::vector{pidS}); planTransfer(collaborativeActionNode, agents, containingMap, objectMgr); @@ -833,7 +834,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, delete (collaborativeActionNode); return; - } else if (holdedObjectName == actionNode->getAction().get_location()[0]) { + } else if (holdedObjectName == actionNode->getAction().get_locations()[0]) { // If the holder is holding the coontainer which is the source of transfer // 1) create robotM grab node auto grabPoseM = std::make_shared(); @@ -886,7 +887,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, // now we will place it back to its original place auto translation = objectMgr->getObjTransform(holdedObjectName).translation(); placePoseM->mTw_e.translation() = translation - Eigen::Vector3d(0.5, 0.0, 0.); - auto epsilon = 0.02; + auto epsilon = 0.005; placePoseM->mBw << -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon, -epsilon, epsilon; auto placeNodeM = std::make_shared(placePoseM, @@ -899,7 +900,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, true); // build an new action node and plan it to get the other sub primitiveTaskGraph - Action action{std::vector{pidS}, actionNode->getAction().get_location(), + Action action{std::vector{pidS}, actionNode->getAction().get_locations(), actionNode->getAction().get_ingredients(), "transfer", actionNode->getAction().get_tool()}; auto collaborativeActionNode = new ActionNode(action, std::vector{pidS}); planTransfer(collaborativeActionNode, agents, containingMap, objectMgr); @@ -917,10 +918,15 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, moveToNodeM->addFather(grabNodeM); moveToNodeM->addChild(placeNodeM); placeNodeM->addFather(moveToNodeM); +// grabNodeM->addChild(grabNodeS); +// grabNodeS->addFather(grabNodeM); + moveToNodeM->addChild(grabNodeS); + grabNodeS->addFather(moveToNodeM); + // TODO inherit; moveToNodeM->addChild(moveToNodeS); moveToNodeS->addFather(moveToNodeM); - predefinedNodeS2->addChild(placeNodeM); - placeNodeM->addFather(predefinedNodeS2); + moveToNodeS2->addChild(placeNodeM); + placeNodeM->addFather(moveToNodeS2); // build sub primitive task graph for this action node PrimitiveTaskGraph ptg{}; @@ -989,7 +995,7 @@ void ActionPlanner::planHolding(wecook::ActionNode *actionNode, // 3) for the other agent, build a new action node auto pidS = actionNode->getAction().get_pids()[1]; - Action action{std::vector{pidS}, actionNode->getAction().get_location(), + Action action{std::vector{pidS}, actionNode->getAction().get_locations(), actionNode->getAction().get_ingredients(), "cut", actionNode->getAction().get_tool()}; auto collaborativeActionNode = new ActionNode(action, std::vector{pidS}); planCut(collaborativeActionNode, agents, containingMap, objectMgr); @@ -1254,7 +1260,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // ROS_INFO("Collaborative transferring!"); // ROS_INFO_STREAM(holdedObjectName << " " << collaborativeAction); // -// if (holdedObjectName != actionNode->getAction().get_location()[0]) { +// if (holdedObjectName != actionNode->getAction().get_locations()[0]) { // // we need robot to move holded object to collaborative point // auto collaborativePoint = Eigen::Isometry3d::Identity(); // collaborativePoint.translation() = Eigen::Vector3d(0.3, -0.175, 0.85); @@ -1378,7 +1384,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // // for (auto foodName : actionNode->getAction().get_ingredients()) { // // Move spoon to be inside pot -// auto oldLocationName = actionNode->getAction().get_location()[0]; +// auto oldLocationName = actionNode->getAction().get_locations()[0]; // auto oldLocationSkeleton = worldS->getSkeleton(oldLocationName); // auto oldLocationPose = getObjectPose(oldLocationSkeleton, containingMap); // auto oldLocationTSR = std::make_shared(); @@ -1622,7 +1628,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // motionSeq.emplace_back(motion3); // // // Move spoon to be above pot -// auto locationName = actionNode->getAction().get_location()[0]; +// auto locationName = actionNode->getAction().get_locations()[0]; // auto locationSkeleton = world->getSkeleton(locationName); // auto locationPose = getObjectPose(locationSkeleton, containingMap); // auto locationTSR = std::make_shared(); @@ -1865,7 +1871,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // motionSeq.emplace_back(motion1); // // // unconnect food with old container -// auto oldLocationName = actionNode->getAction().get_location()[0]; +// auto oldLocationName = actionNode->getAction().get_locations()[0]; // auto oldLocationSkeleton = world->getSkeleton(oldLocationName); // auto motion8 = std::make_shared(foodSkeleton, // oldLocationSkeleton, @@ -1900,7 +1906,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // } // // // move food to new position -// auto locationName = actionNode->getAction().get_location()[1]; +// auto locationName = actionNode->getAction().get_locations()[1]; // auto locationSkeleton = world->getSkeleton(locationName); // auto locationPose = getObjectPose(locationSkeleton, containingMap); // auto locationTSR = std::make_shared(); @@ -1945,13 +1951,13 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // armSpace, // armSkeleton); // motionSeq.emplace_back(motion9); -// } else if (actionNode->getAction().get_tool() == actionNode->getAction().get_location()[0]) { +// } else if (actionNode->getAction().get_tool() == actionNode->getAction().get_locations()[0]) { // ROS_INFO_STREAM( // "Transferring " << actionNode->getAction().get_ingredients()[0] << " from " -// << actionNode->getAction().get_location()[0] << " to " -// << actionNode->getAction().get_location()[1]); +// << actionNode->getAction().get_locations()[0] << " to " +// << actionNode->getAction().get_locations()[1]); // // transfer food by doing pouring -// auto oldLocationName = actionNode->getAction().get_location()[0]; +// auto oldLocationName = actionNode->getAction().get_locations()[0]; // auto oldLocationSkeleton = world->getSkeleton(oldLocationName); // auto oldLocationPose = Eigen::Isometry3d::Identity(); // need to find it out later // dart::dynamics::BodyNode *oldLocationBodyNodePtr = nullptr; // need to find it out later @@ -1971,7 +1977,7 @@ void ActionPlanner::planSqueeze(ActionNode *actionNode, // auto epsilon = 0.02; // // now move the old location object to the new location, // // to easily pour food from old container to new container. -// auto newLocationName = actionNode->getAction().get_location()[1]; +// auto newLocationName = actionNode->getAction().get_locations()[1]; // auto newLocationSkeleton = world->getSkeleton(newLocationName); // auto newLocationPose = getObjectPose(newLocationSkeleton, containingMap); // auto newLocationTSR = std::make_shared(); diff --git a/src/ObjectMgr.cpp b/src/ObjectMgr.cpp index 3416f8d..6e248c0 100644 --- a/src/ObjectMgr.cpp +++ b/src/ObjectMgr.cpp @@ -24,14 +24,24 @@ void ObjectMgr::clear(std::vector &objects, bool ifSim, aikido::planner: } dart::collision::CollisionGroupPtr ObjectMgr::createCollisionGroupExceptFoodAndMovingObj(const std::string &toMove, - dart::collision::FCLCollisionDetectorPtr &collisionDetector) { + dart::collision::FCLCollisionDetectorPtr &collisionDetector, + dart::dynamics::BodyNode *blackNode) { auto envCollisionGroup = collisionDetector->createCollisionGroup(); for (auto &object : m_objects) { - if (object.first == toMove || object.first.find("food") != std::string::npos - || object.first.find("knife") != std::string::npos || object.first.find("spoon") != std::string::npos) + if ((object.first.find("knife") != std::string::npos || object.first.find("fork") != std::string::npos + || object.first.find("spoon") != std::string::npos) + && (toMove.find("knife") != std::string::npos || toMove.find("fork") != std::string::npos + || toMove.find("spoon") != std::string::npos) && toMove.back() != object.first.back()) { continue; - ROS_INFO_STREAM("Adding object for collision detect " << object.first); - envCollisionGroup->addShapeFramesOf(object.second.getBodyNode()); + } + if (object.first == toMove || object.first.find("food") != std::string::npos) + continue; +// ROS_INFO_STREAM("Adding object for collision detect " << object.first); + auto newNode = object.second.getBodyNode(); + if (blackNode && newNode == blackNode) { + continue; + } + envCollisionGroup->addShapeFramesOf(newNode); } return envCollisionGroup; } \ No newline at end of file diff --git a/src/PrimitiveActuateNode.cpp b/src/PrimitiveActuateNode.cpp index dce8db4..d10cf15 100644 --- a/src/PrimitiveActuateNode.cpp +++ b/src/PrimitiveActuateNode.cpp @@ -36,48 +36,67 @@ void PrimitiveActuateNode::execute(std::map> if (m_motionType == "cut") { for (int j = 0; j < 3; j++) { - Eigen::Vector3d delta_x(0., 0., -0.001); + Eigen::Vector3d delta_x(0., 0., -0.003); auto motion4 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion4->plan(robot->m_ada); - delta_x << 0., 0., 0.001; + delta_x << 0., 0., 0.003; auto motion5 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion5->plan(robot->m_ada); } } else if (m_motionType == "stir") { + Eigen::Vector3d delta_x(-0.003, 0., -0.); + delta_x << +0.003, 0., 0.00; + auto motion6 = + std::make_shared(bn, + delta_x, + dart::dynamics::Frame::World(), + 10, + armSpace, + armSkeleton); + motion6->plan(robot->m_ada); for (int j = 0; j < 3; j++) { - Eigen::Vector3d delta_x(-0.001, 0., -0.); + Eigen::Vector3d delta_x(-0.003, 0., -0.); auto motion5 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 20, armSpace, armSkeleton); motion5->plan(robot->m_ada); - delta_x << +0.001, 0., 0.00; + delta_x << +0.003, 0., 0.00; auto motion6 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 20, armSpace, armSkeleton); motion6->plan(robot->m_ada); } + delta_x << -0.003, 0., 0.00; + motion6 = + std::make_shared(bn, + delta_x, + dart::dynamics::Frame::World(), + 10, + armSpace, + armSkeleton); + motion6->plan(robot->m_ada); } else if (m_motionType == "transfer1") { auto rotatePose = Eigen::Isometry3d::Identity(); rotatePose.linear() << 0.5000, 0.500000, 0.7071, 0.5000, 0.5000, -0.7071, -0.7071, 0.7071, 0.0000; @@ -89,34 +108,42 @@ void PrimitiveActuateNode::execute(std::map> motion->plan(robot->m_ada); } else if (m_motionType == "transfer2") { // hacking -// auto world = robot->getWorld(); -// auto ingredientNode = world->getSkeleton("food_item0"); -// auto chopping_board = world->getSkeleton("chopping_board0"); -// auto toolNode = world->getSkeleton("spoon1"); -// robotHand->ungrab(); -// auto motionunconnect = std::make_shared(ingredientNode, chopping_board, "chopping_board0", "food_item0", containingMap, false, armSpace, armSkeleton); -// motionunconnect->plan(robot->m_ada); -// -// auto motionConnect = std::make_shared(ingredientNode, toolNode, "spoon1", "food_item0", containingMap, true, armSpace, armSkeleton); -// motionConnect->plan(robot->m_ada); -// robotHand->grab(toolNode); + // this action is used to grasp some food with the tool + auto action = m_metaActuateInfo.getAction(); + auto ingredientName = action.get_ingredients()[0]; + auto oldLocationName = action.get_locations()[0]; + auto toolName = action.get_tool(); + + auto world = robot->getWorld(); + auto ingredientNode = world->getSkeleton(ingredientName); + auto oldLocation = world->getSkeleton(oldLocationName); + auto toolNode = world->getSkeleton(toolName); + // ungrab for connect + robotHand->ungrab(); + auto motionunconnect = std::make_shared(ingredientNode, oldLocation, oldLocationName, ingredientName, containingMap, false, armSpace, armSkeleton); + motionunconnect->plan(robot->m_ada); + + auto motionConnect = std::make_shared(ingredientNode, toolNode, toolName, ingredientName, containingMap, true, armSpace, armSkeleton); + motionConnect->plan(robot->m_ada); + // grab again + robotHand->grab(toolNode); auto rotatePose = Eigen::Isometry3d::Identity(); rotatePose.linear() << 1., 0., 0., - 0., 0.7071068, -0.7071068, - 0., 0.7071068, 0.7071068; + 0., 0.50000, -0.866, + 0., 0.8666, 0.5; auto motion = std::make_shared(bn, rotatePose, dart::dynamics::Frame::World(), armSpace, armSkeleton); motion->plan(robot->m_ada); - Eigen::Vector3d delta_x(0., 0., 0.001); + Eigen::Vector3d delta_x(0., 0., 0.005); auto motion2 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 50, + 10, armSpace, armSkeleton); motion2->plan(robot->m_ada); @@ -124,14 +151,34 @@ void PrimitiveActuateNode::execute(std::map> auto rotatePose = Eigen::Isometry3d::Identity(); rotatePose.linear() << 1., 0., 0., - 0., 0.7071055, 0.7071081, - 0., -0.7071081, 0.7071055; + 0., 0.7071055, 0.7071081, + 0., -0.7071081, 0.7071055; auto motion = std::make_shared(bn, - rotatePose, - dart::dynamics::Frame::World(), - armSpace, - armSkeleton); + rotatePose, + dart::dynamics::Frame::World(), + armSpace, + armSkeleton); motion->plan(robot->m_ada); + + // this action is used to release some food from the tool + auto action = m_metaActuateInfo.getAction(); + auto ingredientName = action.get_ingredients()[0]; + auto newLocationName = action.get_locations()[1]; + auto toolName = action.get_tool(); + + auto world = robot->getWorld(); + auto ingredientNode = world->getSkeleton(ingredientName); + auto newLocation = world->getSkeleton(newLocationName); + auto toolNode = world->getSkeleton(toolName); + // ungrab for connect + robotHand->ungrab(); + auto motionunconnect = std::make_shared(ingredientNode, toolNode, toolName, ingredientName, containingMap, false, armSpace, armSkeleton); + motionunconnect->plan(robot->m_ada); + + auto motionConnect = std::make_shared(ingredientNode, newLocation, newLocationName, ingredientName, containingMap, true, armSpace, armSkeleton); + motionConnect->plan(robot->m_ada); + // grab again + robotHand->grab(toolNode); } else if (m_motionType == "close") { auto conf = Eigen::Vector2d(); conf << 1., 1.; @@ -140,44 +187,44 @@ void PrimitiveActuateNode::execute(std::map> motion->plan(robot->m_ada); } else if (m_motionType == "roll") { for (int j = 0; j < 3; j++) { - Eigen::Vector3d delta_x(-0.001, 0., -0.); + Eigen::Vector3d delta_x(-0.003, 0., -0.); auto motion5 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion5->plan(robot->m_ada); - delta_x << +0.001, 0., 0.00; + delta_x << +0.003, 0., 0.00; auto motion6 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion6->plan(robot->m_ada); } } else if (m_motionType == "heat") { for (int j = 0; j < 3; j++) { - Eigen::Vector3d delta_x(-0.001, 0., -0.); + Eigen::Vector3d delta_x(-0.003, 0., -0.); auto motion5 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion5->plan(robot->m_ada); - delta_x << +0.001, 0., 0.00; + delta_x << +0.003, 0., 0.00; auto motion6 = std::make_shared(bn, delta_x, dart::dynamics::Frame::World(), - 30, + 10, armSpace, armSkeleton); motion6->plan(robot->m_ada); diff --git a/src/PrimitiveEngageNode.cpp b/src/PrimitiveEngageNode.cpp index d82f3b2..7b5bdfb 100644 --- a/src/PrimitiveEngageNode.cpp +++ b/src/PrimitiveEngageNode.cpp @@ -11,6 +11,13 @@ void PrimitiveEngageNode::execute(std::map> std::shared_ptr &objMgr, std::shared_ptr &containingMap) { auto agent = agents[m_pid]; + auto theOtherPid = m_pid; + if (theOtherPid == "p1") { + theOtherPid = "p2"; + } else { + theOtherPid = "p1"; + } + auto theOther = agents[theOtherPid]; if (agent->getType() == "human") { waitForUser("Please move object to..."); @@ -18,6 +25,7 @@ void PrimitiveEngageNode::execute(std::map> m_ifExecuted = true; } else if (agent->getType() == "robot") { ROS_INFO("Moving..."); + auto theOtherRobot = std::dynamic_pointer_cast(theOther); auto robot = std::dynamic_pointer_cast(agent); auto robotArm = robot->getArm(); auto robotHand = robot->getHand(); @@ -28,11 +36,16 @@ void PrimitiveEngageNode::execute(std::map> auto world = robot->getWorld(); // setup collisionDetector + auto grabbedBodyNode = theOtherRobot->getHand()->getGrabbedBodyNode(); auto moveBn = objMgr->getObjBodyNode(m_toMove); auto collisionDetector = dart::collision::FCLCollisionDetector::create(); + auto containedBodyNodes = containingMap->getContainedBodyNodes(m_toMove); std::shared_ptr - armCollisionGroup = collisionDetector->createCollisionGroup(armSkeleton.get(), moveBn); - auto envCollisionGroup = objMgr->createCollisionGroupExceptFoodAndMovingObj(m_toMove, collisionDetector); + armCollisionGroup = collisionDetector->createCollisionGroup(armSkeleton.get(), moveBn, handSkeleton.get()); + for (const auto &containedBodyNode : containedBodyNodes) { + armCollisionGroup->addShapeFramesOf(containedBodyNode); + } + auto envCollisionGroup = objMgr->createCollisionGroupExceptFoodAndMovingObj(m_toMove, collisionDetector, nullptr); std::shared_ptr collisionFreeConstraint = std::make_shared(armSpace, armSkeleton, collisionDetector); collisionFreeConstraint->addPairwiseCheck(armCollisionGroup, envCollisionGroup); @@ -48,6 +61,7 @@ void PrimitiveEngageNode::execute(std::map> armSkeleton, nullptr, false); + motion1->setTimeStep(m_timeStep); motion1->plan(robot->m_ada); m_ifExecuted = true; diff --git a/src/PrimitiveGraspNode.cpp b/src/PrimitiveGraspNode.cpp index bf1b9c1..362de69 100644 --- a/src/PrimitiveGraspNode.cpp +++ b/src/PrimitiveGraspNode.cpp @@ -14,6 +14,13 @@ void PrimitiveGraspNode::execute(std::map> & std::shared_ptr &objMgr, std::shared_ptr &containingMap) { auto agent = agents[m_pid]; + auto theOtherPid = m_pid; + if (theOtherPid == "p1") { + theOtherPid = "p2"; + } else { + theOtherPid = "p1"; + } + auto theOther = agents[theOtherPid]; // since every primitive action node only involves one agent if (agent->getType() == "human") { @@ -22,6 +29,7 @@ void PrimitiveGraspNode::execute(std::map> & m_ifExecuted = true; } else if (agent->getType() == "robot") { + auto theOtherRobot = std::dynamic_pointer_cast(theOther); auto robot = std::dynamic_pointer_cast(agent); auto robotArm = robot->getArm(); auto robotHand = robot->getHand(); @@ -33,9 +41,20 @@ void PrimitiveGraspNode::execute(std::map> & m_grabPose->mT0_w = objMgr->getObjTransform(m_toGrab); + // setup collisionDetector + auto grabbedBodyNode = theOtherRobot->getHand()->getGrabbedBodyNode(); + auto collisionDetector = dart::collision::FCLCollisionDetector::create(); + std::shared_ptr + armCollisionGroup = collisionDetector->createCollisionGroup(armSkeleton.get(), handSkeleton.get()); + std::cout << "Hand has " << handSkeleton->getNumBodyNodes() << " body nodes: " << handSkeleton->getBodyNode(0)->getName() << " " << handSkeleton->getBodyNode(1)->getName() << std::endl; + auto envCollisionGroup = objMgr->createCollisionGroupExceptFoodAndMovingObj("hand", collisionDetector, grabbedBodyNode); + std::shared_ptr collisionFreeConstraint = + std::make_shared(armSpace, armSkeleton, collisionDetector); + collisionFreeConstraint->addPairwiseCheck(armCollisionGroup, envCollisionGroup); + auto motion1 = std::make_shared(m_grabPose, robotHand->getEndEffectorBodyNode(), - nullptr, + collisionFreeConstraint, armSpace, armSkeleton, nullptr, diff --git a/src/PrimitivePlaceNode.cpp b/src/PrimitivePlaceNode.cpp index 8ac51c5..ef5ea98 100644 --- a/src/PrimitivePlaceNode.cpp +++ b/src/PrimitivePlaceNode.cpp @@ -11,12 +11,19 @@ void PrimitivePlaceNode::execute(std::map> & std::shared_ptr &objMgr, std::shared_ptr &containingMap) { auto agent = agents[m_pid]; - + auto theOtherPid = m_pid; + if (theOtherPid == "p1") { + theOtherPid = "p2"; + } else { + theOtherPid = "p1"; + } + auto theOther = agents[theOtherPid]; if (agent->getType() == "human") { waitForUser("Please place object " + m_toPlace); m_ifExecuted = true; } else if (agent->getType() == "robot") { + auto theOtherRobot = std::dynamic_pointer_cast(theOther); auto robot = std::dynamic_pointer_cast(agent); auto robotArm = robot->getArm(); auto robotHand = robot->getHand(); @@ -28,11 +35,13 @@ void PrimitivePlaceNode::execute(std::map> & if (robot->getHand()->isGrabbing(m_toPlace) == 0) { m_targetPose->mT0_w = objMgr->getObjTransform(m_refObject); + auto grabbedBodyNode = theOtherRobot->getHand()->getGrabbedBodyNode(); auto placeBn = objMgr->getObjBodyNode(m_toPlace); auto collisionDetector = dart::collision::FCLCollisionDetector::create(); std::shared_ptr - armCollisionGroup = collisionDetector->createCollisionGroup(armSkeleton.get(), placeBn); - auto envCollisionGroup = objMgr->createCollisionGroupExceptFoodAndMovingObj(m_toPlace, collisionDetector); + armCollisionGroup = collisionDetector->createCollisionGroup(armSkeleton.get(), placeBn, handSkeleton.get()); + auto envCollisionGroup = + objMgr->createCollisionGroupExceptFoodAndMovingObj(m_toPlace, collisionDetector, grabbedBodyNode); std::shared_ptr collisionFreeConstraint = std::make_shared(armSpace, armSkeleton, collisionDetector); collisionFreeConstraint->addPairwiseCheck(armCollisionGroup, envCollisionGroup); @@ -43,7 +52,7 @@ void PrimitivePlaceNode::execute(std::map> & armSpace, armSkeleton, nullptr, - false); + m_ifDebug); motion1->plan(robot->m_ada); auto motion2 = std::make_shared(world->getSkeleton(m_toPlace), false, armSpace, armSkeleton); @@ -55,12 +64,12 @@ void PrimitivePlaceNode::execute(std::map> & auto motion3 = std::make_shared(conf, handSpace, handSkeleton); motion3->plan(robot->m_ada); - Eigen::Vector3d delta_x(0., 0., 0.001); + Eigen::Vector3d delta_x(0., 0., 0.005); auto motion4 = std::make_shared(robotHand->getEndEffectorBodyNode(), delta_x, dart::dynamics::Frame::World(), - 50, + 20, armSpace, armSkeleton); motion4->plan(robot->m_ada); diff --git a/src/RelativeIKMotionNode.cpp b/src/RelativeIKMotionNode.cpp index f4569fc..9026361 100644 --- a/src/RelativeIKMotionNode.cpp +++ b/src/RelativeIKMotionNode.cpp @@ -17,10 +17,10 @@ void RelativeIKMotionNode::plan(const std::shared_ptr &ada) { auto targetPose = currentPose * m_relT; auto targetSpatialVector = TransformMatrix2SpatialVector(targetPose); - while ((targetSpatialVector - currentSpatialVector).norm() > 0.45) { + while ((targetSpatialVector - currentSpatialVector).norm() > 0.35) { std::cout << (targetSpatialVector - currentSpatialVector).norm() << std::endl; auto jac = m_skeleton->getJacobian(m_bn, m_incoordinatesOf); - delta_q << aikido::common::pseudoinverse(jac) * (targetSpatialVector - currentSpatialVector) * 0.005; + delta_q << aikido::common::pseudoinverse(jac) * (targetSpatialVector - currentSpatialVector) * 0.03; Eigen::VectorXd currPos = m_skeleton->getPositions(); ros::Duration(0.1).sleep(); Eigen::VectorXd new_pos = currPos + delta_q; diff --git a/src/TSRMotionNode.cpp b/src/TSRMotionNode.cpp index 8fc392d..6da27ca 100644 --- a/src/TSRMotionNode.cpp +++ b/src/TSRMotionNode.cpp @@ -6,7 +6,9 @@ #include #include #include + #include "wecook/TSRMotionNode.h" +#include "wecook/planner/SmootherHelpers.h" using namespace wecook; @@ -78,6 +80,7 @@ void TSRMotionNode::plan(const std::shared_ptr &ada) { ROS_INFO("Return"); int curr_conf = 1; while (not trajectory && curr_conf < configurations.size()) { + std::cout << "Failed " << m_goalTSR->mT0_w.translation() << std::endl; // TODO replan ROS_INFO("Try new goal"); trajectory = ada->planToConfiguration(m_stateSpace, @@ -85,30 +88,40 @@ void TSRMotionNode::plan(const std::shared_ptr &ada) { configurations[curr_conf], m_collisionFree, 1); + m_stateSpace->setState(m_skeleton.get(), startState.getState()); curr_conf++; } if (trajectory) { ROS_INFO("Found the trajectory!"); + auto num_waypoints = dynamic_cast(trajectory.get())->getNumWaypoints(); ROS_INFO_STREAM("[TSRMotionNode::plan]: The trajectory has " - << dynamic_cast(trajectory.get())->getNumWaypoints() + << num_waypoints << " waypoints"); -// std::vector constraints; -// if (m_collisionFree) -// constraints.push_back(m_collisionFree); -// auto testable = std::make_shared(m_stateSpace, constraints); -// std::unique_lock lock(m_skeleton->getBodyNode(0)->getSkeleton()->getMutex()); -// aikido::trajectory::TrajectoryPtr timedTrajectory = ada->smoothPath(m_skeleton, -// trajectory.get(), -// testable); -// m_stateSpace->setState(m_skeleton.get(), startState.getState()); -// lock.unlock(); -// ROS_INFO_STREAM("[TSRMotionNode::plan]: The smoothed trajectory has " -// << dynamic_cast(timedTrajectory.get())->getNumWaypoints() -// << " waypoints"); - aikido::trajectory::TrajectoryPtr timedTrajectory = ada->retimePath(m_skeleton, trajectory.get()); - auto future = ada->executeTrajectory(timedTrajectory); - future.wait(); + std::vector constraints; + if (m_collisionFree) + constraints.push_back(m_collisionFree); + auto testable = std::make_shared(m_stateSpace, constraints); + if (num_waypoints == 2) { + auto future = ada->executeTrajectory(trajectory); + future.wait(); + } else { + std::unique_lock lock(m_skeleton->getBodyNode(0)->getSkeleton()->getMutex()); +// aikido::trajectory::TrajectoryPtr unTimedTrajectory = +// planner::simpleSmoothPath(ada, m_stateSpace, m_skeleton, trajectory.get(), testable); +// aikido::trajectory::TrajectoryPtr timedTrajectory = ada->retimePathWithKunz(m_skeleton, unTimedTrajectory.get(), 1e-2, m_retimeTimeStep); +// m_stateSpace->setState(m_skeleton.get(), startState.getState()); +// aikido::trajectory::TrajectoryPtr unTimedTrajectory = +// planner::hauserSmoothPathInterpolated(ada, m_stateSpace, m_skeleton, trajectory.get(), testable); +// aikido::trajectory::TrajectoryPtr +// timedTrajectory = ada->retimePathWithKunz(m_skeleton, unTimedTrajectory.get(), 1e-3, 5e-3); + aikido::trajectory::TrajectoryPtr timedTrajectory = planner::hauserSmoothPath(ada, m_stateSpace, m_skeleton, trajectory.get(), testable); + m_stateSpace->setState(m_skeleton.get(), startState.getState()); + lock.unlock(); + auto future = ada->executeTrajectory(timedTrajectory); + future.wait(); + } + } else { ROS_INFO("[TSRMotionNode::plan]: Didn't find a valid trajectory!"); } diff --git a/src/TaskExecutorThread.cpp b/src/TaskExecutorThread.cpp index ab52340..51e5602 100644 --- a/src/TaskExecutorThread.cpp +++ b/src/TaskExecutorThread.cpp @@ -13,7 +13,13 @@ void TaskExecutorThread::run() { std::cout << pid << " " << curr->getAction().get_verb() << std::endl; m_currentActionNode = curr; // wait until all agents involved in this action are ready - m_syncCallback(m_currentActionNode); +// m_syncCallback(m_currentActionNode); + auto fatherNodes = curr->getFathers(); + for (auto &father : fatherNodes) { + while (!father->m_ifExecuted) { + ros::Duration(0.5).sleep(); + } + } // execute primitive task graph auto ptg = m_currentActionNode->m_primitiveTaskGraph; // get the first primitive action node to execute @@ -37,7 +43,7 @@ void TaskExecutorThread::run() { } } } - + curr->m_ifExecuted = true; // find next action ActionNode *next = nullptr; for (auto &child : curr->getChildren()) { diff --git a/src/TaskGraph.cpp b/src/TaskGraph.cpp index ace01c4..ab2a9b2 100644 --- a/src/TaskGraph.cpp +++ b/src/TaskGraph.cpp @@ -8,9 +8,16 @@ using namespace wecook; -void TaskGraph::addNode(const wecook::Action &action) { +void TaskGraph::addNode(const wecook::Action &action, bool ifEnd) { auto pids = action.get_pids(); + ActionNode *newFatherNode = nullptr; + + if (ifEnd) { + std::cout << "Is end!" << std::endl; + newFatherNode = m_nodes.back(); + } + // we need to find father node for each pid auto fatherNodes = std::vector{}; for (const auto &pid : pids) { @@ -29,6 +36,10 @@ void TaskGraph::addNode(const wecook::Action &action) { for (const auto &pid : pids) { m_headMap.emplace(std::pair{pid, newNode}); } + if (newFatherNode) { + newFatherNode->addChild(newNode); + newNode->addFather(newFatherNode); + } } else { auto newNode = new ActionNode(action, pids, false); for (auto &fatherNode : fatherNodes) { @@ -42,6 +53,11 @@ void TaskGraph::addNode(const wecook::Action &action) { m_headMap.emplace(std::pair{pid, newNode}); } } + + if (newFatherNode) { + newFatherNode->addChild(newNode); + newNode->addFather(newFatherNode); + } } } diff --git a/src/external/hauser_parabolic_smoother/CMakeLists.txt b/src/external/hauser_parabolic_smoother/CMakeLists.txt new file mode 100644 index 0000000..7e34c19 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/CMakeLists.txt @@ -0,0 +1,15 @@ +add_library("${PROJECT_NAME}_external_hauserparabolicsmoother" STATIC + DynamicPath.cpp + ParabolicRamp.cpp + Timer.cpp +) +target_include_directories("${PROJECT_NAME}_external_hauserparabolicsmoother" + PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}" +) +set_target_properties("${PROJECT_NAME}_external_hauserparabolicsmoother" + PROPERTIES POSITION_INDEPENDENT_CODE TRUE +) +#target_compile_options("${PROJECT_NAME}_external_hauserparabolicsmoother" +# PUBLIC ${AIKIDO_CXX_STANDARD_FLAGS} +# PRIVATE -w +#) diff --git a/src/external/hauser_parabolic_smoother/Config.h b/src/external/hauser_parabolic_smoother/Config.h new file mode 100644 index 0000000..82ffd63 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/Config.h @@ -0,0 +1,49 @@ +#ifndef PARABOLIC_RAMP_CONFIG_H +#define PARABOLIC_RAMP_CONFIG_H + +#include + +///assertion function +#define PARABOLIC_RAMP_ASSERT(x) + +///print an error +#define PARABOLIC_RAMP_PERROR(...) + +///print a notification +#define PARABOLIC_RAMP_PLOG(...) + +namespace ParabolicRamp { + + ///tolerance for time equality + const static Real EpsilonT = 1e-6; + + ///tolerance for position equality + const static Real EpsilonX = 1e-5; + + ///tolerance for velocity equality + const static Real EpsilonV = 1e-5; + + ///tolerance for acceleration equality + const static Real EpsilonA = 1e-6; + + ///self validity check level: + ///- 0 no checking + ///- 1 moderate checking + ///- 2 full checking + const static int gValidityCheckLevel = 2; + + ///verbosity level: + ///- 0 all messages off + ///- 1 brief messages + ///- 2 detailed messages + const static int gVerbose = 2; + + ///whether or not to pause on serious errors + const static bool gErrorGetchar = false; + + ///whether or not errors are logged to disk + const static bool gErrorSave = false; + +} //namespace ParabolicRamp + +#endif diff --git a/src/external/hauser_parabolic_smoother/DynamicPath.cpp b/src/external/hauser_parabolic_smoother/DynamicPath.cpp new file mode 100644 index 0000000..136b97e --- /dev/null +++ b/src/external/hauser_parabolic_smoother/DynamicPath.cpp @@ -0,0 +1,868 @@ +/***************************************************************************** + * + * Copyright (c) 2010-2011, the Trustees of Indiana University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Indiana University nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS'' + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + * + ***************************************************************************/ + +#include "DynamicPath.h" +#include "Timer.h" +#include "Config.h" +#include "HauserMath.h" +#include +#include +#include +#include +#include +using namespace std; + +namespace ParabolicRamp { + +inline Real LInfDistance(const Vector &a, const Vector &b) { + PARABOLIC_RAMP_ASSERT(a.size() == b.size()); + Real d = 0; + for (std::size_t i = 0; i < a.size(); i++) + d = Max(d, Abs(a[i] - b[i])); + return d; +} + +inline bool InBounds(const Vector &x, const Vector &bmin, const Vector &bmax) { + PARABOLIC_RAMP_ASSERT(x.size() == bmin.size()); + PARABOLIC_RAMP_ASSERT(x.size() == bmax.size()); + for (std::size_t i = 0; i < x.size(); i++) + if (x[i] < bmin[i] || x[i] > bmax[i]) return false; + return true; +} + +inline Real MaxBBLInfDistance(const Vector &x, const Vector &bmin, const Vector &bmax) { + PARABOLIC_RAMP_ASSERT(x.size() == bmin.size()); + PARABOLIC_RAMP_ASSERT(x.size() == bmax.size()); + Real d = 0; + for (std::size_t i = 0; i < x.size(); i++) + d = Max(d, Max(Abs(x[i] - bmin[i]), Abs(x[i] - bmax[i]))); + return d; +} + +inline bool SolveMinTime(const Vector &x0, + const Vector &dx0, + const Vector &x1, + const Vector &dx1, + const Vector &accMax, + const Vector &velMax, + const Vector &xMin, + const Vector &xMax, + DynamicPath &out) { + if (xMin.empty()) { + out.ramps.resize(1); + ParabolicRampND &temp = out.ramps[0]; + temp.x0 = x0; + temp.x1 = x1; + temp.dx0 = dx0; + temp.dx1 = dx1; + bool res = temp.SolveMinTime(accMax, velMax); + if (!res) return false; + } else { + vector > ramps; + Real res = SolveMinTimeBounded(x0, dx0, x1, dx1, + accMax, velMax, xMin, xMax, + ramps); + if (res < 0) return false; + CombineRamps(ramps, out.ramps); + } + PARABOLIC_RAMP_ASSERT(out.IsValid()); + return true; +} + +DynamicPath::DynamicPath() {} + +void DynamicPath::Init(const Vector &_velMax, const Vector &_accMax) { + velMax = _velMax; + accMax = _accMax; + PARABOLIC_RAMP_ASSERT(velMax.size() == accMax.size()); + if (!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() == velMax.size()); +} + +void DynamicPath::SetJointLimits(const Vector &_xMin, const Vector &_xMax) { + xMin = _xMin; + xMax = _xMax; + PARABOLIC_RAMP_ASSERT(xMin.size() == xMax.size()); + if (!velMax.empty() && !xMin.empty()) PARABOLIC_RAMP_ASSERT(xMin.size() == velMax.size()); +} + +Real DynamicPath::GetTotalTime() const { + Real t = 0; + for (std::size_t i = 0; i < ramps.size(); i++) t += ramps[i].endTime; + return t; +} + +int DynamicPath::GetSegment(Real t, Real &u, bool &outOfBounds) const { + if (t < 0) { + u = 0; + outOfBounds = true; + return 0; + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (t <= ramps[i].endTime) { + u = t; + outOfBounds = false; + return (int) i; + } + t -= ramps[i].endTime; + } + u = t; + outOfBounds = true; + return (int) ramps.size() - 1; +} + +void DynamicPath::Evaluate(Real t, Vector &x) const { + PARABOLIC_RAMP_ASSERT(!ramps.empty()); + if (t < 0) { + x = ramps.front().x0; + } else { + for (std::size_t i = 0; i < ramps.size(); i++) { + if (t <= ramps[i].endTime) { + ramps[i].Evaluate(t, x); + return; + } + t -= ramps[i].endTime; + } + x = ramps.back().x1; + } +} + +void DynamicPath::Derivative(Real t, Vector &dx) const { + PARABOLIC_RAMP_ASSERT(!ramps.empty()); + if (t < 0) { + dx = ramps.front().dx0; + } else { + for (std::size_t i = 0; i < ramps.size(); i++) { + if (t <= ramps[i].endTime) { + ramps[i].Derivative(t, dx); + return; + } + t -= ramps[i].endTime; + } + dx = ramps.back().dx1; + } +} + +void DynamicPath::SetMilestones(const vector &x) { + if (x.empty()) { + ramps.resize(0); + } else if (x.size() == 1) { + ramps.resize(1); + ramps[0].SetConstant(x[0]); + } else { + Vector zero(x[0].size(), 0.0); + ramps.resize(x.size() - 1); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x[i]; + ramps[i].x1 = x[i + 1]; + ramps[i].dx0 = zero; + ramps[i].dx1 = zero; + bool res = ramps[i].SolveMinTimeLinear(accMax, velMax); + PARABOLIC_RAMP_ASSERT(res); + } + } +} + +void DynamicPath::SetMilestones(const vector &x, const vector &dx) { + if (x.empty()) { + ramps.resize(0); + } else if (x.size() == 1) { + ramps.resize(1); + ramps[0].SetConstant(x[0]); + } else { + if (xMin.empty()) { + ramps.resize(x.size() - 1); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x[i]; + ramps[i].x1 = x[i + 1]; + ramps[i].dx0 = dx[i]; + ramps[i].dx1 = dx[i + 1]; + bool res = ramps[i].SolveMinTime(accMax, velMax); + PARABOLIC_RAMP_ASSERT(res); + } + } else { + //bounded solving + ramps.resize(0); + ramps.reserve(x.size() - 1); + std::vector > tempRamps; + std::vector tempRamps2; + PARABOLIC_RAMP_ASSERT(InBounds(x[0], xMin, xMax)); + for (std::size_t i = 0; i + 1 < x.size(); i++) { + PARABOLIC_RAMP_ASSERT(InBounds(x[i + 1], xMin, xMax)); + Real res = SolveMinTimeBounded(x[i], dx[i], x[i + 1], dx[i + 1], + accMax, velMax, xMin, xMax, + tempRamps); + PARABOLIC_RAMP_ASSERT(res >= 0); + CombineRamps(tempRamps, tempRamps2); + ramps.insert(ramps.end(), tempRamps2.begin(), tempRamps2.end()); + } + } + } +} + +void DynamicPath::GetMilestones(vector &x, vector &dx) const { + if (ramps.empty()) { + x.resize(0); + dx.resize(0); + return; + } + x.resize(ramps.size() + 1); + dx.resize(ramps.size() + 1); + x[0] = ramps[0].x0; + dx[0] = ramps[0].dx0; + for (std::size_t i = 0; i < ramps.size(); i++) { + x[i + 1] = ramps[i].x1; + dx[i + 1] = ramps[i].dx1; + } +} + +void DynamicPath::Append(const Vector &x) { + std::size_t n = ramps.size(); + std::size_t p = n - 1; + if (ramps.size() == 0) { + ramps.resize(1); + ramps[0].SetConstant(x); + } else { + if (xMin.empty()) { + ramps.resize(ramps.size() + 1); + ramps[n].x0 = ramps[p].x1; + ramps[n].dx0 = ramps[p].dx1; + ramps[n].x1 = x; + ramps[n].dx1.resize(x.size()); + fill(ramps[n].dx1.begin(), ramps[n].dx1.end(), 0); + bool res = ramps[n].SolveMinTime(accMax, velMax); + PARABOLIC_RAMP_ASSERT(res); + } else { + PARABOLIC_RAMP_ASSERT(InBounds(x, xMin, xMax)); + std::vector > tempRamps; + std::vector tempRamps2; + Vector zero(x.size(), 0.0); + Real res = SolveMinTimeBounded(ramps[p].x1, ramps[p].dx1, x, zero, + accMax, velMax, xMin, xMax, tempRamps); + PARABOLIC_RAMP_ASSERT(res >= 0); + CombineRamps(tempRamps, tempRamps2); + ramps.insert(ramps.end(), tempRamps2.begin(), tempRamps2.end()); + } + } +} + +void DynamicPath::Append(const Vector &x, const Vector &dx) { + std::size_t n = ramps.size(); + std::size_t p = n - 1; + PARABOLIC_RAMP_ASSERT(ramps.size() != 0); + if (xMin.empty()) { + ramps.resize(ramps.size() + 1); + ramps[n].x0 = ramps[p].x1; + ramps[n].dx0 = ramps[p].dx1; + ramps[n].x1 = x; + ramps[n].dx1 = dx; + bool res = ramps[n].SolveMinTime(accMax, velMax); + PARABOLIC_RAMP_ASSERT(res); + } else { + PARABOLIC_RAMP_ASSERT(InBounds(x, xMin, xMax)); + std::vector > tempRamps; + std::vector tempRamps2; + Real res = SolveMinTimeBounded(ramps[p].x1, ramps[p].dx1, x, dx, + accMax, velMax, xMin, xMax, tempRamps); + PARABOLIC_RAMP_ASSERT(res >= 0); + CombineRamps(tempRamps, tempRamps2); + ramps.insert(ramps.end(), tempRamps2.begin(), tempRamps2.end()); + } +} + +void DynamicPath::Concat(const DynamicPath &suffix) { + PARABOLIC_RAMP_ASSERT(&suffix != this); + if (suffix.ramps.empty()) return; + if (ramps.empty()) { + *this = suffix; + return; + } + //double check continuity + if (ramps.back().x1 != suffix.ramps.front().x0 || + ramps.back().dx1 != suffix.ramps.front().dx0) { + Real xmax = 0, dxmax = 0; + for (std::size_t i = 0; i < ramps.back().x1.size(); i++) { + xmax = Max(xmax, Abs(ramps.back().x1[i] - suffix.ramps.front().x0[i])); + dxmax = Max(dxmax, Abs(ramps.back().dx1[i] - suffix.ramps.front().dx0[i])); + } + if (Abs(xmax) > EpsilonX || Abs(dxmax) > EpsilonV) { + printf("Concat endpoint error\n"); + printf("x:\n"); + for (std::size_t i = 0; i < ramps.back().x1.size(); i++) + printf("%g - %g = %g\n", + ramps.back().x1[i], + suffix.ramps.front().x0[i], + ramps.back().x1[i] - suffix.ramps.front().x0[i]); + printf("dx:\n"); + for (std::size_t i = 0; i < ramps.back().x1.size(); i++) + printf("%g - %g = %g\n", + ramps.back().dx1[i], + suffix.ramps.front().dx0[i], + ramps.back().dx1[i] - suffix.ramps.front().dx0[i]); + getchar(); + } + ramps.back().x1 = ramps.front().x0; + ramps.back().dx1 = ramps.front().dx0; + for (std::size_t i = 0; i < ramps.back().x1.size(); i++) { + ramps.back().ramps[i].x1 = suffix.ramps.front().x0[i]; + ramps.back().ramps[i].dx1 = suffix.ramps.front().dx0[i]; + } + } + PARABOLIC_RAMP_ASSERT(ramps.back().x1 == suffix.ramps.front().x0); + PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == suffix.ramps.front().dx0); + ramps.insert(ramps.end(), suffix.ramps.begin(), suffix.ramps.end()); +} + +void DynamicPath::Split(Real t, DynamicPath &before, DynamicPath &after) const { + PARABOLIC_RAMP_ASSERT(IsValid()); + PARABOLIC_RAMP_ASSERT(&before != this); + PARABOLIC_RAMP_ASSERT(&after != this); + if (ramps.empty()) { + before = *this; + after = *this; + return; + } + after.velMax = before.velMax = velMax; + after.accMax = before.accMax = accMax; + after.xMin = before.xMin = xMin; + after.xMax = before.xMax = xMax; + after.ramps.resize(0); + before.ramps.resize(0); + if (t < 0) { //we're before the path starts + before.ramps.resize(1); + before.ramps[0].SetConstant(ramps[0].x0); + //place a constant for time -t on the after path + after.ramps.resize(1); + after.ramps[0].SetConstant(ramps[0].x0, -t); + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (t < 0) { + after.ramps.push_back(ramps[i]); + } else { + if (t < ramps[i].endTime) { + //cut current path + ParabolicRampND temp = ramps[i]; + temp.TrimBack(temp.endTime - t); + before.ramps.push_back(temp); + temp = ramps[i]; + temp.TrimFront(t); + if (!after.ramps.empty()) { + printf("DynamicPath::Split: Uh... weird, after is not empty?\n"); + printf("t = %g, i = %zu, endtime = %g\n", t, i, ramps[i].endTime); + } + PARABOLIC_RAMP_ASSERT(after.ramps.size() == 0); + after.ramps.push_back(temp); + } else { + before.ramps.push_back(ramps[i]); + } + t -= ramps[i].endTime; + } + } + + if (t > 0) { //dt is longer than path + ParabolicRampND temp; + temp.SetConstant(ramps.back().x1, t); + before.ramps.push_back(temp); + } + if (t >= 0) { + ParabolicRampND temp; + temp.SetConstant(ramps.back().x1); + after.ramps.push_back(temp); + } + PARABOLIC_RAMP_ASSERT(before.IsValid()); + PARABOLIC_RAMP_ASSERT(after.IsValid()); +} + +struct RampSection { + Real ta, tb; + Vector xa, xb; + Real da, db; +}; + +bool CheckRamp(const ParabolicRampND &ramp, FeasibilityCheckerBase *feas, DistanceCheckerBase *distance, int maxiters) { + if (!feas->ConfigFeasible(ramp.x0)) return false; + if (!feas->ConfigFeasible(ramp.x1)) return false; + PARABOLIC_RAMP_ASSERT(distance->ObstacleDistanceNorm() == Inf); + RampSection section; + section.ta = 0; + section.tb = ramp.endTime; + section.xa = ramp.x0; + section.xb = ramp.x1; + section.da = distance->ObstacleDistance(ramp.x0); + section.db = distance->ObstacleDistance(ramp.x1); + if (section.da <= 0.0) return false; + if (section.db <= 0.0) return false; + list queue; + queue.push_back(section); + int iters = 0; + while (!queue.empty()) { + section = queue.front(); + queue.erase(queue.begin()); + + //check the bounds around this section + if (LInfDistance(section.xa, section.xb) <= section.da + section.db) { + Vector bmin, bmax; + ramp.Bounds(section.ta, section.tb, bmin, bmax); + if (MaxBBLInfDistance(section.xa, bmin, bmax) < section.da && + MaxBBLInfDistance(section.xb, bmin, bmax) < section.db) + //can cull out the section + continue; + } + Real tc = (section.ta + section.tb) * 0.5; + Vector xc; + ramp.Evaluate(tc, xc); + if (!feas->ConfigFeasible(xc)) return false; //infeasible config + //subdivide + Real dc = distance->ObstacleDistance(xc); + RampSection sa, sb; + sa.ta = section.ta; + sa.xa = section.xa; + sa.da = section.da; + sa.tb = sb.ta = tc; + sa.xb = sb.xa = xc; + sa.db = sb.da = dc; + sb.tb = section.tb; + sb.xb = section.xb; + sb.db = section.db; + + //recurse on segments + queue.push_back(sa); + queue.push_back(sb); + + if (iters++ >= maxiters) return false; + } + return true; +} + +bool CheckRamp(const ParabolicRampND &ramp, FeasibilityCheckerBase *space, Real tol) { + PARABOLIC_RAMP_ASSERT(tol > 0); + if (!space->ConfigFeasible(ramp.x0)) return false; + if (!space->ConfigFeasible(ramp.x1)) return false; + //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x0)); + //PARABOLIC_RAMP_ASSERT(space->ConfigFeasible(ramp.x1)); + + //for a parabola of form f(x) = a x^2 + b x, and the straight line + //of form g(X,u) = u*f(X) + //d^2(g(X,u),p) = |p - / f(X)|^2 < tol^2 + // - ^2/ = p^T (I-f(X)f(X)^T/f(X)^T f(X)) p + //max_x d^2(f(x)) => f(x)^T (I-f(X)f(X)^T/f(X)^T f(X)) f'(x) = 0 + //(x^2 a^T + x b^T) A (2a x + b) = 0 + //(x a^T + b^T) A (2a x + b) = 0 + //2 x^2 a^T A a + 3 x b^T A a + b^T A b = 0 + + //the max X for which f(x) deviates from g(X,x) by at most tol is... + //max_x |g(X,x)-f(x)| = max_x x/X f(X)-f(x) + //=> f(X)/X - f'(x) = 0 + //=> X/2 = x + //=> max_x |g(X,x)-f(x)| = |(X/2)/X f(X)-f(X/2)| + //= |1/2 (aX^2+bX) - a(X/2)^2 - b(X/2) + c | + //= |a| X^2 / 4 + //so... max X st max_x |g(X,x)-f(x)| < tol => X = 2*sqrt(tol/|a|) + vector divs; + Real t = 0; + divs.push_back(t); + while (t < ramp.endTime) { + Real tnext = t; + Real amax = 0; + Real switchNext = ramp.endTime; + for (std::size_t i = 0; i < ramp.ramps.size(); i++) { + if (t < ramp.ramps[i].tswitch1) { //ramp up + switchNext = Min(switchNext, ramp.ramps[i].tswitch1); + amax = Max(amax, Max(Abs(ramp.ramps[i].a1), Abs(ramp.ramps[i].a2))); + } else if (t < ramp.ramps[i].tswitch2) { //constant vel + switchNext = Min(switchNext, ramp.ramps[i].tswitch2); + } else if (t < ramp.ramps[i].ttotal) { //ramp down + amax = Max(amax, Max(Abs(ramp.ramps[i].a1), Abs(ramp.ramps[i].a2))); + } + } + Real dt = 2.0 * Sqrt(tol / amax); + if (t + dt > switchNext) tnext = switchNext; + else tnext = t + dt; + + t = tnext; + divs.push_back(tnext); + } + divs.push_back(ramp.endTime); + + //do a bisection thingie + list > segs; + segs.push_back(pair(0, divs.size() - 1)); + Vector q1, q2; + while (!segs.empty()) { + int i = segs.front().first; + int j = segs.front().second; + segs.erase(segs.begin()); + if (j == i + 1) { + //check path from t to tnext + ramp.Evaluate(divs[i], q1); + ramp.Evaluate(divs[j], q2); + if (!space->SegmentFeasible(q1, q2)) return false; + } else { + int k = (i + j) / 2; + ramp.Evaluate(divs[k], q1); + if (!space->ConfigFeasible(q1)) return false; + segs.push_back(pair(i, k)); + segs.push_back(pair(k, j)); + } + } + return true; +} + +RampFeasibilityChecker::RampFeasibilityChecker(FeasibilityCheckerBase *_feas, Real _tol) + : feas(_feas), tol(_tol), distance(NULL), maxiters(0) {} + +RampFeasibilityChecker::RampFeasibilityChecker(FeasibilityCheckerBase *_feas, + DistanceCheckerBase *_distance, + int _maxiters) + : feas(_feas), tol(0), distance(_distance), maxiters(_maxiters) {} + +bool RampFeasibilityChecker::Check(const ParabolicRampND &x) { + if (distance) return CheckRamp(x, feas, distance, maxiters); + else return CheckRamp(x, feas, tol); +} + +bool DynamicPath::TryShortcut(Real t1, Real t2, RampFeasibilityChecker &check) { + if (t1 > t2) Swap(t1, t2); + Real u1, u2; + bool delete_i1, delete_i2; + int i1 = GetSegment(t1, u1, delete_i1); + int i2 = GetSegment(t2, u2, delete_i2); + if (i1 == i2) return false; + PARABOLIC_RAMP_ASSERT(u1 >= 0); + PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime + EpsilonT); + PARABOLIC_RAMP_ASSERT(u2 >= 0); + PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime + EpsilonT); + u1 = Min(u1, ramps[i1].endTime); + u2 = Min(u2, ramps[i2].endTime); + DynamicPath intermediate; + if (xMin.empty()) { + intermediate.ramps.resize(1); + ParabolicRampND &test = intermediate.ramps[0]; + ramps[i1].Evaluate(u1, test.x0); + ramps[i2].Evaluate(u2, test.x1); + ramps[i1].Derivative(u1, test.dx0); + ramps[i2].Derivative(u2, test.dx1); + bool res = test.SolveMinTime(accMax, velMax); + if (!res) return false; + PARABOLIC_RAMP_ASSERT(test.endTime >= 0); + PARABOLIC_RAMP_ASSERT(test.IsValid()); + } else { + Vector x0, x1, dx0, dx1; + ramps[i1].Evaluate(u1, x0); + ramps[i2].Evaluate(u2, x1); + ramps[i1].Derivative(u1, dx0); + ramps[i2].Derivative(u2, dx1); + vector > intramps; + Real res = SolveMinTimeBounded(x0, dx0, x1, dx1, + accMax, velMax, xMin, xMax, + intramps); + if (res < 0) return false; + CombineRamps(intramps, intermediate.ramps); + intermediate.accMax = accMax; + intermediate.velMax = velMax; + PARABOLIC_RAMP_ASSERT(intermediate.IsValid()); + } + for (std::size_t i = 0; i < intermediate.ramps.size(); i++) + if (!check.Check(intermediate.ramps[i])) return false; + + //perform shortcut + //crop i1 and i2 + ramps[i1].TrimBack(ramps[i1].endTime - u1); + ramps[i1].x1 = intermediate.ramps.front().x0; + ramps[i1].dx1 = intermediate.ramps.front().dx0; + ramps[i2].TrimFront(u2); + ramps[i2].x0 = intermediate.ramps.back().x1; + ramps[i2].dx0 = intermediate.ramps.back().dx1; + + // The end of ramp i1 is a new waypoint which is part of the shortcutted + // trajectory. We don't need to blend this in future iterations. + ramps[i1].blendAttempts = -1; + + // Remove the last waypoint. We do this before we change the length of the + // trajectory with the shortcut, so i2 is still a valid index. + if (delete_i2) { + ramps.erase(ramps.begin() + i2); + } + + //replace intermediate ramps with test + for (int i = 0; i < i2 - i1 - 1; i++) + ramps.erase(ramps.begin() + i1 + 1); + ramps.insert(ramps.begin() + i1 + 1, intermediate.ramps.begin(), intermediate.ramps.end()); + + // Remove the first waypoint. We do this after we change the length of the + // trajectory so we don't affect where the shortcut is inserted. + if (delete_i1) { + ramps.erase(ramps.begin() + i1); + } + + //check for consistency + for (std::size_t i = 0; i + 1 < ramps.size(); i++) { + PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i + 1].x0); + PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i + 1].dx0); + } + return true; +} + +int DynamicPath::Shortcut(int numIters, RampFeasibilityChecker &check) { + RandomNumberGeneratorBase rng; + return Shortcut(numIters, check, &rng); +} + +int DynamicPath::Shortcut(int numIters, RampFeasibilityChecker &check, RandomNumberGeneratorBase *rng) { + int shortcuts = 0; + vector rampStartTime(ramps.size()); + Real endTime = 0; + for (std::size_t i = 0; i < ramps.size(); i++) { + rampStartTime[i] = endTime; + endTime += ramps[i].endTime; + } + Vector x0, x1, dx0, dx1; + DynamicPath intermediate; + for (int iters = 0; iters < numIters; iters++) { + Real t1 = rng->Rand() * endTime, t2 = rng->Rand() * endTime; + if (t1 > t2) Swap(t1, t2); + int i1 = std::upper_bound(rampStartTime.begin(), rampStartTime.end(), t1) - rampStartTime.begin() - 1; + int i2 = std::upper_bound(rampStartTime.begin(), rampStartTime.end(), t2) - rampStartTime.begin() - 1; + if (i1 == i2) continue; //same ramp + Real u1 = t1 - rampStartTime[i1]; + Real u2 = t2 - rampStartTime[i2]; + PARABOLIC_RAMP_ASSERT(u1 >= 0); + PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime + EpsilonT); + PARABOLIC_RAMP_ASSERT(u2 >= 0); + PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime + EpsilonT); + u1 = Min(u1, ramps[i1].endTime); + u2 = Min(u2, ramps[i2].endTime); + ramps[i1].Evaluate(u1, x0); + ramps[i2].Evaluate(u2, x1); + ramps[i1].Derivative(u1, dx0); + ramps[i2].Derivative(u2, dx1); + bool res = SolveMinTime(x0, dx0, x1, dx1, accMax, velMax, xMin, xMax, intermediate); + if (!res) continue; + bool feas = true; + for (std::size_t i = 0; i < intermediate.ramps.size(); i++) + if (!check.Check(intermediate.ramps[i])) { + feas = false; + break; + } + if (!feas) continue; + //perform shortcut + shortcuts++; + ramps[i1].TrimBack(ramps[i1].endTime - u1); + ramps[i1].x1 = intermediate.ramps.front().x0; + ramps[i1].dx1 = intermediate.ramps.front().dx0; + ramps[i2].TrimFront(u2); + ramps[i2].x0 = intermediate.ramps.back().x1; + ramps[i2].dx0 = intermediate.ramps.back().dx1; + + //replace intermediate ramps + for (int i = 0; i < i2 - i1 - 1; i++) + ramps.erase(ramps.begin() + i1 + 1); + ramps.insert(ramps.begin() + i1 + 1, intermediate.ramps.begin(), intermediate.ramps.end()); + + //check for consistency + for (std::size_t i = 0; i + 1 < ramps.size(); i++) { + PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i + 1].x0); + PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i + 1].dx0); + } + + //revise the timing + rampStartTime.resize(ramps.size()); + endTime = 0; + for (std::size_t i = 0; i < ramps.size(); i++) { + rampStartTime[i] = endTime; + endTime += ramps[i].endTime; + } + } + return shortcuts; +} + +int DynamicPath::ShortCircuit(RampFeasibilityChecker &check) { + int shortcuts = 0; + DynamicPath intermediate; + for (int i = 0; i + 1 < (int) ramps.size(); i++) { + bool res = + SolveMinTime(ramps[i].x0, ramps[i].dx0, ramps[i].x1, ramps[i].dx1, accMax, velMax, xMin, xMax, intermediate); + if (!res) continue; + bool feas = true; + for (std::size_t j = 0; j < intermediate.ramps.size(); j++) + if (!check.Check(intermediate.ramps[j])) { + feas = false; + break; + } + if (!feas) continue; + + ramps.erase(ramps.begin() + i + 1); + ramps.insert(ramps.begin() + i + 1, intermediate.ramps.begin(), intermediate.ramps.end()); + i += (int) intermediate.ramps.size() - 2; + shortcuts++; + } + return shortcuts; +} + +int DynamicPath::OnlineShortcut(Real leadTime, Real padTime, RampFeasibilityChecker &check) { + RandomNumberGeneratorBase rng; + return OnlineShortcut(leadTime, padTime, check, &rng); +} + +int DynamicPath::OnlineShortcut(Real leadTime, + Real padTime, + RampFeasibilityChecker &check, + RandomNumberGeneratorBase *rng) { + Timer timer; + int shortcuts = 0; + vector rampStartTime(ramps.size()); + Real endTime = 0; + for (std::size_t i = 0; i < ramps.size(); i++) { + rampStartTime[i] = endTime; + endTime += ramps[i].endTime; + } + Vector x0, x1, dx0, dx1; + DynamicPath intermediate; + while (1) { + //can only start from here + Real starttime = timer.ElapsedTime() - leadTime; + if (starttime + padTime >= endTime) break; + starttime = Max(0.0, starttime + padTime); + + Real t1 = starttime + Sqr(rng->Rand()) * (endTime - starttime), + t2 = starttime + rng->Rand() * (endTime - starttime); + if (t1 > t2) Swap(t1, t2); + int i1 = std::upper_bound(rampStartTime.begin(), rampStartTime.end(), t1) - rampStartTime.begin() - 1; + int i2 = std::upper_bound(rampStartTime.begin(), rampStartTime.end(), t2) - rampStartTime.begin() - 1; + if (i1 == i2) continue; //same ramp + Real u1 = t1 - rampStartTime[i1]; + Real u2 = t2 - rampStartTime[i2]; + PARABOLIC_RAMP_ASSERT(u1 >= 0); + PARABOLIC_RAMP_ASSERT(u1 <= ramps[i1].endTime + EpsilonT); + PARABOLIC_RAMP_ASSERT(u2 >= 0); + PARABOLIC_RAMP_ASSERT(u2 <= ramps[i2].endTime + EpsilonT); + u1 = Min(u1, ramps[i1].endTime); + u2 = Min(u2, ramps[i2].endTime); + ramps[i1].Evaluate(u1, x0); + ramps[i2].Evaluate(u2, x1); + ramps[i1].Derivative(u1, dx0); + ramps[i2].Derivative(u2, dx1); + bool res = SolveMinTime(x0, dx0, x1, dx1, accMax, velMax, xMin, xMax, intermediate); + if (!res) continue; + bool feas = true; + for (std::size_t i = 0; i < intermediate.ramps.size(); i++) + if (!check.Check(intermediate.ramps[i])) { + feas = false; + break; + } + if (!feas) continue; + //check for time elapse, otherwise can't perform this shortcut + if (timer.ElapsedTime() - leadTime > t1) continue; + + shortcuts++; + //crop i1 and i2 + ramps[i1].TrimBack(ramps[i1].endTime - u1); + ramps[i1].x1 = intermediate.ramps.front().x0; + ramps[i1].dx1 = intermediate.ramps.front().dx0; + ramps[i2].TrimFront(u2); + ramps[i2].x0 = intermediate.ramps.back().x1; + ramps[i2].dx0 = intermediate.ramps.back().dx1; + PARABOLIC_RAMP_ASSERT(ramps[i1].IsValid()); + PARABOLIC_RAMP_ASSERT(ramps[i2].IsValid()); + + //replace intermediate ramps + for (int i = 0; i < i2 - i1 - 1; i++) + ramps.erase(ramps.begin() + i1 + 1); + ramps.insert(ramps.begin() + i1 + 1, intermediate.ramps.begin(), intermediate.ramps.end()); + + //check for consistency + for (std::size_t i = 0; i + 1 < ramps.size(); i++) { + PARABOLIC_RAMP_ASSERT(ramps[i].x1 == ramps[i + 1].x0); + PARABOLIC_RAMP_ASSERT(ramps[i].dx1 == ramps[i + 1].dx0); + } + + //revise the timing + rampStartTime.resize(ramps.size()); + endTime = 0; + for (std::size_t i = 0; i < ramps.size(); i++) { + rampStartTime[i] = endTime; + endTime += ramps[i].endTime; + } + } + return shortcuts; +} + +bool DynamicPath::IsValid() const { + if (ramps.empty()) { + fprintf(stderr, "DynamicPath::IsValid: empty path\n"); + return false; + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (!ramps[i].IsValid()) { + fprintf(stderr, "DynamicPath::IsValid: ramp %zu is invalid\n", i); + return false; + } + for (std::size_t j = 0; j < ramps[i].ramps.size(); j++) { + if (Abs(ramps[i].ramps[j].a1) > accMax[j] + EpsilonA || + Abs(ramps[i].ramps[j].v) > velMax[j] || + Abs(ramps[i].ramps[j].a2) > accMax[j] + EpsilonA) { + fprintf(stderr, "DynamicPath::IsValid: invalid acceleration or velocity on ramp %zu\n", i); + fprintf(stderr, + "\ta1 %g, v %g, a2 %g. amax %g, vmax %g\n", + ramps[i].ramps[j].a1, + ramps[i].ramps[j].v, + ramps[i].ramps[j].a2, + accMax[j], + velMax[j]); + return false; + } + } + } + for (std::size_t i = 1; i < ramps.size(); i++) { + if (ramps[i].x0 != ramps[i - 1].x1) { + fprintf(stderr, "DynamicPath::IsValid: discontinuity at ramp %zu\n", i); + for (std::size_t j = 0; j < ramps[i].x0.size(); j++) + fprintf(stderr, "%g ", ramps[i].x0[j]); + fprintf(stderr, "\n"); + for (std::size_t j = 0; j < ramps[i - 1].x1.size(); j++) + fprintf(stderr, "%g ", ramps[i - 1].x1[j]); + fprintf(stderr, "\n"); + return false; + } + if (ramps[i].dx0 != ramps[i - 1].dx1) { + fprintf(stderr, "DynamicPath::IsValid: derivative discontinuity at ramp %zu\n", i); + for (std::size_t j = 0; j < ramps[i].dx0.size(); j++) + fprintf(stderr, "%g ", ramps[i].dx0[j]); + fprintf(stderr, "\n"); + for (std::size_t j = 0; j < ramps[i - 1].dx1.size(); j++) + fprintf(stderr, "%g ", ramps[i - 1].dx1[j]); + fprintf(stderr, "\n"); + + return false; + } + } + return true; +} + +} //namespace ParabolicRamp diff --git a/src/external/hauser_parabolic_smoother/DynamicPath.h b/src/external/hauser_parabolic_smoother/DynamicPath.h new file mode 100644 index 0000000..1058b32 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/DynamicPath.h @@ -0,0 +1,151 @@ +/***************************************************************************** + * + * Copyright (c) 2010-2011, the Trustees of Indiana University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Indiana University nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS'' + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + * + ***************************************************************************/ + +#ifndef DYNAMIC_PATH_H +#define DYNAMIC_PATH_H + +#include "ParabolicRamp.h" + +namespace ParabolicRamp { + +/** @brief A base class for a feasibility checker. + */ +class FeasibilityCheckerBase +{ + public: + virtual ~FeasibilityCheckerBase() {} + virtual bool ConfigFeasible(const Vector& x)=0; + virtual bool SegmentFeasible(const Vector& a,const Vector& b)=0; +}; + +/** @brief A base class for a distance checker. + * ObstacleDistance returns the radius of a L-z norm guaranteed to + * be collision-free. ObstacleDistanceNorm returns the value of z. + * + * DynamicPath can currently only handle L-Inf norms. + */ +class DistanceCheckerBase +{ + public: + virtual ~DistanceCheckerBase() {} + virtual Real ObstacleDistanceNorm() const { return Inf; } + virtual Real ObstacleDistance(const Vector& x)=0; +}; + +/// Checks whether the ramp is feasible using exact checking +bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters); + +/// Checks whether the ramp is feasible using a piecewise linear approximation +/// with tolerance tol +bool CheckRamp(const ParabolicRampND& ramp,FeasibilityCheckerBase* space,Real tol); + + +/** @brief A class that encapsulates feaibility checking of a + * ParabolicRampND. + * + * If given a feasibility checker and a tolerance tol, this performs + * a piecewise linear discretization that deviates no more than tol + * from the parabolic ramp along any axis, and then checks for + * configuration and segment feasibility along that piecewise linear path. + * + * If given a feasibility checker and a distance checker, this performs + * an exact recursive bisection. + */ +class RampFeasibilityChecker +{ + public: + RampFeasibilityChecker(FeasibilityCheckerBase* feas,Real tol); + RampFeasibilityChecker(FeasibilityCheckerBase* feas,DistanceCheckerBase* distance,int maxiters); + bool Check(const ParabolicRampND& x); + + FeasibilityCheckerBase* feas; + Real tol; + DistanceCheckerBase* distance; + int maxiters; +}; + +/** @brief A custom random number generator that can be provided to + * DynamicPath::Shortcut() + */ +class RandomNumberGeneratorBase +{ + public: + virtual Real Rand() { return ::ParabolicRamp::Rand(); } +}; + + +/** @brief A bounded-velocity, bounded-acceleration trajectory consisting + * of parabolic ramps. + * + * Optionally, joint limits xMin and xMax may be specified as well. If so, + * then the XXXBounded functions are used for smoothing. + * + * The Shortcut and OnlineShortcut methods can optionally take a + * custom random number generator (may be useful for multithreading). + */ +class DynamicPath +{ + public: + DynamicPath(); + void Init(const Vector& velMax,const Vector& accMax); + void SetJointLimits(const Vector& qMin,const Vector& qMax); + inline void Clear() { ramps.clear(); } + inline bool Empty() const { return ramps.empty(); } + Real GetTotalTime() const; + int GetSegment(Real t,Real& u,bool& outOfBounds) const; + void Evaluate(Real t,Vector& x) const; + void Derivative(Real t,Vector& dx) const; + void SetMilestones(const std::vector& x); + void SetMilestones(const std::vector& x,const std::vector& dx); + void GetMilestones(std::vector& x,std::vector& dx) const; + void Append(const Vector& x); + void Append(const Vector& x,const Vector& dx); + void Concat(const DynamicPath& suffix); + void Split(Real t,DynamicPath& before,DynamicPath& after) const; + bool TryShortcut(Real t1,Real t2,RampFeasibilityChecker& check); + int Shortcut(int numIters,RampFeasibilityChecker& check); + int Shortcut(int numIters,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng); + int ShortCircuit(RampFeasibilityChecker& check); + /// leadTime: the amount of time before this path should be executable + /// padTime: an approximate bound on the time it takes to check a shortcut + int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check); + int OnlineShortcut(Real leadTime,Real padTime,RampFeasibilityChecker& check,RandomNumberGeneratorBase* rng); + + bool IsValid() const; + + /// The joint limits (optional), velocity bounds, and acceleration bounds + Vector xMin,xMax,velMax,accMax; + /// The path is stored as a series of ramps + std::vector ramps; +}; + +} //namespace ParabolicRamp + +#endif diff --git a/src/external/hauser_parabolic_smoother/HauserMath.h b/src/external/hauser_parabolic_smoother/HauserMath.h new file mode 100644 index 0000000..c9d3788 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/HauserMath.h @@ -0,0 +1,33 @@ +#ifndef PARABOLIC_RAMP_MATH_H +#define PARABOLIC_RAMP_MATH_H + +#include +#include +#include + +namespace ParabolicRamp { + +typedef double Real; +typedef std::vector Vector; + +//can replace this with your favorite representation/tests of infinity +const static Real Inf = 1e300; +inline bool IsInf(Real x) { return x==Inf; } +inline bool IsFinite(Real x) { return std::abs(x)0 ? 1 : (x<0 ? -1 : 0)); } +inline Real Min(Real x,Real y) { return (xy?x:y); } +inline bool FuzzyZero(Real x,Real tol) { return Abs(x)<=tol; } +inline bool FuzzyEquals(Real x,Real y,Real tol) { return Abs(x-y)<=tol; } +inline void Swap(Real& x,Real& y) { Real temp=x; x=y; y=temp; } + +// Returns a random number in [0,1) +inline Real Rand() { return Real(rand())/Real(RAND_MAX); } + +} //namespace ParabolicRamp + +#endif diff --git a/src/external/hauser_parabolic_smoother/ParabolicRamp.cpp b/src/external/hauser_parabolic_smoother/ParabolicRamp.cpp new file mode 100644 index 0000000..adf18f4 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/ParabolicRamp.cpp @@ -0,0 +1,2852 @@ +/***************************************************************************** + * + * Copyright (c) 2010-2011, the Trustees of Indiana University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Indiana University nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS'' + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + * + ***************************************************************************/ + +#include "ParabolicRamp.h" +#include "Config.h" +#include +#include +using namespace std; + +namespace ParabolicRamp { + +//a flag used during testing of failed ramps +static bool gSuppressSavingRamps = false; + +//a flag used to stop SolveMinAccel from complaining during internal testing +static bool gMinAccelQuiet = false; + +//a flag used to stop SolveMinTime2 from complaining during internal testing +static bool gMinTime2Quiet = false; + +//solves the quadratic formula and returns the number of roots found +int quadratic(Real a, Real b, Real c, Real &x1, Real &x2) { + if (a == 0) { + if (b == 0) { + if (c == 0) + return -1; + return 0; + } + x1 = -c / b; + return 1; + } + if (c == 0) { //det = b^2 + x1 = 0; + x2 = -b / a; + return 2; + } + + Real det = b * b - 4.0 * a * c; + if (det < 0.0) + return 0; + if (det == 0.0) { + x1 = -b / (2.0 * a); + return 1; + } + det = sqrt(det); + if (Abs(-b - det) < Abs(a)) + x1 = 0.5 * (-b + det) / a; + else + x1 = 2.0 * c / (-b - det); + if (Abs(-b + det) < Abs(a)) + x2 = 0.5 * (-b - det) / a; + else + x2 = 2.0 * c / (-b + det); + return 2; +} + +bool SaveRamp(const char *fn, Real x0, Real dx0, Real x1, Real dx1, + Real a, Real v, Real t) { + if (gSuppressSavingRamps) return true; + PARABOLIC_RAMP_PLOG("Saving ramp to %s\n", fn); + FILE *f = fopen(fn, "ab"); + if (!f) { + f = fopen(fn, "wb"); + if (!f) { + PARABOLIC_RAMP_PLOG("Unable to open file %s for saving\n", fn); + return false; + } + } + double vals[7] = {x0, dx0, x1, dx1, a, v, t}; + fwrite(vals, sizeof(double), 7, f); + fclose(f); + return true; +} + +bool LoadRamp(FILE *f, Real &x0, Real &dx0, Real &x1, Real &dx1, + Real &a, Real &v, Real &t) { + double vals[7]; + int size = fread(vals, sizeof(double), 7, f); + if (size != 7) return false; + x0 = vals[0]; + dx0 = vals[1]; + x1 = vals[2]; + dx1 = vals[3]; + a = vals[4]; + v = vals[5]; + t = vals[6]; + return true; +} + +bool LoadRamp(const char *fn, Real &x0, Real &dx0, Real &x1, Real &dx1, + Real &a, Real &v, Real &t) { + FILE *f = fopen(fn, "rb"); + if (!f) return false; + bool res = LoadRamp(f, x0, dx0, x1, dx1, a, v, t); + fclose(f); + return res; +} + +void TestRamps(const char *fn) { + FILE *f = fopen(fn, "rb"); + if (!f) return; + + gSuppressSavingRamps = true; + ParabolicRamp1D ramp; + Real a, v, t; + int numRamps = 0; + while (LoadRamp(f, ramp.x0, ramp.dx0, ramp.x1, ramp.dx1, a, v, t)) { + if (t < 0) { + PARABOLIC_RAMP_ASSERT(a >= 0 && v >= 0); + bool res = ramp.SolveMinTime(a, v); + PARABOLIC_RAMP_PLOG("Result %d: t=%g\n", (int) res, ramp.ttotal); + } else if (a < 0) { + PARABOLIC_RAMP_ASSERT(t >= 0 && v >= 0); + bool res = ramp.SolveMinAccel(t, v); + PARABOLIC_RAMP_PLOG("Result %d: a=%g\n", (int) res, ramp.a1); + } else { + bool res = ramp.SolveMinTime2(a, v, t); + PARABOLIC_RAMP_PLOG("Result %d: t=%g\n", (int) res, ramp.ttotal); + + if (!res) { + bool res = ramp.SolveMinAccel(t, v); + PARABOLIC_RAMP_PLOG("SolveMinAccel result %d: a=%g\n", (int) res, ramp.a1); + } + } + PARABOLIC_RAMP_PLOG("\n"); + numRamps++; + } + fclose(f); + PARABOLIC_RAMP_PLOG("%d ramps loaded from file %s\n", numRamps, fn); + gSuppressSavingRamps = false; +} + +class ParabolicRamp { + public: + Real Evaluate(Real t) const; + Real Derivative(Real t) const; + Real Accel(Real t) const; + bool Solve(Real amax); + bool SolveFixedTime(Real endTime); + Real MaxVelocity() const; + + //input + Real x0, dx0; + Real x1, dx1; + + //calculated + Real a; + Real ttotal; +}; + +class PPRamp { + public: + Real Evaluate(Real t) const; + Real Derivative(Real t) const; + Real Accel(Real t) const; + bool SolveMinTime(Real amax); + bool SolveMinTime2(Real amax, Real timeLowerBound); + bool SolveMinAccel(Real endTime); + Real SolveMinAccel2(Real endTime); + Real MaxVelocity() const; + + Real CalcTotalTime(Real a) const; + Real CalcSwitchTime(Real a) const; + Real CalcMinAccel(Real endTime, Real sign, Real &switchTime) const; + int CalcSwitchTimes(Real a, Real &t1, Real &t2) const; + int CalcTotalTimes(Real a, Real &t1, Real &t2) const; + + //input + Real x0, dx0; + Real x1, dx1; + + //calculated + Real a; + Real tswitch, ttotal; +}; + +class PLPRamp { + public: + Real Evaluate(Real t) const; + Real Derivative(Real t) const; + Real Accel(Real t) const; + bool SolveMinTime(Real amax, Real vmax); + bool SolveMinTime2(Real amax, Real vmax, Real timeLowerBound); + bool SolveMinAccel(Real endTime, Real vmax); + Real SolveMinAccel2(Real endTime, Real vmax); + + Real CalcTotalTime(Real a, Real v) const; + Real CalcSwitchTime1(Real a, Real v) const; + Real CalcSwitchTime2(Real a, Real v) const; + Real CalcMinAccel(Real endTime, Real v) const; //variable a + Real CalcMinTime2(Real endTime, Real a, Real vmax) const; //variable v + + //input + Real x0, dx0; + Real x1, dx1; + + //calculated + Real a, v; + Real tswitch1, tswitch2, ttotal; +}; + +Real ParabolicRamp::Evaluate(Real t) const { + return x0 + t * dx0 + 0.5 * a * Sqr(t); +} + +Real ParabolicRamp::Derivative(Real t) const { + return dx0 + a * t; +} + +Real ParabolicRamp::Accel(Real t) const { + return a; +} + +bool ParabolicRamp::Solve(Real amax) { + if (FuzzyEquals(x0, x1, EpsilonX)) { + if (FuzzyEquals(dx0, dx1, EpsilonV)) { + a = 0; + ttotal = 0; + return true; + } else if (FuzzyEquals(dx0, -dx1, EpsilonV)) { //pure parabola, any acceleration works + a = amax * Sign(dx1); + ttotal = (dx1 - dx0) / a; + return true; + } + //no parabola will work + return false; + } + a = 0.5 * (Sqr(dx0) - Sqr(dx1)) / (x0 - x1); + //pick the denominator less likely to result in numerical errors + if (Abs(a) < Abs(dx0 + dx1)) { + if (Abs(dx0 + dx1) < EpsilonV) { + //danger of numerical errors + //dx0 = - dx1 + //need x0 = x1 + return false; + } else { + ttotal = 2.0 * (x1 - x0) / (dx0 + dx1); + } + } else { + ttotal = (dx1 - dx0) / a; + } + if (ttotal < 0 && ttotal > -EpsilonT) ttotal = 0; + if (ttotal < 0) { + ttotal = -1; + a = 0; + return false; + } + + //check for numerical errors + if (Abs(a) > amax && Abs(a) <= amax + EpsilonA) { + //double check if the capped version works + a = Sign(a) * amax; + } + if (FuzzyEquals(Evaluate(ttotal), x1, EpsilonX) && FuzzyEquals(Derivative(ttotal), dx1, EpsilonV)) { + return true; + } + return false; +} + +bool ParabolicRamp::SolveFixedTime(Real endTime) { + ttotal = endTime; + if (FuzzyEquals(x0, x1, EpsilonX)) { + if (FuzzyEquals(dx0, dx1, EpsilonV)) { + a = 0; + return FuzzyEquals(endTime, 0.0, EpsilonT); + } else if (FuzzyEquals(dx0, -dx1, EpsilonV)) { //pure parabola, any acceleration works + a = (dx1 - dx0) / endTime; + return true; + } + //no parabola will work + return false; + } + a = 0.5 * (Sqr(dx0) - Sqr(dx1)) / (x0 - x1); + //pick the denominator less likely to result in numerical errors + if (Abs(a) < Abs(dx0 + dx1)) { + if (Abs(dx0 + dx1) < EpsilonV) { + //danger of numerical errors + //dx0 = - dx1 + //need x0 = x1 + return false; + } else { + ttotal = 2.0 * (x1 - x0) / (dx0 + dx1); + } + } else { + ttotal = (dx1 - dx0) / a; + } + if (!FuzzyEquals(ttotal, endTime, EpsilonT)) return false; + ttotal = endTime; + if (FuzzyEquals(Evaluate(ttotal), x1, EpsilonX) && FuzzyEquals(Derivative(ttotal), dx1, EpsilonV)) { + return true; + } + return false; +} + +Real ParabolicRamp::MaxVelocity() const { + if (fabs(dx0) > fabs(dx1)) return dx0; + else return dx1; +} + +Real PPRamp::Evaluate(Real t) const { + if (t < tswitch) return x0 + 0.5 * a * t * t + dx0 * t; + else { + Real tmT = t - ttotal; + return x1 - 0.5 * a * tmT * tmT + dx1 * tmT; + } +} + +Real PPRamp::Derivative(Real t) const { + if (t < tswitch) return a * t + dx0; + else { + Real tmT = t - ttotal; + return -a * tmT + dx1; + } +} + +Real PPRamp::Accel(Real t) const { + if (t < tswitch) return a; + else return -a; +} + +bool PPRamp::SolveMinTime(Real amax) { + Real tpn = CalcTotalTime(amax), tnp = CalcTotalTime(-amax); + //cout<<"Time for parabola +-: "<= 0) { + if (tnp >= 0 && tnp < tpn) { + a = -amax; + ttotal = tnp; + } else { + a = amax; + ttotal = tpn; + } + } else if (tnp >= 0) { + a = -amax; + ttotal = tnp; + } else { + tswitch = -1; + ttotal = -1; + a = 0; + return false; + } + tswitch = CalcSwitchTime(a); + + //uncomment for additional debugging + if (gValidityCheckLevel >= 1) { + if (!FuzzyEquals(x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch, + x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal), + EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Numerical error computing parabolic-parabolic min-time...\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0=%g,%g, x1=%g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("a = %g, tswitch = %g, ttotal = %g\n", a, tswitch, ttotal); + PARABOLIC_RAMP_PERROR("Forward %g, backward %g, diff %g\n", + x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch, + x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal), + x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch + - (x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal))); + //Real b = 2.0*dx0; //2.0*(dx0-dx1); + //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1; + Real b = 2.0 * a * dx0; //2.0*(dx0-dx1); + Real c = (Sqr(dx0) - Sqr(dx1)) * 0.5 + (x0 - x1) * a; + Real t1, t2; + int res = quadratic(a * a, b, c, t1, t2); + PARABOLIC_RAMP_PERROR("Quadratic equation %g x^2 + %g x + %g = 0\n", a * a, b, c); + PARABOLIC_RAMP_PERROR("%d results, %g %g\n", res, t1, t2); + if (gErrorGetchar) getchar(); + } + if (gErrorSave) SaveRamp("PP_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, Inf, -1); + return false; + } + } + + return true; +} + +bool PPRamp::SolveMinTime2(Real amax, Real timeLowerBound) { + PARABOLIC_RAMP_ASSERT(timeLowerBound >= 0); + Real t1pn, t1np, t2pn, t2np; + int respn = CalcTotalTimes(amax, t1pn, t2pn); + int resnp = CalcTotalTimes(-amax, t1np, t2np); + ttotal = Inf; + if (respn >= 1) { + if (t1pn >= timeLowerBound && t1pn < ttotal) { + ttotal = t1pn; + a = amax; + } + } + if (respn >= 2) { + if (t2pn >= timeLowerBound && t2pn < ttotal) { + ttotal = t2pn; + a = amax; + } + } + if (resnp >= 1) { + if (t1np >= timeLowerBound && t1np < ttotal) { + ttotal = t1np; + a = -amax; + } + } + if (resnp >= 2) { + if (t2np >= timeLowerBound && t2np < ttotal) { + ttotal = t2np; + a = -amax; + } + } + if (IsInf(ttotal)) { + a = 0; + tswitch = ttotal = -1; + return false; + } + + Real ts1, ts2; + int res = CalcSwitchTimes(a, ts1, ts2); + PARABOLIC_RAMP_ASSERT(res > 0); + if (res == 1) { + tswitch = ts1; + PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal, ts1 * 2.0 - (dx1 - dx0) / a, EpsilonT)); + } else { + if (FuzzyEquals(ttotal, ts1 * 2.0 - (dx1 - dx0) / a, EpsilonT)) + tswitch = ts1; + else { + PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal, ts2 * 2.0 - (dx1 - dx0) / a, EpsilonT)); + tswitch = ts2; + } + } + + //uncomment for additional debugging + if (gValidityCheckLevel >= 1) { + Real eps = EpsilonX; + if (!FuzzyEquals(x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch, + x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal), + eps)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Numerical error in PPRamp::SolveMinTime2...\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0=%g,%g, x1=%g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("a = %g, tswitch = %g, ttotal = %g\n", a, tswitch, ttotal); + PARABOLIC_RAMP_PERROR("Forward %g, backward %g, diff %g\n", + x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch, + x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal), + x0 + 0.5 * a * Sqr(tswitch) + dx0 * tswitch + - (x1 - 0.5 * a * Sqr(tswitch - ttotal) + dx1 * (tswitch - ttotal))); + //Real b = 2.0*dx0; //2.0*(dx0-dx1); + //Real c = (Sqr(dx0)-Sqr(dx1))*0.5/a+x0-x1; + Real b = 2.0 * a * dx0; //2.0*(dx0-dx1); + Real c = (Sqr(dx0) - Sqr(dx1)) * 0.5 + (x0 - x1) * a; + Real t1, t2; + int res = quadratic(a * a, b, c, t1, t2); + PARABOLIC_RAMP_PERROR("Quadratic equation %g x^2 + %g x + %g = 0\n", a * a, b, c); + PARABOLIC_RAMP_PERROR("%d results, %g %g\n", res, t1, t2); + } + if (gErrorGetchar) getchar(); + if (gErrorSave) SaveRamp("PP_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, Inf, timeLowerBound); + return false; + } + } + return true; +} + +bool PPRamp::SolveMinAccel(Real endTime) { + Real switch1, switch2; + Real apn = CalcMinAccel(endTime, 1.0, switch1); + Real anp = CalcMinAccel(endTime, -1.0, switch2); + //cout<<"Accel for parabola +-: "<= 0) { + if (anp >= 0 && anp < apn) a = -anp; + else a = apn; + } else if (anp >= 0) a = -anp; + else { + a = 0; + tswitch = -1; + ttotal = -1; + return false; + } + ttotal = endTime; + if (a == apn) + tswitch = switch1; + else + tswitch = switch2; + + //debug + if (gValidityCheckLevel >= 1) { + Real t2mT = tswitch - ttotal; + if (!FuzzyEquals(x0 + tswitch * dx0 + 0.5 * a * Sqr(tswitch), x1 + t2mT * dx1 - 0.5 * a * Sqr(t2mT), EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("PPRamp::SolveMinAccel: Numerical error, x mismatch!\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Forward ramp: %g, backward %g, diff %g\n", + x0 + tswitch * dx0 + 0.5 * a * Sqr(tswitch), + x1 + t2mT * dx1 - 0.5 * a * Sqr(t2mT), + x0 + tswitch * dx0 + 0.5 * a * Sqr(tswitch) - (x1 + t2mT * dx1 - 0.5 * a * Sqr(t2mT))); + PARABOLIC_RAMP_PERROR("A+ = %g, A- = %g\n", apn, anp); + PARABOLIC_RAMP_PERROR("ramp %g,%g -> %g, %g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("Switch 1 %g, switch 2 %g, total %g\n", switch1, switch2, ttotal); + + { + Real sign = 1.0; + Real a = Sqr(endTime); + Real b = sign * (2.0 * (dx0 + dx1) * endTime + 4.0 * (x0 - x1)); + Real c = -Sqr(dx1 - dx0); + PARABOLIC_RAMP_PERROR("Quadratic %g x^2 + %g x + %g = 0\n", a, b, c); + Real t1, t2; + int res = quadratic(a, b, c, t1, t2); + PARABOLIC_RAMP_PERROR("Solutions: %d, %g and %g\n", res, t1, t2); + } + { + Real sign = -1.0; + Real a = Sqr(endTime); + Real b = sign * (2.0 * (dx0 + dx1) * endTime + 4.0 * (x0 - x1)); + Real c = -Sqr(dx1 - dx0); + PARABOLIC_RAMP_PERROR("Quadratic %g x^2 + %g x + %g = 0\n", a, b, c); + Real t1, t2; + int res = quadratic(a, b, c, t1, t2); + PARABOLIC_RAMP_PERROR("Solutions: %d, %g and %g\n", res, t1, t2); + } + } + if (gErrorSave) SaveRamp("PP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, Inf, endTime); + if (gErrorGetchar) getchar(); + } + if (!FuzzyEquals(dx0 + a * tswitch, dx1 - a * t2mT, EpsilonV)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("PPRamp::SolveMinAccel: Numerical error, v mismatch!\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Velocity error %g vs %g, err %g\n", + dx0 + a * tswitch, + dx1 - a * t2mT, + dx0 + a * tswitch - (dx1 - a * t2mT)); + PARABOLIC_RAMP_PERROR("ramp %g,%g -> %g, %g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("Accel %g\n", a); + PARABOLIC_RAMP_PERROR("Switch %g, total %g\n", tswitch, ttotal); + } + if (gErrorSave) SaveRamp("PP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, Inf, endTime); + if (gErrorGetchar) getchar(); + return false; + } + } + return true; +} + +Real PPRamp::MaxVelocity() const { + return tswitch * a + dx0; +} + +Real PPRamp::CalcTotalTime(Real a) const { + Real tswitch = CalcSwitchTime(a); + //printf("a = %g, switch time %g\n",a,tswitch); + if (tswitch < 0) return -1; + if (tswitch < (dx1 - dx0) / a) return -1; + return tswitch * 2.0 - (dx1 - dx0) / a; +} + +int PPRamp::CalcTotalTimes(Real a, Real &t1, Real &t2) const { + Real ts1, ts2; + int res = CalcSwitchTimes(a, ts1, ts2); + if (res == 0) return res; + else if (res == 1) { + if (ts1 < (dx1 - dx0) / a) return 0; + t1 = ts1 * 2.0 - (dx1 - dx0) / a; + return 1; + } else { + //check limits + if (ts1 < (dx1 - dx0) / a) t1 = -1; + else t1 = ts1 * 2.0 - (dx1 - dx0) / a; + if (ts2 < (dx1 - dx0) / a) t2 = -1; + else t2 = ts2 * 2.0 - (dx1 - dx0) / a; + if (t1 < 0) { + if (t2 < 0) return 0; + t1 = t2; + return 1; + } else { + if (t2 < 0) return 1; + else return 2; + } + } +} + +int PPRamp::CalcSwitchTimes(Real a, Real &t1, Real &t2) const { + int res; + if (Abs(a) > 1.0) { + //this may be prone to numerical errors + Real b = 2.0 * dx0; //2.0*(dx0-dx1); + Real c = (Sqr(dx0) - Sqr(dx1)) * 0.5 / a + x0 - x1; + res = quadratic(a, b, c, t1, t2); + } else { + Real b = 2.0 * a * dx0; //2.0*(dx0-dx1); + Real c = (Sqr(dx0) - Sqr(dx1)) * 0.5 + (x0 - x1) * a; + res = quadratic(a * a, b, c, t1, t2); + } + if (res == 0) { + return res; + } else if (res == 2) { + if (t1 < 0 && t1 > -EpsilonT * 0.1) t1 = 0; + if (t2 < 0 && t2 > -EpsilonT * 0.1) t2 = 0; + if (t1 < 0 || t1 * Abs(a) < (dx1 - dx0) * Sign(a)) { + if (t2 < 0 || t2 * Abs(a) < (dx1 - dx0) * Sign(a)) return 0; + t1 = t2; + return 1; + } else if (t2 < 0 || t2 * Abs(a) < (dx1 - dx0) * Sign(a)) { + return 1; + } else { + return 2; //both are ok + } + } else { + if (t1 < 0 && t1 > -EpsilonT) t1 = 0; + if (t1 < 0) return 0; + return 1; + } +} + +Real PPRamp::CalcSwitchTime(Real a) const { + Real t1, t2; + int res = CalcSwitchTimes(a, t1, t2); + if (res == 0) { + return -1; + } else if (res == 2) { + //check total time + if (t2 * Abs(a) < (dx1 - dx0) * Sign(a)) return t1; + else if (t1 * Abs(a) < (dx1 - dx0) * Sign(a)) return t2; + else return Min(t1, t2); //both are ok + } else return t1; +} + +//for a PP ramp: +//xs = x0 + ts*dx0 + 0.5*z*ts^2 +//xs = x1 - (T-ts)*dx1 - 0.5*z*(T-ts)^2 +//xs' = dx0 + ts*z +//xs' = dx1 + (T-ts)*z +//z = sign*a +//(2ts-T)*z = dx1 - dx0 +//ts = 1/2* (T+(dx1 - dx0)/z) +//T-ts = 1/2* (T-(dx1 - dx0)/z) +//2 T(dx0+dx1) + 4(x0 - x1) - (dx1 - dx0)^2/z + z*T^2 = 0 +//what if z is close to 0? +//suppose dx1 ~= dx0, then the other solution is +//4 T dx0 + 4(x0 - x1) + z*T^2 = 0 +//=>z = - 4 dx0 / T + 4(x1 - x0)/T^2 +// +//alt: let y = (dx1 - dx0)/z, z = (dx1 - dx0)/y (y is a time shift) +//ts = 1/2* (T+y) +//T-ts = 1/2* (T-y) +//x0 + 1/2(T+y)*dx0 + 0.5*z*1/4(T+y)^2 = x1 - 1/2(T-y)*dx1 - 0.5*z*1/4(T-y)^2 +//4(x0-x1) + 2(T+y)*dx0 + 0.5*z*(T+y)^2 = - 2(T-y)*dx1 - 0.5*z*(T-y)^2 +//[4(x0-x1)/T + 2(dx0+dx1)] y - y^2 (dx1 - dx0)/T + (dx1 - dx0) T = 0 +Real PPRamp::CalcMinAccel(Real endTime, Real sign, Real &switchTime) const { + Real a = -(dx1 - dx0) / endTime; + Real b = (2.0 * (dx0 + dx1) + 4.0 * (x0 - x1) / endTime); + Real c = (dx1 - dx0) * endTime; + Real rat1, rat2; + int res = quadratic(a, b, c, rat1, rat2); + Real accel1 = (dx1 - dx0) / rat1; + Real accel2 = (dx1 - dx0) / rat2; + Real switchTime1 = endTime * 0.5 + 0.5 * rat1; + Real switchTime2 = endTime * 0.5 + 0.5 * rat2; + //fix up numerical errors + if (switchTime1 > endTime && switchTime1 < endTime + EpsilonT * 1e-1) + switchTime1 = endTime; + if (switchTime2 > endTime && switchTime2 < endTime + EpsilonT * 1e-1) + switchTime2 = endTime; + if (switchTime1 < 0 && switchTime1 > EpsilonT * 1e-1) + switchTime1 = 0; + if (switchTime2 < 0 && switchTime2 > EpsilonT * 1e-1) + switchTime2 = 0; + if (res > 0 && FuzzyZero(rat1, EpsilonT)) { + //consider it as a zero, ts = T/2 + //z = - 4*(x0-x1)/T^2 - 2 (dx0+dx1)/T + if (!FuzzyZero(endTime, EpsilonT)) { //no good answer if endtime is small + accel1 = -2.0 * (dx0 + dx1) / endTime + 4.0 * (x1 - x0) / Sqr(endTime); + } + } + if (res > 1 && FuzzyZero(rat2, EpsilonT)) { + if (!FuzzyZero(endTime, EpsilonT)) { //no good answer if endtime is small + accel2 = -2.0 * (dx0 + dx1) / endTime + 4.0 * (x1 - x0) / Sqr(endTime); + } + } + bool firstInfeas = false; + if (res > 0 + && (FuzzyZero(accel1, EpsilonA) || FuzzyZero(endTime / rat1, EpsilonA))) { //infer that accel must be small + if (!FuzzyZero(dx0 - dx1, EpsilonT)) { //no good answer if dx0=dx1 + switchTime1 = endTime * 0.5; + } + if (!FuzzyEquals(x0 + switchTime1 * dx0 + 0.5 * Sqr(switchTime1) * accel1, + x1 - (endTime - switchTime1) * dx1 - 0.5 * Sqr(endTime - switchTime1) * accel1, + EpsilonX) || + !FuzzyEquals(dx0 + switchTime1 * accel1, dx1 + (endTime - switchTime1) * accel1, EpsilonV)) { + firstInfeas = true; + } + } + if (res > 1 && (FuzzyZero(accel2, EpsilonA) || FuzzyZero(endTime / rat2, EpsilonA))) { + if (!FuzzyZero(dx0 - dx1, EpsilonT)) { //no good answer if dx0=dx1 + switchTime2 = endTime * 0.5; + } + if (!FuzzyEquals(x0 + switchTime2 * dx0 + 0.5 * Sqr(switchTime2) * accel2, + x1 - (endTime - switchTime2) * dx1 - 0.5 * Sqr(endTime - switchTime2) * accel2, + EpsilonX) || + !FuzzyEquals(dx0 + switchTime2 * accel2, dx1 + (endTime - switchTime2) * accel2, EpsilonV)) { + res--; + } + } + if (firstInfeas) { + accel1 = accel2; + rat1 = rat2; + switchTime1 = switchTime2; + res--; + } + if (res == 0) return -1; + else if (res == 1) { + if (switchTime1 >= 0 && switchTime1 <= endTime) { + switchTime = switchTime1; + return sign * accel1; + } + return -1.0; + } else if (res == 2) { + if (switchTime1 >= 0 && switchTime1 <= endTime) { + if (switchTime2 >= 0 && switchTime2 <= endTime) { + if (accel1 < accel2) { + switchTime = switchTime1; + return sign * accel1; + } + else { + switchTime = switchTime2; + return sign * accel2; + } + } else { + switchTime = switchTime1; + return sign * accel1; + } + } else if (switchTime2 >= 0 && switchTime2 <= endTime) { + switchTime = switchTime2; + return sign * accel2; + } + return -1.0; + } + if (FuzzyZero(a, EpsilonT) && FuzzyZero(b, EpsilonT) && FuzzyZero(c, EpsilonT)) { + switchTime = 0.5 * endTime; + return 0; + } + return -1.0; + + /* + Real a=endTime; + Real b=sign*(2.0*(dx0+dx1)+4.0*(x0-x1)/endTime); + if(FuzzyZero(b,EpsilonX)) { + //need to double check for numerical instability + //if sign is +, this means we're switching directly to - + //if sign is -, this means we're switching directly to + + //if(sign > 0.0 && x1 > x0+dx0*endTime) return -1; + //else if(sign < 0.0 && x1 < x0+dx0*endTime) return -1; + switchTime = 0; + Real a=(dx1-dx0)/endTime; + if((sign > 0.0) == (a >= 0.0)) return -1; + else return Abs(a); + } + Real c=-Sqr(dx1-dx0)/endTime; + if(FuzzyEquals(dx1,dx0,EpsilonV)) { + //one of the solutions will be very close to zero, use alt solution + Real a=-2.0*(dx0+dx1)/endTime + 4.0*(x1-x0)/Sqr(endTime); + printf("only two solutions: 0 and %g\n",a); + switchTime = 0.5*endTime; + //try out the zero solution + printf("diff at 0 solution: %g\n",x0-x1 + switchTime*(dx0+dx1)); + if(FuzzyEquals(x0 + switchTime*dx0,x1 - switchTime*dx1,EpsilonX)) + return 0; + PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0 + switchTime*a,dx1 + switchTime*a,EpsilonV)); + PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0 + switchTime*dx0 + 0.5*a*Sqr(switchTime),x1 - switchTime*dx1-0.5*a*Sqr(switchTime),EpsilonX)); + if((sign > 0.0) != (a >= 0.0)) return -1; + return Abs(a); + } + if(FuzzyZero(c,EpsilonA)) { + //need better numerical performance when dx1 ~= dx0 + a = a/Abs(dx1-dx0); + b = b/Abs(dx1-dx0); + c = -Abs(dx1-dx0)/endTime; + } + Real accel1,accel2; + int res=quadratic(a,b,c,accel1,accel2); + //remove negative accelerations + if(res >= 1 && accel1 < 0) { + accel1 = accel2; + res--; + } + if(res >= 2 && accel2 < 0) { + res--; + } + + Real switchTime1 = endTime*0.5+sign*0.5*(dx1-dx0)/accel1; + Real switchTime2 = endTime*0.5+sign*0.5*(dx1-dx0)/accel2; + //if(accel1 == 0 && x0 == x1) switchTime1 = 0; + //if(accel2 == 0 && x0 == x1) switchTime2 = 0; + if(res==0) return -1; + else if(res==1) { + if(!IsFinite(accel1)) { + printf("Error computing accelerations!\n"); + printf("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); + printf("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1); + printf("EndTime %g, sign %g\n",endTime,sign); + printf("Results %g %g\n",accel1,accel2); + getchar(); + } + if(switchTime1 >= 0 && switchTime1 <= endTime) { switchTime=switchTime1; return accel1; } + return -1.0; + } + else if(res==2) { + if(!IsFinite(accel1) || !IsFinite(accel2)) { + printf("Error computing accelerations!\n"); + printf("Quadratic %g x^2 + %g x + %g = 0\n",a,b,c); + printf("x0 %g, dx0 %g, x1 %g, dx1 %g\n",x0,dx0,x1,dx1); + printf("EndTime %g, sign %g\n",endTime,sign); + printf("Results %g %g\n",accel1,accel2); + getchar(); + } + if(FuzzyZero(switchTime1,EpsilonT) || FuzzyZero(switchTime2,EpsilonT)) { + //need to double check for numerical instability + //if sign is +, this means we're switching directly to - + //if sign is -, this means we're switching directly to + + if(sign > 0.0 && x1 > x0+dx0*endTime) return -1; + else if(sign < 0 && x1 < x0+dx0*endTime) return -1; + switchTime = 0; + if(FuzzyZero(switchTime1,EpsilonT)) return accel1; + else return accel2; + } + if(switchTime1 >= 0 && switchTime1 <= endTime) { + if(switchTime2 >= 0 && switchTime2 <= endTime) { + if(switchTime1 < switchTime2) { switchTime=switchTime1; return accel1; } + else { switchTime=switchTime2; return accel2; } + } + else { switchTime=switchTime1; return accel1; } + } + else if(switchTime2 >= 0 && switchTime2 <= endTime) { switchTime=switchTime2; return accel2; } + return -1.0; + } + return -1.0; + */ +} + +Real PLPRamp::Evaluate(Real t) const { + Real tmT = t - ttotal; + if (t < tswitch1) return x0 + 0.5 * a * Sqr(t) + dx0 * t; + else if (t < tswitch2) { + Real xswitch = x0 + 0.5 * a * Sqr(tswitch1) + dx0 * tswitch1; + return xswitch + (t - tswitch1) * v; + } else return x1 - 0.5 * a * Sqr(tmT) + dx1 * tmT; +} + +Real PLPRamp::Derivative(Real t) const { + Real tmT = t - ttotal; + if (t < tswitch1) return a * t + dx0; + else if (t < tswitch2) return v; + else return -a * tmT + dx1; +} + +Real PLPRamp::Accel(Real t) const { + if (t < tswitch1) return a; + else if (t < tswitch2) return 0; + else return -a; +} + +Real PLPRamp::CalcTotalTime(Real a, Real v) const { + Real t1 = (v - dx0) / a; + Real t2mT = (dx1 - v) / a; + Real y1 = 0.5 * (Sqr(v) - Sqr(dx0)) / a + x0; + Real y2 = 0.5 * (Sqr(dx1) - Sqr(v)) / a + x1; + Real t2mt1 = (y2 - y1) / v; + //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1; + if (t1 < 0 || t2mT > 0 || t2mt1 < 0) return -1; + if (!IsFinite(t1) || !IsFinite(t2mT)) return -1; + return t1 + t2mt1 - t2mT; +} + +Real PLPRamp::CalcSwitchTime1(Real a, Real v) const { + Real t1 = (v - dx0) / a; + if (t1 < 0) return -1; + return t1; +} + +Real PLPRamp::CalcSwitchTime2(Real a, Real v) const { + Real t1 = (v - dx0) / a; + Real y1 = 0.5 * (Sqr(v) - Sqr(dx0)) / a + x0; + Real y2 = 0.5 * (Sqr(dx1) - Sqr(v)) / a + x1; + Real t2mt1 = (y2 - y1) / v; + //Real t2mt1 = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v; + //Real xswitch = x0 + 0.5*a*Sqr(t1) + dx0*t1; + if (t1 < 0 || t2mt1 < 0) return -1; + return t1 + t2mt1; +} + +Real PLPRamp::CalcMinAccel(Real endTime, Real v) const { + Real den = endTime * v - (x1 - x0); + //straight line sections have den ~= 0 + if (FuzzyZero(den, EpsilonX)) { + if (FuzzyEquals(dx0, v, EpsilonV) && FuzzyEquals(dx1, v, EpsilonV)) return 0; + return Inf; + } + + //Real a = (v - (dx0+dx1) + 0.5/v*(Sqr(dx0)+Sqr(dx1)))/(endTime - (x1-x0)/v); + Real a = (Sqr(v) - v * (dx0 + dx1) + 0.5 * (Sqr(dx0) + Sqr(dx1))) / den; + /* + Real t1 = (v-dx0)/a; + Real t2mT = (dx1-v)/a; + Real y1 = 0.5*(Sqr(v) - Sqr(dx0))/a + x0; + Real y2 = 0.5*(Sqr(dx1) - Sqr(v))/a + x1; + //Real t2mt1 = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v; + Real t2mt1 = (y2-y1)/v; + Real vold = v; + //cout<<"EndTime "< %g,%g\n",x0,dx0,x1,dx1); + fprintf(stderr," endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n",endTime,a,v,CalcSwitchTime1(a,v),CalcSwitchTime2(a,v),CalcTotalTime(a,v)); + PARABOLIC_RAMP_ASSERT(v == vold); + printf("y1=%g, y2=%g, t2mt1 = %g\n",y1,y2,t2mt1); + Real y1_test = 0.5*(Sqr(v) - Sqr(dx0))/a + x0; + Real y2_test = 0.5*(Sqr(dx1) - Sqr(v))/a + x1; + Real t2mt1_test = (y2_test-y1_test)/v; + //Real t2mt1_test = 0.5*(Sqr(dx1) + Sqr(dx0))/(v*a) - v/a + (x1 - x0)/v; + printf("y1=%g, y2=%g, t2mt1 = %g\n",y1_test,y2_test,t2mt1_test); + printf("dy1=%g, dy2=%g, dt2mt1 = %g\n",y1-y1_test,y2-y2_test,t2mt1-t2mt1_test); + getchar(); + return Inf; + PARABOLIC_RAMP_ASSERT(y1 == y1_test); + PARABOLIC_RAMP_ASSERT(y2 == y2_test); + PARABOLIC_RAMP_ASSERT(y2-y1 == y2_test-y1_test); + PARABOLIC_RAMP_ASSERT(t2mt1 == t2mt1_test); + } + PARABOLIC_RAMP_ASSERT(CalcTotalTime(a,v) >= 0); + return a; + */ +} + +bool PLPRamp::SolveMinTime(Real amax, Real vmax) { + return SolveMinTime2(amax, vmax, 0); +} + +bool PLPRamp::SolveMinTime2(Real amax, Real vmax, Real timeLowerBound) { + Real t1 = CalcTotalTime(amax, vmax); + Real t2 = CalcTotalTime(-amax, vmax); + Real t3 = CalcTotalTime(amax, -vmax); + Real t4 = CalcTotalTime(-amax, -vmax); + //cout<<"Time for PLP ++-: "<= timeLowerBound && t1 < ttotal) { + a = amax; + v = vmax; + ttotal = t1; + } + if (t2 >= timeLowerBound && t2 < ttotal) { + a = -amax; + v = vmax; + ttotal = t2; + } + if (t3 >= timeLowerBound && t3 < ttotal) { + a = amax; + v = -vmax; + ttotal = t3; + } + if (t4 >= timeLowerBound && t4 < ttotal) { + a = -amax; + v = -vmax; + ttotal = t4; + } + if (IsInf(ttotal)) { + a = v = 0; + tswitch1 = tswitch2 = ttotal = -1; + + //printf("Times... %g %g %g %g\n",t1,t2,t3,t4); + //printf("Trying alternate MinTime2 solution technique...\n"); + Real v1 = CalcMinTime2(timeLowerBound, amax, vmax); + Real v2 = CalcMinTime2(timeLowerBound, -amax, vmax); + if (v1 > 0) { + a = amax; + v = v1; + tswitch1 = (v1 - dx0) / a; + tswitch2 = timeLowerBound - (v1 - dx1) / a; + ttotal = timeLowerBound; + //printf("Candidate 1 timing %g %g %g\n",tswitch1,tswitch2,ttotal); + //printf("Test 1 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch2)*v1); + //printf("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswitch1)*a,dx0+tswitch1*a); + //printf("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch2)+0.5*Sqr(ttotal-tswitch2)*a,dx1+(ttotal-tswitch2)*a); + return true; + } + if (v2 > 0) { + a = -amax; + v = v2; + tswitch1 = (v2 - dx0) / a; + tswitch2 = timeLowerBound - (v2 - dx1) / a; + ttotal = timeLowerBound; + //printf("Candidate 2 timing %g %g %g\n",tswitch1,tswitch2,ttotal); + //printf("Test 2 x %g %g\n",x1-(ttotal-tswitch2)*v1+0.5*Sqr(ttotal-tswitch2)*a,x0+tswitch1*dx0+0.5*Sqr(tswitch1)*a+(tswitch1-tswitch2)*v1); + //printf("x1, v1 = %g, %g\n",x0+dx0*tswitch1+0.5*Sqr(tswitch1)*a,dx0+tswitch1*a); + //printf("x2, v2 = %g, %g\n",x1-dx1*(ttotal-tswitch1)+0.5*Sqr(ttotal-tswitch1)*a,dx1+(ttotal-tswitch2)*a); + return true; + } + return false; + } + tswitch1 = CalcSwitchTime1(a, v); + tswitch2 = CalcSwitchTime2(a, v); + + if (tswitch1 > tswitch2 && FuzzyEquals(tswitch1, tswitch2, EpsilonT)) { + tswitch1 = tswitch2 = 0.5 * (tswitch1 + tswitch2); + } + if (tswitch2 > ttotal && FuzzyEquals(tswitch2, ttotal, EpsilonT)) { + tswitch2 = ttotal; + } + + if (gValidityCheckLevel >= 1) { + Real t2mT = tswitch2 - ttotal; + Real xswitch = x0 + 0.5 * a * Sqr(tswitch1) + dx0 * tswitch1; + Real xswitch2 = xswitch + (tswitch2 - tswitch1) * v; + if (!FuzzyEquals(xswitch2, x1 - 0.5 * a * Sqr(t2mT) + dx1 * t2mT, EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinTime2: incorrect switch 2 position: %g vs %g\n", + xswitch2, + x1 - 0.5 * a * Sqr(t2mT) + dx1 * t2mT); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n", a, v, -a); + PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n", tswitch1, tswitch2, ttotal); + } + if (gErrorSave) SaveRamp("PLP_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, vmax, timeLowerBound); + if (gErrorGetchar) getchar(); + return false; + } + } + return true; +} + +bool PLPRamp::SolveMinAccel(Real endTime, Real vmax) { + Real a1 = CalcMinAccel(endTime, vmax); + Real a2 = CalcMinAccel(endTime, -vmax); + a = Inf; + if (fabs(a1) < a) { + a = a1; + v = vmax; + } + if (fabs(a2) < a) { + a = a2; + v = -vmax; + } + if (IsInf(a)) { + a = 0; + tswitch1 = tswitch2 = ttotal = -1; + return false; + } + if (fabs(a) == 0) { + tswitch1 = 0; + tswitch2 = endTime; + ttotal = endTime; + } else { + ttotal = CalcTotalTime(a, v); + tswitch1 = CalcSwitchTime1(a, v); + tswitch2 = CalcSwitchTime2(a, v); + + if (tswitch1 > tswitch2 && FuzzyEquals(tswitch1, tswitch2, EpsilonT)) { + tswitch1 = tswitch2 = 0.5 * (tswitch1 + tswitch2); + } + if (tswitch2 > endTime && FuzzyEquals(tswitch2, endTime, EpsilonT)) { + tswitch2 = endTime; + } + if (ttotal < 0) { //there was an error computing the total time + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR( + "PLPRamp::SolveMinAccel: some numerical error prevented computing total time\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR(" Ramp %g,%g -> %g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n", + endTime, + a, + v, + tswitch1, + tswitch2, + ttotal); + } + if (gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, vmax, endTime); + if (gErrorGetchar) getchar(); + return false; + } + } + if (gValidityCheckLevel >= 1) { + if (ttotal > endTime + EpsilonT) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinAccel: total time greater than endTime!\n"); + if (gVerbose >= 2) + PARABOLIC_RAMP_PERROR(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n", + endTime, + a, + v, + tswitch1, + tswitch2, + ttotal); + if (gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, vmax, endTime); + if (gErrorGetchar) getchar(); + return false; + } + if (fabs(ttotal - endTime) >= EpsilonT) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("PLPRamp::SolveMinAccel: total time and endTime are different!\n"); + if (gVerbose >= 2) + PARABOLIC_RAMP_PERROR(" endTime %g, accel %g, vel %g, switch times %g %g, total time %g\n", + endTime, + a, + v, + tswitch1, + tswitch2, + ttotal); + if (gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, vmax, endTime); + if (gErrorGetchar) getchar(); + } + } + PARABOLIC_RAMP_ASSERT(fabs(ttotal - endTime) < EpsilonT); + //fiddle with the numerical errors + ttotal = endTime; + if (tswitch2 > ttotal) tswitch2 = ttotal; + if (tswitch1 > ttotal) tswitch1 = ttotal; + + if (gValidityCheckLevel >= 1) { + Real t2mT = tswitch2 - ttotal; + Real xswitch = x0 + 0.5 * a * Sqr(tswitch1) + dx0 * tswitch1; + Real xswitch2 = xswitch + (tswitch2 - tswitch1) * v; + if (!FuzzyEquals(xswitch2, x1 - 0.5 * a * Sqr(t2mT) + dx1 * t2mT, EpsilonX)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("PLP Ramp has incorrect switch 2 position: %g vs %g\n", + xswitch2, + x1 - 0.5 * a * Sqr(t2mT) + dx1 * t2mT); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n", a, v, -a); + PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n", tswitch1, tswitch2, ttotal); + } + if (gErrorSave) SaveRamp("PLP_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, vmax, endTime); + if (gErrorGetchar) getchar(); + return false; + } + } + return true; +} + +Real PLPRamp::CalcMinTime2(Real endTime, Real a, Real vmax) const { + //Try alternate solution technique with acceleration bounded, time fixed, velocity variable + Real b = -a * endTime - (dx1 + dx0); + Real c = a * (x1 - x0) + (Sqr(dx0) + Sqr(dx1)) * 0.5; + Real v1, v2; + //printf("Quadratic coeffs %g, %g, %g\n",1.0,b,c); + int res = quadratic(1.0, b, c, v1, v2); + //printf("Quadratic res %d, accel %g, velocities %g %g\n",res,a,v1,v2); + if (res >= 1) { + Real ts1 = (v1 - dx0) / a; + Real ts2 = endTime - (v1 - dx1) / a; + //printf("Solution 1 times %g %g %g\n",ts1,ts2,endTime); + if (Abs(v1) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) return v1; //it's a valid solution! + } + if (res == 2) { + Real ts1 = (v2 - dx0) / a; + Real ts2 = endTime - (v2 - dx1) / a; + //printf("Solution 2 times %g %g %g\n",ts1,ts2,endTime); + if (Abs(v2) <= vmax && ts1 >= 0 && ts2 >= ts1 && ts2 <= endTime) return v2; //it's a valid solution! + } + return -1; +} + +void ParabolicRamp1D::SetConstant(Real x, Real t) { + x0 = x1 = x; + dx0 = dx1 = 0; + tswitch1 = tswitch2 = ttotal = t; + v = a1 = a2 = 0; +} + +void ParabolicRamp1D::SetLinear(Real _x0, Real _x1, Real t) { + PARABOLIC_RAMP_ASSERT(t > 0); + x0 = _x0; + x1 = _x1; + v = dx0 = dx1 = (_x1 - _x0) / t; + a1 = a2 = 0; + tswitch1 = 0; + tswitch2 = t; + ttotal = t; +} + +Real ParabolicRamp1D::Evaluate(Real t) const { + Real tmT = t - ttotal; + if (t < tswitch1) return x0 + 0.5 * a1 * t * t + dx0 * t; + else if (t < tswitch2) { + Real xswitch = x0 + 0.5 * a1 * tswitch1 * tswitch1 + dx0 * tswitch1; + return xswitch + (t - tswitch1) * v; + } else return x1 + 0.5 * a2 * tmT * tmT + dx1 * tmT; +} + +Real ParabolicRamp1D::Derivative(Real t) const { + if (t < tswitch1) return a1 * t + dx0; + else if (t < tswitch2) return v; + else { + Real tmT = t - ttotal; + return a2 * tmT + dx1; + } +} + +Real ParabolicRamp1D::Accel(Real t) const { + if (t < tswitch1) return a1; + else if (t < tswitch2) return 0; + else return a2; +} + +bool ParabolicRamp1D::SolveMinAccel(Real endTime, Real vmax) { + ParabolicRamp p; + PPRamp pp; + PLPRamp plp; + p.x0 = pp.x0 = plp.x0 = x0; + p.x1 = pp.x1 = plp.x1 = x1; + p.dx0 = pp.dx0 = plp.dx0 = dx0; + p.dx1 = pp.dx1 = plp.dx1 = dx1; + bool pres = p.SolveFixedTime(endTime); + bool ppres = pp.SolveMinAccel(endTime); + bool plpres = false; + if (!IsInf(vmax)) + plpres = plp.SolveMinAccel(endTime, vmax); + //cout<<"PP a: "<= 1) + PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinAccel: No ramp equation was successful.\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("end time %g, vmax = %g\n", endTime, vmax); + + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + PARABOLIC_RAMP_PERROR("p.a = %g, max vel=%g, end x=%g, end dx=%g\n", + p.a, + p.MaxVelocity(), + p.Evaluate(endTime), + p.Derivative(endTime)); + PARABOLIC_RAMP_PERROR("pp.a = %g, max vel=%g\n", pp.a, pp.MaxVelocity()); + PARABOLIC_RAMP_PERROR("plp.a = %g, v=%g\n", plp.a, plp.v); + + Real switch1, switch2; + Real apn = pp.CalcMinAccel(endTime, 1.0, switch1); + Real anp = pp.CalcMinAccel(endTime, -1.0, switch2); + PARABOLIC_RAMP_PERROR("PP Calcuations: +: %g %g, -: %g %g\n", apn, switch1, anp, switch2); + } + if (gErrorSave) SaveRamp("Ramp_SolveMinAccel_failure.dat", x0, dx0, x1, dx1, -1, vmax, endTime); + if (gErrorGetchar) getchar(); + } + return false; + } + if (gValidityCheckLevel >= 2) { + PARABOLIC_RAMP_ASSERT(ttotal == endTime); + if (!IsValid()) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("Invalid min-accel!\n"); + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("end time %g, vmax = %g\n", endTime, vmax); + + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + PARABOLIC_RAMP_PERROR("p.a = %g, max vel=%g\n", p.a, p.MaxVelocity()); + PARABOLIC_RAMP_PERROR("pp.a = %g, max vel=%g\n", pp.a, pp.MaxVelocity()); + PARABOLIC_RAMP_PERROR("plp.a = %g, v=%g\n", plp.a, plp.v); + } + if (gErrorGetchar) getchar(); + return false; + } + } + return true; +} + +bool ParabolicRamp1D::SolveMinTime(Real amax, Real vmax) { + ParabolicRamp p; + PPRamp pp; + PLPRamp plp; + p.x0 = pp.x0 = plp.x0 = x0; + p.x1 = pp.x1 = plp.x1 = x1; + p.dx0 = pp.dx0 = plp.dx0 = dx0; + p.dx1 = pp.dx1 = plp.dx1 = dx1; + bool pres = p.Solve(amax); + bool ppres = pp.SolveMinTime(amax); + bool plpres = false; + if (!IsInf(vmax)) + plpres = plp.SolveMinTime(amax, vmax); + //cout<<"P time: "<= 1) + PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime: No solution found!\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n", vmax, amax); + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + if (pres) + PARABOLIC_RAMP_PERROR(" P a=%g, ttotal=%g\n", p.a, p.ttotal); + if (ppres) + PARABOLIC_RAMP_PERROR(" PP a=%g, tswitch=%g, ttotal=%g\n", pp.a, pp.tswitch, pp.ttotal); + if (plpres) + PARABOLIC_RAMP_PERROR(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n", + pp.a, + plp.tswitch1, + plp.tswitch2, + plp.ttotal); + PARABOLIC_RAMP_PERROR("\n"); + } + if (gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, vmax, -1); + a1 = a2 = v = 0; + tswitch1 = tswitch2 = ttotal = -1; + return false; + } + a2 = -a1; + //cout<<"switch time 1: "<= 2) { + if (!IsValid()) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("Solved for invalid min-time path!\n"); + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n", vmax, amax); + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + } + if (gErrorGetchar) getchar(); + return false; + } + } + return true; +} + +bool ParabolicRamp1D::SolveMinTime2(Real amax, Real vmax, Real tLowerBound) { + ParabolicRamp p; + PPRamp pp; + PLPRamp plp; + p.x0 = pp.x0 = plp.x0 = x0; + p.x1 = pp.x1 = plp.x1 = x1; + p.dx0 = pp.dx0 = plp.dx0 = dx0; + p.dx1 = pp.dx1 = plp.dx1 = dx1; + bool pres = p.Solve(amax); + bool ppres = pp.SolveMinTime2(amax, tLowerBound); + bool plpres = false; + if (!IsInf(vmax)) + plpres = plp.SolveMinTime2(amax, vmax, tLowerBound); + //cout<<"P time: "<= tLowerBound) { + if (Abs(p.a) <= amax) { + a1 = p.a; + v = 0; + tswitch1 = tswitch2 = p.ttotal; + ttotal = p.ttotal; + } else { + //double check + p.a = Sign(p.a) * amax; + if (FuzzyEquals(p.Evaluate(p.ttotal), x1, EpsilonX) && FuzzyEquals(p.Derivative(p.ttotal), dx1, EpsilonV)) { + a1 = p.a; + v = 0; + tswitch1 = tswitch2 = p.ttotal; + ttotal = p.ttotal; + } + } + } + if (ppres && Abs(pp.MaxVelocity()) <= vmax && pp.ttotal < ttotal) { + a1 = pp.a; + v = 0; + tswitch1 = tswitch2 = pp.tswitch; + ttotal = pp.ttotal; + } + if (plpres && plp.ttotal < ttotal) { + a1 = plp.a; + v = plp.v; + tswitch1 = plp.tswitch1; + tswitch2 = plp.tswitch2; + ttotal = plp.ttotal; + } + if (IsInf(ttotal)) { + if (!gMinTime2Quiet) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime2: No solution found!\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g, tmax = %g\n", vmax, amax, tLowerBound); + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + if (pres) + PARABOLIC_RAMP_PERROR(" P a=%g, ttotal=%g\n", p.a, p.ttotal); + if (ppres) + PARABOLIC_RAMP_PERROR(" PP a=%g, tswitch=%g, ttotal=%g\n", pp.a, pp.tswitch, pp.ttotal); + if (plpres) + PARABOLIC_RAMP_PERROR(" PLP a=%g, tswitch=%g, %g, ttotal=%g\n", + pp.a, + plp.tswitch1, + plp.tswitch2, + plp.ttotal); + ppres = pp.SolveMinTime(amax); + plpres = plp.SolveMinTime(amax, vmax); + PARABOLIC_RAMP_PERROR("unconstrained PP (%d): %g, PLP (%d): %g\n", + (int) ppres, + pp.ttotal, + (int) plpres, + plp.ttotal); + PARABOLIC_RAMP_PERROR("\n"); + } + if (gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, vmax, tLowerBound); + } + a1 = a2 = v = 0; + tswitch1 = tswitch2 = ttotal = -1; + return false; + } + a2 = -a1; + //cout<<"switch time 1: "<= 2) { + if (!IsValid()) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("ParabolicRamp1D::SolveMinTime: Failure to find valid path!\n"); + PARABOLIC_RAMP_PERROR("x0=%g, x1=%g, dx0=%g, dx1=%g\n", x0, x1, dx0, dx1); + PARABOLIC_RAMP_PERROR("vmax = %g, amax = %g\n", vmax, amax); + PARABOLIC_RAMP_PERROR("P=%d, PP=%d, PLP=%d\n", (int) pres, (int) ppres, (int) plpres); + if (gErrorSave) SaveRamp("Ramp_SolveMinTime_failure.dat", x0, dx0, x1, dx1, amax, vmax, tLowerBound); + PARABOLIC_RAMP_PERROR("\n"); + if (gErrorGetchar) getchar(); + } + return false; + } + PARABOLIC_RAMP_ASSERT(ttotal >= tLowerBound); + } + return true; +} + +void ParabolicRamp1D::SolveBraking(Real amax) { + tswitch1 = 0; + tswitch2 = 0; + a1 = Sign(dx0) * amax; + v = 0; + a2 = -Sign(dx0) * amax; + ttotal = Abs(dx0) / amax; + x1 = x0 + dx0 * ttotal + 0.5 * Sqr(ttotal) * a2; + dx1 = 0; + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); +} + +void ParabolicRamp1D::Dilate(Real timeScale) { + tswitch1 *= timeScale; + tswitch2 *= timeScale; + ttotal *= timeScale; + a1 *= 1.0 / Sqr(timeScale); + a2 *= 1.0 / Sqr(timeScale); + v *= 1.0 / timeScale; +} + +void ParabolicRamp1D::TrimFront(Real tcut) { + if (tcut > ttotal) { + PARABOLIC_RAMP_PERROR("Hmm... want to trim front of curve at time %g, end time %g\n", tcut, ttotal); + } + PARABOLIC_RAMP_ASSERT(tcut <= ttotal); + x0 = Evaluate(tcut); + dx0 = Derivative(tcut); + ttotal -= tcut; + tswitch1 -= tcut; + tswitch2 -= tcut; + if (tswitch1 < 0) tswitch1 = 0; + if (tswitch2 < 0) tswitch2 = 0; + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); + +} + +void ParabolicRamp1D::TrimBack(Real tcut) { + x1 = Evaluate(ttotal - tcut); + dx1 = Derivative(ttotal - tcut); + ttotal -= tcut; + tswitch1 = Min(tswitch1, ttotal); + tswitch2 = Min(tswitch2, ttotal); + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); +} + +void ParabolicRamp1D::Bounds(Real &xmin, Real &xmax) const { + Bounds(0, ttotal, xmin, xmax); +} + +void ParabolicRamp1D::Bounds(Real ta, Real tb, Real &xmin, Real &xmax) const { + if (ta > tb) { //orient the interval + return Bounds(tb, ta, xmin, xmax); + } + if (ta < 0) ta = 0; + if (tb <= 0) { + xmin = xmax = x0; + return; + } + if (tb > ttotal) tb = ttotal; + if (ta >= ttotal) { + xmin = xmax = x1; + return; + } + + xmin = Evaluate(ta); + xmax = Evaluate(tb); + + if (xmin > xmax) Swap(xmin, xmax); + + Real tflip1 = 0, tflip2 = 0; + if (ta < tswitch1) { + //x' = a1*t + v0 = 0 => t = -v0/a1 + tflip1 = -dx0 / a1; + if (tflip1 > tswitch1) tflip1 = 0; + } + if (tb > tswitch2) { + //x' = a2*(T-t) + v1 = 0 => (T-t) = v1/a2 + tflip2 = ttotal - dx1 / a2; + if (tflip2 < tswitch2) tflip2 = 0; + } + if (ta < tflip1 && tb > tflip1) { + Real xflip = Evaluate(tflip1); + if (xflip < xmin) xmin = xflip; + else if (xflip > xmax) xmax = xflip; + } + if (ta < tflip2 && tb > tflip2) { + Real xflip = Evaluate(tflip2); + if (xflip < xmin) xmin = xflip; + else if (xflip > xmax) xmax = xflip; + } +} + +void ParabolicRamp1D::DerivBounds(Real &vmin, Real &vmax) const { + DerivBounds(0, ttotal, vmin, vmax); +} + +void ParabolicRamp1D::DerivBounds(Real ta, Real tb, Real &vmin, Real &vmax) const { + if (ta > tb) { //orient the interval + return DerivBounds(tb, ta, vmin, vmax); + } + if (ta < 0) ta = 0; + if (tb <= 0) { + vmin = vmax = dx0; + return; + } + if (tb > ttotal) tb = ttotal; + if (ta >= ttotal) { + vmin = vmax = dx1; + return; + } + + vmin = Derivative(ta); + vmax = Derivative(tb); + if (vmin > vmax) Swap(vmin, vmax); + + if (tswitch2 > tswitch1) { //consider linear part + if (ta < tswitch2 && tb > tswitch1) { + vmin = Min(vmin, v); + vmax = Min(vmax, v); + } + } else if (ta < tswitch1 && tb > tswitch1) { //PP ramp + //compute bending + Real vbend = dx0 + tswitch1 * a1; + if (vbend < vmin) vmin = vbend; + else if (vbend > vmax) vmax = vbend; + vbend = dx1 + (tswitch2 - ttotal) * a2; + if (vbend < vmin) vmin = vbend; + else if (vbend > vmax) vmax = vbend; + } +} + +bool ParabolicRamp1D::IsValid() const { + if (tswitch1 < 0 || tswitch2 < tswitch1 || ttotal < tswitch2) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Ramp has invalid timing %g %g %g\n", tswitch1, tswitch2, ttotal); + return false; + } + + Real t2mT = tswitch2 - ttotal; + if (tswitch1 != tswitch2) { + if (!FuzzyEquals(a1 * tswitch1 + dx0, v, EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 1 speed: %g vs %g\n", a1 * tswitch1 + dx0, v); + return false; + } + if (!FuzzyEquals(a2 * t2mT + dx1, v, EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 2 speed: %g vs %g\n", a2 * t2mT + dx1, v); + return false; + } + } + //check switch2 + Real xswitch = x0 + 0.5 * a1 * Sqr(tswitch1) + dx0 * tswitch1; + Real xswitch2 = xswitch + (tswitch2 - tswitch1) * v; + if (!FuzzyEquals(xswitch2, x1 + 0.5 * a2 * Sqr(t2mT) + dx1 * t2mT, EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Ramp has incorrect switch 2 position: %g vs %g\n", + xswitch2, + x1 + 0.5 * a2 * Sqr(t2mT) + dx1 * t2mT); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Ramp %g,%g -> %g,%g\n", x0, dx0, x1, dx1); + PARABOLIC_RAMP_PERROR("Acceleration %g, vel %g, deceleration %g\n", a1, v, a2); + PARABOLIC_RAMP_PERROR("Switch times %g %g %g\n", tswitch1, tswitch2, ttotal); + } + return false; + } + return true; +} + +ParabolicRampND::ParabolicRampND() + : blendAttempts(-1) { +} + +void ParabolicRampND::SetConstant(const Vector &x, Real t) { + x0 = x1 = x; + dx0.resize(x.size()); + dx1.resize(x.size()); + fill(dx0.begin(), dx0.end(), 0); + fill(dx1.begin(), dx1.end(), 0); + endTime = t; + ramps.resize(x.size()); + for (std::size_t i = 0; i < x.size(); i++) + ramps[i].SetConstant(x[i], t); +} + +void ParabolicRampND::SetLinear(const Vector &_x0, const Vector &_x1, Real t) { + PARABOLIC_RAMP_ASSERT(_x0.size() == _x1.size()); + PARABOLIC_RAMP_ASSERT(t > 0); + x0 = _x0; + x1 = _x1; + dx0.resize(_x1.size()); + for (std::size_t i = 0; i < _x1.size(); i++) + dx0[i] = (_x1[i] - _x0[i]) / t; + dx1 = dx0; + endTime = t; + ramps.resize(_x0.size()); + for (std::size_t i = 0; i < _x0.size(); i++) + ramps[i].SetLinear(_x0[i], _x1[i], t); +} + +bool ParabolicRampND::SolveMinTimeLinear(const Vector &amax, const Vector &vmax) { + PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == amax.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + endTime = 0; + ramps.resize(x0.size()); + ParabolicRamp1D sramp; + sramp.x0 = 0; + sramp.x1 = 1; + sramp.dx0 = 0; + sramp.dx1 = 0; + Real svmax = Inf, samax = Inf; + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x0[i]; + ramps[i].x1 = x1[i]; + ramps[i].dx0 = dx0[i]; + ramps[i].dx1 = dx1[i]; + if (vmax[i] == 0 || amax[i] == 0) { + if (!FuzzyEquals(x0[i], x1[i], EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n", + i, + vmax[i], + amax[i], + x0[i], + x1[i]); + return false; + } + if (!FuzzyEquals(dx0[i], dx1[i], EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n", + i, + vmax[i], + amax[i], + dx0[i], + dx1[i]); + return false; + } + ramps[i].tswitch1 = ramps[i].tswitch2 = ramps[i].ttotal = 0; + ramps[i].a1 = ramps[i].a1 = ramps[i].v = 0; + continue; + } + if (vmax[i] < svmax * Abs(x1[i] - x0[i])) + svmax = vmax[i] / Abs(x1[i] - x0[i]); + if (amax[i] < samax * Abs(x1[i] - x0[i])) + samax = amax[i] / Abs(x1[i] - x0[i]); + } + + if (IsInf(svmax) && IsInf(samax)) { + //must have equal start/end state + SetConstant(x0); + return true; + } + + bool res = sramp.SolveMinTime(samax, svmax); + if (!res) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Warning in straight-line parameter solve\n"); + if (gErrorGetchar) getchar(); + return false; + } + + endTime = sramp.ttotal; + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].v = svmax * (x1[i] - x0[i]); + ramps[i].a1 = samax * (x1[i] - x0[i]); + ramps[i].a2 = -samax * (x1[i] - x0[i]); + ramps[i].tswitch1 = sramp.tswitch1; + ramps[i].tswitch2 = sramp.tswitch2; + ramps[i].ttotal = endTime; + if (gValidityCheckLevel >= 2) { + if (!ramps[i].IsValid()) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Warning, error in straight-line path formula\n"); + if (gVerbose >= 2) { + for (std::size_t j = 0; j < dx0.size(); j++) + PARABOLIC_RAMP_PERROR("%g ", dx0[j]); + for (std::size_t j = 0; j < dx1.size(); j++) + PARABOLIC_RAMP_PERROR("%g ", dx1[j]); + } + if (gErrorGetchar) getchar(); + } + } + + //correct for small numerical errors + if (Abs(ramps[i].v) > vmax[i]) { + if (Abs(ramps[i].v) > vmax[i] + EpsilonV) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n"); + PARABOLIC_RAMP_PERROR("velocity |%g|>%g\n", ramps[i].v, vmax[i]); + } + if (gErrorGetchar) getchar(); + } else ramps[i].v = Sign(ramps[i].v) * vmax[i]; + } + if (Abs(ramps[i].a1) > amax[i]) { + if (Abs(ramps[i].a1) > amax[i] + EpsilonA) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n"); + PARABOLIC_RAMP_PERROR("accel |%g|>%g\n", ramps[i].a1, amax[i]); + } + if (gErrorGetchar) getchar(); + } else ramps[i].a1 = Sign(ramps[i].a1) * amax[i]; + } + if (Abs(ramps[i].a2) > amax[i]) { + if (Abs(ramps[i].a2) > amax[i] + EpsilonA) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("Warning, numerical error in straight-line formula?\n"); + PARABOLIC_RAMP_PERROR("accel |%g|>%g\n", ramps[i].a2, amax[i]); + } + if (gErrorGetchar) getchar(); + } else ramps[i].a2 = Sign(ramps[i].a2) * amax[i]; + } + } + return true; +} + +bool ParabolicRampND::SolveMinTime(const Vector &amax, const Vector &vmax) { + PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == amax.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + endTime = 0; + ramps.resize(x0.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x0[i]; + ramps[i].x1 = x1[i]; + ramps[i].dx0 = dx0[i]; + ramps[i].dx1 = dx1[i]; + if (vmax[i] == 0 || amax[i] == 0) { + if (!FuzzyEquals(x0[i], x1[i], EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n", + i, + vmax[i], + amax[i], + x0[i], + x1[i]); + return false; + } + if (!FuzzyEquals(dx0[i], dx1[i], EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n", + i, + vmax[i], + amax[i], + dx0[i], + dx1[i]); + return false; + } + ramps[i].tswitch1 = ramps[i].tswitch2 = ramps[i].ttotal = 0; + ramps[i].a1 = ramps[i].a2 = ramps[i].v = 0; + continue; + } + if (!ramps[i].SolveMinTime(amax[i], vmax[i])) return false; + if (ramps[i].ttotal > endTime) endTime = ramps[i].ttotal; + } + //now we have a candidate end time -- repeat looking through solutions + //until we have solved all ramps + while (true) { + bool solved = true; + for (std::size_t i = 0; i < ramps.size(); i++) { + if (ramps[i].ttotal == endTime) continue; + if (vmax[i] == 0 || amax[i] == 0) { + ramps[i].ttotal = endTime; + continue; + } + if (!ramps[i].SolveMinAccel(endTime, vmax[i])) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Failed solving min accel for joint %d\n", i); + if (gVerbose >= 2) { + ramps[i].SolveMinTime(amax[i], vmax[i]); + PARABOLIC_RAMP_PERROR("its min time is %g\n", ramps[i].ttotal); + if (ramps[i].tswitch1 == ramps[i].tswitch2) + PARABOLIC_RAMP_PERROR("its type is PP\n"); + else if (Abs(ramps[i].v) == vmax[i]) + PARABOLIC_RAMP_PERROR("its type is PLP (vmax)\n"); + else + PARABOLIC_RAMP_PERROR("its type is PLP (v=%g %%)\n", ramps[i].v / vmax[i]); + } + if (gErrorSave) + SaveRamp("ParabolicRampND_SolveMinAccel_failure.dat", + ramps[i].x0, + ramps[i].dx0, + ramps[i].x1, + ramps[i].dx1, + -1, + vmax[i], + endTime); + if (gErrorSave) { + PARABOLIC_RAMP_PERROR("Saving to failed_ramps.txt\n"); + FILE *f = fopen("failed_ramps.txt", "w+"); + fprintf(f, "MinAccel T=%g, vmax=%g\n", endTime, vmax[i]); + fprintf(f, "x0=%g, dx0=%g\n", ramps[i].x0, ramps[i].dx0); + fprintf(f, "x1=%g, dx1=%g\n", ramps[i].x1, ramps[i].dx1); + fprintf(f, + "MinTime solution v=%g, t1=%g, t2=%g, T=%g\n", + ramps[i].v, + ramps[i].tswitch1, + ramps[i].tswitch2, + ramps[i].ttotal); + fprintf(f, "\n"); + fclose(f); + } + return false; + } + if (Abs(ramps[i].a1) > amax[i] || Abs(ramps[i].a2) > amax[i] || Abs(ramps[i].v) > vmax[i]) { + bool res = ramps[i].SolveMinTime2(amax[i], vmax[i], endTime); + if (!res) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Couldn't solve min-time with lower bound!\n"); + if (gErrorGetchar) getchar(); + return false; + } + PARABOLIC_RAMP_ASSERT(ramps[i].ttotal > endTime); + endTime = ramps[i].ttotal; + solved = false; + break; //go back and re-solve + } + PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a1) <= amax[i] + EpsilonA); + PARABOLIC_RAMP_ASSERT(Abs(ramps[i].a2) <= amax[i] + EpsilonA); + PARABOLIC_RAMP_ASSERT(Abs(ramps[i].v) <= vmax[i] + EpsilonV); + PARABOLIC_RAMP_ASSERT(ramps[i].ttotal == endTime); + } + //done + if (solved) break; + } + return true; +} + +bool ParabolicRampND::SolveMinAccel(const Vector &vmax, Real time) { + PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + endTime = time; + ramps.resize(x0.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x0[i]; + ramps[i].x1 = x1[i]; + ramps[i].dx0 = dx0[i]; + ramps[i].dx1 = dx1[i]; + if (vmax[i] == 0) { + PARABOLIC_RAMP_ASSERT(FuzzyEquals(x0[i], x1[i], EpsilonX)); + PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0[i], dx1[i], EpsilonV)); + ramps[i].tswitch1 = ramps[i].tswitch2 = ramps[i].ttotal = 0; + ramps[i].a1 = ramps[i].a2 = ramps[i].v = 0; + continue; + } + if (!ramps[i].SolveMinAccel(endTime, vmax[i])) { + return false; + } + } + return true; +} + +bool ParabolicRampND::SolveMinAccelLinear(const Vector &vmax, Real time) { + PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == dx1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + endTime = 0; + ramps.resize(x0.size()); + ParabolicRamp1D sramp; + sramp.x0 = 0; + sramp.x1 = 1; + sramp.dx0 = 0; + sramp.dx1 = 0; + Real svmax = Inf; + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].x0 = x0[i]; + ramps[i].x1 = x1[i]; + ramps[i].dx0 = dx0[i]; + ramps[i].dx1 = dx1[i]; + if (vmax[i] == 0) { + if (!FuzzyEquals(x0[i], x1[i], EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, X0 != X1 (%g != %g)\n", i, vmax[i], x0[i], x1[i]); + return false; + } + if (!FuzzyEquals(dx0[i], dx1[i], EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, DX0 != DX1 (%g != %g)\n", i, vmax[i], dx0[i], dx1[i]); + return false; + } + ramps[i].tswitch1 = ramps[i].tswitch2 = ramps[i].ttotal = 0; + ramps[i].a1 = ramps[i].a1 = ramps[i].v = 0; + continue; + } + if (vmax[i] < svmax * Abs(x1[i] - x0[i])) + svmax = vmax[i] / Abs(x1[i] - x0[i]); + } + + if (IsInf(svmax)) { + //must have equal start/end state + SetConstant(x0); + return true; + } + + bool res = sramp.SolveMinAccel(svmax, time); + if (!res) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Warning in straight-line parameter solve\n"); + if (gErrorGetchar) getchar(); + return false; + } + + endTime = sramp.ttotal; + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].v = sramp.v * (x1[i] - x0[i]); + ramps[i].a1 = sramp.a1 * (x1[i] - x0[i]); + ramps[i].a2 = sramp.a2 * (x1[i] - x0[i]); + ramps[i].tswitch1 = sramp.tswitch1; + ramps[i].tswitch2 = sramp.tswitch2; + ramps[i].ttotal = endTime; + if (gValidityCheckLevel >= 2) { + if (!ramps[i].IsValid()) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Warning, error in straight-line path formula\n"); + if (gErrorGetchar) getchar(); + res = false; + } + } + } + return res; +} + +void ParabolicRampND::SolveBraking(const Vector &amax) { + PARABOLIC_RAMP_ASSERT(x0.size() == dx0.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == amax.size()); + x1.resize(x0.size()); + dx1.resize(x0.size()); + endTime = 0; + ramps.resize(x0.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + if (amax[i] == 0) { + PARABOLIC_RAMP_ASSERT(FuzzyEquals(dx0[i], 0.0, EpsilonV)); + ramps[i].SetConstant(0); + continue; + } + ramps[i].x0 = x0[i]; + ramps[i].dx0 = dx0[i]; + ramps[i].SolveBraking(amax[i]); + } + for (std::size_t i = 0; i < ramps.size(); i++) + endTime = Max(endTime, ramps[i].ttotal); + for (std::size_t i = 0; i < ramps.size(); i++) { + if (amax[i] != 0 && ramps[i].ttotal != endTime) { + //scale ramp acceleration to meet endTimeMax + ramps[i].ttotal = endTime; + //y(t) = x0 + t*dx0 + 1/2 t^2 a + //y'(T) = dx0 + T a = 0 + ramps[i].a2 = -dx0[i] / endTime; + ramps[i].a1 = -ramps[i].a2; + ramps[i].x1 = ramps[i].x0 + endTime * ramps[i].dx0 + 0.5 * Sqr(endTime) * ramps[i].a2; + } + x1[i] = ramps[i].x1; + dx1[i] = 0; + } + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); +} + +void ParabolicRampND::Evaluate(Real t, Vector &x) const { + x.resize(ramps.size()); + + for (std::size_t j = 0; j < ramps.size(); j++) + x[j] = ramps[j].Evaluate(t); +} + +void ParabolicRampND::Derivative(Real t, Vector &x) const { + x.resize(ramps.size()); + for (std::size_t j = 0; j < ramps.size(); j++) + x[j] = ramps[j].Derivative(t); +} + +void ParabolicRampND::Accel(Real t, Vector &x) const { + x.resize(ramps.size()); + for (std::size_t j = 0; j < ramps.size(); j++) + x[j] = ramps[j].Accel(t); +} + +void ParabolicRampND::Output(Real dt, std::vector &path) const { + PARABOLIC_RAMP_ASSERT(!ramps.empty()); + int size = (int) ceil(endTime / dt) + 1; + path.resize(size); + if (size == 1) { + path[0].resize(ramps.size()); + for (std::size_t j = 0; j < ramps.size(); j++) + path[0][j] = ramps[j].x0; + return; + } + for (int i = 0; i < size; i++) { + Real t = endTime * Real(i) / Real(size - 1); + path[i].resize(ramps.size()); + for (std::size_t j = 0; j < ramps.size(); j++) + path[i][j] = ramps[j].Evaluate(t); + } + + /* + path[0].resize(ramps.size()); + for(std::size_t j=0;j= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); +} + +void ParabolicRampND::TrimBack(Real tcut) { + for (std::size_t i = 0; i < ramps.size(); i++) + PARABOLIC_RAMP_ASSERT(endTime == ramps[i].ttotal); + PARABOLIC_RAMP_ASSERT(tcut <= endTime); + Evaluate(endTime - tcut, x1); + Derivative(endTime - tcut, dx1); + endTime -= tcut; + for (std::size_t i = 0; i < ramps.size(); i++) + ramps[i].TrimBack(tcut); + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(IsValid()); +} + +void ParabolicRampND::Bounds(Vector &xmin, Vector &xmax) const { + xmin.resize(ramps.size()); + xmax.resize(ramps.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].Bounds(xmin[i], xmax[i]); + } +} + +void ParabolicRampND::Bounds(Real ta, Real tb, Vector &xmin, Vector &xmax) const { + xmin.resize(ramps.size()); + xmax.resize(ramps.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].Bounds(ta, tb, xmin[i], xmax[i]); + } +} + +void ParabolicRampND::DerivBounds(Vector &vmin, Vector &vmax) const { + vmin.resize(ramps.size()); + vmax.resize(ramps.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].DerivBounds(vmin[i], vmax[i]); + } +} + +void ParabolicRampND::DerivBounds(Real ta, Real tb, Vector &vmin, Vector &vmax) const { + vmin.resize(ramps.size()); + vmax.resize(ramps.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].DerivBounds(ta, tb, vmin[i], vmax[i]); + } +} + +bool ParabolicRampND::IsValid() const { + if (endTime < 0) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): endTime is negative\n"); + return false; + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (!ramps[i].IsValid()) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d is invalid\n", i); + return false; + } + if (!FuzzyEquals(ramps[i].ttotal, endTime, EpsilonT)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR( + "ParabolicRampND::IsValid(): element %d has different end time %g != %g\n", + i, + ramps[i].ttotal, + endTime); + return false; + } + if (!FuzzyEquals(ramps[i].x0, x0[i], EpsilonX)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different x0 %g != %g\n", + i, + ramps[i].x0, + x0[i]); + return false; + } + if (!FuzzyEquals(ramps[i].x1, x1[i], EpsilonX)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different x1 %g != %g\n", + i, + ramps[i].x1, + x1[i]); + return false; + } + if (!FuzzyEquals(ramps[i].dx0, dx0[i], EpsilonV)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different dx0 %g != %g\n", + i, + ramps[i].dx0, + dx0[i]); + return false; + } + if (!FuzzyEquals(ramps[i].dx1, dx1[i], EpsilonV)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("ParabolicRampND::IsValid(): element %d has different dx1 %g != %g\n", + i, + ramps[i].dx1, + dx1[i]); + return false; + } + } + return true; +} + +bool SolveMinTimeBounded(Real x0, + Real v0, + Real x1, + Real v1, + Real amax, + Real vmax, + Real xmin, + Real xmax, + ParabolicRamp1D &ramp) { + PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax); + ramp.x0 = x0; + ramp.dx0 = v0; + ramp.x1 = x1; + ramp.dx1 = v1; + if (!ramp.SolveMinTime(amax, vmax)) return false; + Real bmin, bmax; + ramp.Bounds(bmin, bmax); + if (bmin < xmin || bmax > xmax) return false; + return true; +} + +inline Real BrakeTime(Real x, Real v, Real xbound) { + return 2.0 * (xbound - x) / v; +} + +inline Real BrakeAccel(Real x, Real v, Real xbound) { + Real tb = BrakeTime(x, v, xbound); + if (FuzzyEquals(tb, 0.0, EpsilonT)) return 0; + return -v / tb; +} + +bool SolveMinAccelBounded(Real x0, + Real v0, + Real x1, + Real v1, + Real endTime, + Real vmax, + Real xmin, + Real xmax, + std::vector &ramps) { + PARABOLIC_RAMP_ASSERT(x0 >= xmin && x0 <= xmax && x1 >= xmin && x1 <= xmax); + ParabolicRamp1D ramp; + ramp.x0 = x0; + ramp.dx0 = v0; + ramp.x1 = x1; + ramp.dx1 = v1; + if (!ramp.SolveMinAccel(endTime, vmax)) return false; + Real bmin, bmax; + ramp.Bounds(bmin, bmax); + if (bmin >= xmin && bmax <= xmax) { + ramps.resize(1); + ramps[0] = ramp; + return true; + } + + //not within bounds, do the more complex procedure + ramps.resize(0); + vector temp; + //Look at the IV cases + Real bt0 = Inf, bt1 = Inf; + Real ba0 = Inf, ba1 = Inf; + Real bx0 = Inf, bx1 = Inf; + if (v0 > 0) { + bt0 = BrakeTime(x0, v0, xmax); + bx0 = xmax; + ba0 = BrakeAccel(x0, v0, xmax); + } else if (v0 < 0) { + bt0 = BrakeTime(x0, v0, xmin); + bx0 = xmin; + ba0 = BrakeAccel(x0, v0, xmin); + } + if (v1 < 0) { + bt1 = BrakeTime(x1, -v1, xmax); + bx1 = xmax; + ba1 = BrakeAccel(x1, -v1, xmax); + } else if (v1 > 0) { + bt1 = BrakeTime(x1, -v1, xmin); + bx1 = xmin; + ba1 = BrakeAccel(x1, -v1, xmin); + } + Real amax = Inf; + //Explore types II and III, or II and IV depending on the side + //Type I path: no collision + //Type II path: touches one side instantaneously + // (IIa: first segment is braking, IIb: last segment is braking) + //Type III path: touches one side and remains there for some time + //Type IV path: hits both top and bottom + //consider braking to side, then solving to x1,v1 + if (bt0 < endTime && Abs(ba0) < amax) { + //type IIa + temp.resize(2); + temp[0].x0 = x0; + temp[0].dx0 = v0; + temp[0].x1 = bx0; + temp[0].dx1 = 0; + temp[0].a1 = ba0; + temp[0].v = 0; + temp[0].a2 = 0; + temp[0].tswitch1 = bt0; + temp[0].tswitch2 = bt0; + temp[0].ttotal = bt0; + temp[1].x0 = bx0; + temp[1].dx0 = 0; + temp[1].x1 = x1; + temp[1].dx1 = v1; + gMinAccelQuiet = true; + //first check is a quick reject + if (Abs(x1 - bx0) < (endTime - bt0) * vmax) { + if (temp[1].SolveMinAccel(endTime - bt0, vmax)) { + if (Max(Abs(temp[1].a1), Abs(temp[1].a2)) < amax) { + temp[1].Bounds(bmin, bmax); + if (bmin >= xmin && bmax <= xmax) { + //got a better path + ramps = temp; + amax = Max(Abs(ba0), Max(Abs(temp[1].a1), Abs(temp[1].a2))); + } + } + } + } + gMinAccelQuiet = false; + } + //consider reverse braking from x1,v1, then solving from x0,v0 + //consider braking to side, then solving to x1,v1 + if (bt1 < endTime && Abs(ba1) < amax) { + //type IIb + temp.resize(2); + temp[0].x0 = x0; + temp[0].dx0 = v0; + temp[0].x1 = bx1; + temp[0].dx1 = 0; + temp[1].x0 = bx1; + temp[1].dx0 = 0; + temp[1].x1 = x1; + temp[1].dx1 = v1; + temp[1].a1 = ba1; + temp[1].v = 0; + temp[1].a2 = 0; + temp[1].tswitch1 = bt1; + temp[1].tswitch2 = bt1; + temp[1].ttotal = bt1; + gMinAccelQuiet = true; + //first perform a quick reject + if (Abs(x0 - bx1) < (endTime - bt1) * vmax) { + if (temp[0].SolveMinAccel(endTime - bt1, vmax)) { + if (Max(Abs(temp[0].a1), Abs(temp[0].a2)) < amax) { + temp[0].Bounds(bmin, bmax); + if (bmin >= xmin && bmax <= xmax) { + //got a better path + ramps = temp; + amax = Max(Abs(ba1), Max(Abs(temp[0].a1), Abs(temp[0].a2))); + } + } + } + } + gMinAccelQuiet = false; + } + if (bx0 == bx1) { + //type III: braking to side, then continuing, then accelerating to x1 + if (bt0 + bt1 < endTime && Max(Abs(ba0), Abs(ba1)) < amax) { + temp.resize(1); + temp[0].x0 = x0; + temp[0].dx0 = v0; + temp[0].x1 = x1; + temp[0].dx1 = v1; + temp[0].a1 = ba0; + temp[0].v = 0; + temp[0].a2 = ba1; + temp[0].tswitch1 = bt0; + temp[0].tswitch2 = endTime - bt1; + temp[0].ttotal = endTime; + ramps = temp; + amax = Max(Abs(ba0), Abs(ba1)); + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(temp[0].IsValid()); + } + } else { + //type IV paths + if (bt0 + bt1 < endTime && Max(Abs(ba0), Abs(ba1)) < amax) { + //first segment brakes to one side, last segment brakes to the other + //first + temp.resize(3); + temp[0].x0 = x0; + temp[0].dx0 = v0; + temp[0].x1 = bx0; + temp[0].dx1 = 0; + temp[0].a1 = ba0; + temp[0].v = 0; + temp[0].a2 = 0; + temp[0].tswitch1 = bt0; + temp[0].tswitch2 = bt0; + temp[0].ttotal = bt0; + //last + temp[2].x0 = bx1; + temp[2].dx0 = 0; + temp[2].x1 = x1; + temp[2].dx1 = v1; + temp[2].a1 = ba1; + temp[2].v = 0; + temp[2].a2 = 0; + temp[2].tswitch1 = bt1; + temp[2].tswitch2 = bt1; + temp[2].ttotal = bt1; + //middle section + temp[1].x0 = bx0; + temp[1].dx0 = 0; + temp[1].x1 = bx1; + temp[1].dx1 = 0; + gMinAccelQuiet = true; + if (Abs(bx0 - bx1) < (endTime - bt0 - bt1) * vmax) { + if (temp[1].SolveMinAccel(endTime - bt0 - bt1, vmax)) { + temp[1].Bounds(bmin, bmax); + PARABOLIC_RAMP_ASSERT(bmin >= xmin && bmax <= xmax); + if (Max(Abs(temp[1].a1), Abs(temp[1].a2)) < amax) { + ramps = temp; + amax = Max(Max(Abs(temp[1].a1), Abs(temp[1].a2)), Max(Abs(ba0), Abs(ba1))); + } + } + } + gMinAccelQuiet = false; + } + } + if (ramps.empty()) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("SolveMinAccelBounded: Warning, can't find bounded trajectory?\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("x0 %g v0 %g, x1 %g v1 %g\n", x0, v0, x1, v1); + PARABOLIC_RAMP_PERROR("endTime %g, vmax %g\n", endTime, vmax); + PARABOLIC_RAMP_PERROR("x bounds [%g,%g]\n", xmin, xmax); + } + if (gErrorGetchar) getchar(); + return false; + } + if (gValidityCheckLevel >= 1) { + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].Bounds(bmin, bmax); + if (bmin < xmin - EpsilonX || bmax > xmax + EpsilonX) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("SolveMinAccelBounded: Warning, path exceeds bounds?\n"); + if (gVerbose >= 2) + PARABOLIC_RAMP_PERROR(" ramp[%d] bounds %g %g, limits %g %g\n", i, bmin, bmax, xmin, xmax); + if (gErrorGetchar) getchar(); + return false; + } + } + } + + PARABOLIC_RAMP_ASSERT(ramps.front().x0 == x0); + PARABOLIC_RAMP_ASSERT(ramps.front().dx0 == v0); + PARABOLIC_RAMP_ASSERT(ramps.back().x1 == x1); + PARABOLIC_RAMP_ASSERT(ramps.back().dx1 == v1); + double ttotal = 0; + for (std::size_t i = 0; i < ramps.size(); i++) + ttotal += ramps[i].ttotal; + if (!FuzzyEquals(ttotal, endTime, EpsilonT * 0.1)) { + if (gVerbose >= 1) PARABOLIC_RAMP_PERROR("SolveMinTimeBounded: Numerical timing error"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("Ramp times: "); + for (std::size_t i = 0; i < ramps.size(); i++) + PARABOLIC_RAMP_PERROR("%g ", ramps[i].ttotal); + PARABOLIC_RAMP_PERROR("\n"); + } + } + PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal, endTime, EpsilonT * 0.1)); + return true; +} + +Real SolveMinTimeBounded(const Vector &x0, const Vector &v0, const Vector &x1, const Vector &v1, + const Vector &amax, const Vector &vmax, const Vector &xmin, const Vector &xmax, + vector > &ramps) { + PARABOLIC_RAMP_ASSERT(x0.size() == v0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == v1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == amax.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + for (std::size_t i = 0; i < x0.size(); i++) { + PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]); + PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]); + PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]); + PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]); + } + Real endTime = 0; + ramps.resize(x0.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + ramps[i].resize(1); + ramps[i][0].x0 = x0[i]; + ramps[i][0].x1 = x1[i]; + ramps[i][0].dx0 = v0[i]; + ramps[i][0].dx1 = v1[i]; + if (vmax[i] == 0 || amax[i] == 0) { + if (!FuzzyEquals(x0[i], x1[i], EpsilonX)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, X0 != X1 (%g != %g)\n", + i, + vmax[i], + amax[i], + x0[i], + x1[i]); + return -1; + } + if (!FuzzyEquals(v0[i], v1[i], EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("index %d vmax = %g, amax = %g, DX0 != DX1 (%g != %g)\n", + i, + vmax[i], + amax[i], + v0[i], + v1[i]); + return -1; + } + ramps[i][0].tswitch1 = ramps[i][0].tswitch2 = ramps[i][0].ttotal = 0; + ramps[i][0].a1 = ramps[i][0].a2 = ramps[i][0].v = 0; + continue; + } + if (!ramps[i][0].SolveMinTime(amax[i], vmax[i])) return -1; + Real bmin, bmax; + ramps[i][0].Bounds(bmin, bmax); + if (bmin < xmin[i] || bmax > xmax[i]) return -1; + if (ramps[i][0].ttotal > endTime) endTime = ramps[i][0].ttotal; + } + //now we have a candidate end time -- repeat looking through solutions + //until we have solved all ramps + while (true) { + bool solved = true; + for (std::size_t i = 0; i < ramps.size(); i++) { + PARABOLIC_RAMP_ASSERT(ramps[i].size() > 0); + if (vmax[i] == 0 || amax[i] == 0) { + ramps[i][0].ttotal = endTime; + continue; + } + //already at maximum + Real ttotal = 0; + for (std::size_t j = 0; j < ramps[i].size(); j++) + ttotal += ramps[i][j].ttotal; + if (FuzzyEquals(ttotal, endTime, EpsilonT)) continue; + + //now solve minimum acceleration within bounds + if (!SolveMinAccelBounded(x0[i], v0[i], x1[i], v1[i], endTime, vmax[i], xmin[i], xmax[i], ramps[i])) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Failed solving bounded min accel for joint %d\n", i); + return -1; + } + //now check accel/velocity bounds + bool inVelBounds = true; + for (std::size_t j = 0; j < ramps[i].size(); j++) + if (Abs(ramps[i][j].a1) > amax[i] + EpsilonA || Abs(ramps[i][j].a2) > amax[i] + EpsilonA + || Abs(ramps[i][j].v) > vmax[i] + EpsilonV) { + //printf("Ramp %d entry %d accels: %g %g, vel %g\n",i,j,ramps[i][j].a1,ramps[i][j].a2,ramps[i][j].v); + inVelBounds = false; + break; + } + if (!inVelBounds) { + ramps[i].resize(1); + ramps[i][0].x0 = x0[i]; + ramps[i][0].x1 = x1[i]; + ramps[i][0].dx0 = v0[i]; + ramps[i][0].dx1 = v1[i]; + gMinTime2Quiet = true; + bool res = ramps[i][0].SolveMinTime2(amax[i], vmax[i], endTime); + gMinTime2Quiet = false; + if (!res) { + return -1; + } + Real bmin, bmax; + ramps[i][0].Bounds(bmin, bmax); + if (bmin < xmin[i] || bmax > xmax[i]) { + //printf("Couldn't solve min-time with lower bound while staying in bounds\n"); + //getchar(); + return -1; + } + + //revise total time + ttotal = 0; + for (std::size_t j = 0; j < ramps[i].size(); j++) + ttotal += ramps[i][j].ttotal; + PARABOLIC_RAMP_ASSERT(ttotal > endTime); + endTime = ttotal; + solved = false; + break; //go back and re-solve + } + ttotal = 0; + for (std::size_t j = 0; j < ramps[i].size(); j++) { + PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a1) <= amax[i] + EpsilonA); + PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].a2) <= amax[i] + EpsilonA); + PARABOLIC_RAMP_ASSERT(Abs(ramps[i][j].v) <= vmax[i] + EpsilonV); + ttotal += ramps[i][j].ttotal; + } + PARABOLIC_RAMP_ASSERT(FuzzyEquals(ttotal, endTime, EpsilonT * 0.1)); + } + //done + if (solved) break; + } + return endTime; +} + +bool SolveMinAccelBounded(const Vector &x0, const Vector &v0, const Vector &x1, const Vector &v1, + Real endTime, const Vector &vmax, const Vector &xmin, const Vector &xmax, + vector > &ramps) { + PARABOLIC_RAMP_ASSERT(x0.size() == v0.size()); + PARABOLIC_RAMP_ASSERT(x1.size() == v1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == x1.size()); + PARABOLIC_RAMP_ASSERT(x0.size() == vmax.size()); + for (std::size_t i = 0; i < x0.size(); i++) { + PARABOLIC_RAMP_ASSERT(x0[i] >= xmin[i] && x0[i] <= xmax[i]); + PARABOLIC_RAMP_ASSERT(x1[i] >= xmin[i] && x1[i] <= xmax[i]); + PARABOLIC_RAMP_ASSERT(Abs(v0[i]) <= vmax[i]); + PARABOLIC_RAMP_ASSERT(Abs(v1[i]) <= vmax[i]); + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (vmax[i] == 0) { + ramps[i].resize(1); + ramps[i][0].x0 = x0[i]; + ramps[i][0].x1 = x1[i]; + ramps[i][0].dx0 = v0[i]; + ramps[i][0].dx1 = v1[i]; + ramps[i][0].ttotal = endTime; + continue; + } + //now solve minimum acceleration within bounds + if (!SolveMinAccelBounded(x0[i], v0[i], x1[i], v1[i], endTime, vmax[i], xmin[i], xmax[i], ramps[i])) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("Failed solving bounded min accel for joint %d\n", i); + return false; + } + } + return true; +} + +void CombineRamps(const std::vector > &ramps, std::vector &ndramps) { + ndramps.resize(0); + vector::const_iterator> indices(ramps.size()); + for (std::size_t i = 0; i < ramps.size(); i++) { + PARABOLIC_RAMP_ASSERT(!ramps[i].empty()); + indices[i] = ramps[i].begin(); + } + vector timeOffsets(ramps.size(), 0); //start time of current index + Real t = 0; + while (true) { + //pick next ramp + Real tnext = Inf; + for (std::size_t i = 0; i < ramps.size(); i++) { + if (indices[i] != ramps[i].end()) + tnext = Min(tnext, timeOffsets[i] + indices[i]->ttotal); + } + if (IsInf(tnext)) break; //done + if (!(tnext > t || t == 0)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("CombineRamps: error finding next time step?\n"); + if (gVerbose >= 2) { + PARABOLIC_RAMP_PERROR("tnext = %g, t = %g, step = %d\n", tnext, t, ndramps.size()); + for (std::size_t k = 0; k < ramps.size(); k++) { + PARABOLIC_RAMP_PERROR("Ramp %d times: ", k); + Real ttotal = 0.0; + for (std::size_t j = 0; j < ramps[k].size(); j++) { + PARABOLIC_RAMP_PERROR("%g ", ramps[k][j].ttotal); + ttotal += ramps[k][j].ttotal; + } + PARABOLIC_RAMP_PERROR(", total %g\n", ttotal); + } + } + } + PARABOLIC_RAMP_ASSERT(tnext > t || t == 0); + if (tnext == 0) { + for (std::size_t i = 0; i < ramps.size(); i++) + PARABOLIC_RAMP_ASSERT(ramps[i].size() == 1); + } + + ndramps.resize(ndramps.size() + 1); + ParabolicRampND &ramp = ndramps.back(); + ramp.x0.resize(ramps.size()); + ramp.x1.resize(ramps.size()); + ramp.dx0.resize(ramps.size()); + ramp.dx1.resize(ramps.size()); + ramp.ramps.resize(ramps.size()); + ramp.endTime = tnext - t; + for (std::size_t i = 0; i < ramps.size(); i++) { + if (indices[i] != ramps[i].end()) { + ParabolicRamp1D iramp = *indices[i]; + if (indices[i] == --ramps[i].end() && FuzzyEquals(tnext - timeOffsets[i], indices[i]->ttotal, EpsilonT * 0.1)) { + //don't trim back + } else { + iramp.TrimBack((timeOffsets[i] - tnext) + indices[i]->ttotal); + } + iramp.TrimFront(t - timeOffsets[i]); + Real oldTotal = iramp.ttotal; + iramp.ttotal = ramp.endTime; + if (iramp.tswitch1 > iramp.ttotal) iramp.tswitch1 = iramp.ttotal; + if (iramp.tswitch2 > iramp.ttotal) iramp.tswitch2 = iramp.ttotal; + if (gValidityCheckLevel >= 2) { + if (!iramp.IsValid()) { + if (gVerbose >= 1) { + PARABOLIC_RAMP_PERROR("CombineRamps: Trimming caused ramp to become invalid\n"); + PARABOLIC_RAMP_PERROR("Old total time %g, new total time %g\n", oldTotal, iramp.ttotal); + } + } + PARABOLIC_RAMP_ASSERT(iramp.IsValid()); + } + ramp.ramps[i] = iramp; + ramp.x0[i] = iramp.x0; + ramp.dx0[i] = iramp.dx0; + ramp.x1[i] = iramp.x1; + ramp.dx1[i] = iramp.dx1; + if (FuzzyEquals(tnext, timeOffsets[i] + indices[i]->ttotal, EpsilonT * 0.1)) { + timeOffsets[i] = tnext; + indices[i]++; + } + PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime); + } else { + //after the last segment, propagate a constant off the last ramp + PARABOLIC_RAMP_ASSERT(!ramps[i].empty()); + ramp.x0[i] = ramps[i].back().x1; + ramp.dx0[i] = ramps[i].back().dx1; + //ramp.x1[i] = ramps[i].back().x1+ramps[i].back().dx1*(tnext-t); + ramp.x1[i] = ramps[i].back().x1; + ramp.dx1[i] = ramps[i].back().dx1; + if (!FuzzyEquals(ramps[i].back().dx1 * (tnext - t), 0.0, EpsilonV)) { + if (gVerbose >= 1) + PARABOLIC_RAMP_PERROR("CombineRamps: warning, propagating time %g distance %g off the back, vel %g\n", + (tnext - t), + ramps[i].back().dx1 * (tnext - t), + ramp.dx0[i]); + if (gVerbose >= 2) { + for (std::size_t k = 0; k < ramps.size(); k++) { + PARABOLIC_RAMP_PERROR("Ramp %d times: ", k); + Real ttotal = 0.0; + for (std::size_t j = 0; j < ramps[k].size(); j++) { + PARABOLIC_RAMP_PERROR("%g ", ramps[k][j].ttotal); + ttotal += ramps[k][j].ttotal; + } + PARABOLIC_RAMP_PERROR(", total %g\n", ttotal); + } + if (gErrorGetchar) getchar(); + } + } + //set the 1D ramp manually + ramp.ramps[i].x0 = ramp.x0[i]; + ramp.ramps[i].dx0 = ramp.dx0[i]; + ramp.ramps[i].x1 = ramp.x1[i]; + ramp.ramps[i].dx1 = ramp.dx1[i]; + ramp.ramps[i].ttotal = ramp.ramps[i].tswitch2 = (tnext - t); + ramp.ramps[i].tswitch1 = 0; + ramp.ramps[i].v = ramp.dx1[i]; + ramp.ramps[i].a1 = ramp.ramps[i].a2 = 0; + if (gValidityCheckLevel >= 2) { + PARABOLIC_RAMP_ASSERT(ramp.ramps[i].IsValid()); + PARABOLIC_RAMP_ASSERT(ramp.ramps[i].ttotal == ramp.endTime); + } + } + } + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(ramp.IsValid()); + if (ndramps.size() > 1) { //fix up endpoints + ramp.x0 = ndramps[ndramps.size() - 2].x1; + ramp.dx0 = ndramps[ndramps.size() - 2].dx1; + for (std::size_t i = 0; i < ramp.ramps.size(); i++) { + ramp.ramps[i].x0 = ramp.x0[i]; + ramp.ramps[i].dx0 = ramp.dx0[i]; + } + } + if (gValidityCheckLevel >= 2) + PARABOLIC_RAMP_ASSERT(ramp.IsValid()); + + t = tnext; + if (tnext == 0) //all null ramps + break; + } + for (std::size_t i = 0; i < ramps.size(); i++) { + if (!FuzzyEquals(ramps[i].front().x0, ndramps.front().x0[i], EpsilonX)) { + PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d start %g != %g\n", i, ramps[i].front().x0, ndramps.front().x0[i]); + if (gErrorGetchar) getchar(); + } + if (!FuzzyEquals(ramps[i].front().dx0, ndramps.front().dx0[i], EpsilonV)) { + PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d start %g != %g\n", + i, + ramps[i].front().dx0, + ndramps.front().dx0[i]); + if (gErrorGetchar) getchar(); + } + if (!FuzzyEquals(ramps[i].back().x1, ndramps.back().x1[i], EpsilonX)) { + PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d back %g != %g\n", i, ramps[i].back().x1, ndramps.back().x1[i]); + if (gErrorGetchar) getchar(); + } + if (!FuzzyEquals(ramps[i].back().dx1, ndramps.back().dx1[i], EpsilonV)) { + PARABOLIC_RAMP_PERROR("CombineRamps: Error: %d back %g != %g\n", i, ramps[i].back().dx1, ndramps.back().dx1[i]); + if (gErrorGetchar) getchar(); + } + ndramps.front().x0[i] = ndramps.front().ramps[i].x0 = ramps[i].front().x0; + ndramps.front().dx0[i] = ndramps.front().ramps[i].dx0 = ramps[i].front().dx0; + ndramps.back().x1[i] = ndramps.back().ramps[i].x1 = ramps[i].back().x1; + ndramps.back().dx1[i] = ndramps.back().ramps[i].dx1 = ramps[i].back().dx1; + } +} + +} //namespace ParabolicRamp diff --git a/src/external/hauser_parabolic_smoother/ParabolicRamp.h b/src/external/hauser_parabolic_smoother/ParabolicRamp.h new file mode 100644 index 0000000..96d6a77 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/ParabolicRamp.h @@ -0,0 +1,172 @@ +/***************************************************************************** + * + * Copyright (c) 2010-2011, the Trustees of Indiana University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Indiana University nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + + * THIS SOFTWARE IS PROVIDED BY THE TRUSTEES OF INDIANA UNIVERSITY ''AS IS'' + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE TRUSTEES OF INDIANA UNIVERSITY BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + * + ***************************************************************************/ + +#ifndef PARABOLIC_RAMP_H +#define PARABOLIC_RAMP_H + +#include +#include "HauserMath.h" + +namespace ParabolicRamp { + +/** @file ParabolicRamp.h + * @brief Functions for optimal acceleration-bounded trajectories. + */ + +/** @brief Stores optimal trajectores for an acceleration and + * velocity-bounded 1D system. + * + * Initialize the members x0 (start position), x1 (end position), + * dx0 (start velocity), and dx1 (end velocity) before calling the + * SolveX functions. + */ +class ParabolicRamp1D +{ + public: + /// Sets the ramp to a constant function for time t + void SetConstant(Real x,Real t=0); + /// Sets the ramp to a linear function from x0 to x1 with time t. + void SetLinear(Real x0,Real x1,Real t); + /// Solves for minimum time given acceleration and velocity bounds + bool SolveMinTime(Real amax,Real vmax); + /// Solves for minimum time given acceleration and velocity bounds, min time + bool SolveMinTime2(Real amax,Real vmax,Real tLowerBound); + /// Solves for minimum acceleration given end time and velocity bounds + bool SolveMinAccel(Real endTime,Real vmax); + /// Same, but if fails, returns the minimum time > endTime + Real SolveMinAccel2(Real endTime,Real vmax); + /// Solves for the minimum-time braking trajectory starting from x0,dx0 + void SolveBraking(Real amax); + /// Evaluates the trajectory + Real Evaluate(Real t) const; + /// Evaluates the derivative of the trajectory + Real Derivative(Real t) const; + /// Evaluates the second derivative of the trajectory + Real Accel(Real t) const; + /// Returns the time at which x1 is reached + Real EndTime() const { return ttotal; } + /// Scales time to slow down (value > 1) or speed up (< 1) the trajectory + void Dilate(Real timeScale); + /// Trims off the front [0,tcut] of the trajectory + void TrimFront(Real tcut); + /// Trims off the front [T-tcut,T] of the trajectory + void TrimBack(Real tcut); + /// Returns the x bounds on the path + void Bounds(Real& xmin,Real& xmax) const; + /// Returns the x bounds for the given time interval + void Bounds(Real ta,Real tb,Real& xmin,Real& xmax) const; + /// Returns the v bounds on the path + void DerivBounds(Real& vmin,Real& vmax) const; + /// Returns the v bounds for the given time interval + void DerivBounds(Real ta,Real tb,Real& vmin,Real& vmax) const; + /// Sanity check + bool IsValid() const; + + /// Input + Real x0,dx0; + Real x1,dx1; + + /// Calculated upon SolveX + Real tswitch1,tswitch2; //time to switch between ramp/flat/ramp + Real ttotal; + Real a1,v,a2; // accel of first ramp, velocity of linear section, accel of second ramp +}; + +/** @brief Solves for optimal trajectores for a velocity-bounded ND system. + * + * Methods are essentially the same as for ParabolicRamp1D. + */ +class ParabolicRampND +{ + public: + ParabolicRampND(); + void SetConstant(const Vector& x,Real t=0); + void SetLinear(const Vector& x0,const Vector& x1,Real t); + bool SolveMinTimeLinear(const Vector& amax,const Vector& vmax); + bool SolveMinTime(const Vector& amax,const Vector& vmax); + bool SolveMinAccel(const Vector& vmax,Real time); + bool SolveMinAccelLinear(const Vector& vmax,Real time); + void SolveBraking(const Vector& amax); + void Evaluate(Real t,Vector& x) const; + void Derivative(Real t,Vector& dx) const; + void Accel(Real t,Vector& ddx) const; + void Output(Real dt,std::vector& path) const; + void Dilate(Real timeScale); + void TrimFront(Real tcut); + void TrimBack(Real tcut); + void Bounds(Vector& xmin,Vector& xmax) const; + void Bounds(Real ta,Real tb,Vector& xmin,Vector& xmax) const; + void DerivBounds(Vector& vmin,Vector& vmax) const; + void DerivBounds(Real ta,Real tb,Vector& vmin,Vector& vmax) const; + bool IsValid() const; + + /// Input + Vector x0,dx0; + Vector x1,dx1; + + /// Calculated upon SolveX + Real endTime; + std::vector ramps; + + /// Used for shortcutting. + int blendAttempts; +}; + +/// Computes a min-time ramp from (x0,v0) to (x1,v1) under the given +/// acceleration, velocity, and x bounds. Returns true if successful. +bool SolveMinTimeBounded(Real x0,Real v0,Real x1,Real v1, + Real amax,Real vmax,Real xmin,Real xmax, + ParabolicRamp1D& ramp); + +/// Computes a sequence of up to three ramps connecting (x0,v0) to (x1,v1) +/// in minimum-acceleration fashion with a fixed end time, under the given +/// velocity and x bounds. Returns true if successful. +bool SolveMinAccelBounded(Real x0,Real v0,Real x1,Real v1, + Real endTime,Real vmax,Real xmin,Real xmax, + std::vector& ramps); + +/// Vector version of above. +/// Returns the time of the minimum time trajectory, or -1 on failure +Real SolveMinTimeBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1, + const Vector& amax,const Vector& vmax,const Vector& xmin,const Vector& xmax, + std::vector >& ramps); + +/// Vector version of above. +/// Returns true if successful. +bool SolveMinAccelBounded(const Vector& x0,const Vector& v0,const Vector& x1,const Vector& v1, + Real endTime,const Vector& vmax,const Vector& xmin,const Vector& xmax, + std::vector >& ramps); + +/// Combines an array of 1-d ramp sequences into a sequence of N-d ramps +void CombineRamps(const std::vector >& ramps,std::vector& ndramps); + +} //namespace ParabolicRamp + +#endif diff --git a/src/external/hauser_parabolic_smoother/Timer.cpp b/src/external/hauser_parabolic_smoother/Timer.cpp new file mode 100644 index 0000000..e5817c0 --- /dev/null +++ b/src/external/hauser_parabolic_smoother/Timer.cpp @@ -0,0 +1,103 @@ +#include "Timer.h" +#include +using namespace ParabolicRamp; + +#ifdef WIN32 +#define GETCURRENTTIME(x) x=timeGetTime() +#else +#ifdef _POSIX_MONOTONIC_CLOCK +#define GETCURRENTTIME(x) clock_gettime(CLOCK_MONOTONIC,&x) +#else +#define GETCURRENTTIME(x) gettimeofday(&x,NULL) +#endif //_POSIX_MONOTONIC_CLOCK +#endif //WIN32 + +// Sadly, timersub isn't defined in Solaris. :( +// So we use this instead. (added by Ryan) + +#if defined (__SVR4) && defined (__sun) +#include "timersub.h" +#endif + +Timer::Timer() +{ + Reset(); +} + +void Timer::Reset() +{ + GETCURRENTTIME(start); + current=start; +} + +long long Timer::ElapsedTicks() +{ + GETCURRENTTIME(current); + return LastElapsedTicks(); +} + +long long Timer::LastElapsedTicks() const +{ +#ifdef WIN32 + return current-start; +#else +#ifdef _POSIX_MONOTONIC_CLOCK + long long ticks = (current.tv_sec-start.tv_sec)*1000 + (current.tv_nsec-start.tv_nsec)/1000000; + return ticks; +#else + timeval delta; + timersub(¤t,&start,&delta); + long long ticks = delta.tv_sec*1000 + delta.tv_usec/1000; + return ticks; +#endif //_POSIX_MONOTONIC_CLOCK +#endif //WIN32 +} + +double Timer::ElapsedTime() +{ + GETCURRENTTIME(current); + return LastElapsedTime(); +} + +double Timer::LastElapsedTime() const +{ +#ifdef WIN32 + return double(current-start)/1000.0; +#else +#ifdef _POSIX_MONOTONIC_CLOCK + double secs=double(current.tv_sec-start.tv_sec); + secs += double(current.tv_nsec-start.tv_nsec)/1000000000.0; + return secs; +#else + timeval delta; + timersub(¤t,&start,&delta); + double secs=double(delta.tv_sec); + secs += double(delta.tv_usec)/1000000.0; + return secs; +#endif //_POSIX_MONOTONIC_CLOCK +#endif //WIN32 +} + +/* +clock_t Timer::ElapsedTicks() +{ + current = clock(); + return (current-start); +} + +double Timer::ElapsedTime() +{ + current = clock(); + return double(current-start)/CLOCKS_PER_SEC; +} + +clock_t Timer::LastElapsedTicks() const +{ + return current-start; +} + +double Timer::LastElapsedTime() const +{ + return double(current-start)/CLOCKS_PER_SEC; +} +*/ diff --git a/src/external/hauser_parabolic_smoother/Timer.h b/src/external/hauser_parabolic_smoother/Timer.h new file mode 100644 index 0000000..b3e9b2c --- /dev/null +++ b/src/external/hauser_parabolic_smoother/Timer.h @@ -0,0 +1,43 @@ +#ifndef MY_TIMER_H +#define MY_TIMER_H + +#ifdef WIN32 +#include +#else +#include +#endif //WIN32 + +namespace ParabolicRamp { + +#ifdef WIN32 +typedef DWORD TimerCounterType; +#else +#ifdef _POSIX_MONOTONIC_CLOCK +typedef timespec TimerCounterType; +#else +typedef timeval TimerCounterType; +#endif //_POSIX_MONOTONIC_CLOCK +#endif //WIN32 + +class Timer +{ + public: + Timer(); + void Reset(); + + // Returns elapsed time in milliseconds,seconds respectively + long long ElapsedTicks(); + double ElapsedTime(); + + // Doesn't refresh the current time + long long LastElapsedTicks() const; + double LastElapsedTime() const; + + private: + TimerCounterType start; + TimerCounterType current; +}; + +} //namespace ParabolicRamp + +#endif diff --git a/src/planner/SimpleDynamicPath.cpp b/src/planner/SimpleDynamicPath.cpp new file mode 100644 index 0000000..fc64920 --- /dev/null +++ b/src/planner/SimpleDynamicPath.cpp @@ -0,0 +1,40 @@ +// +// Created by hejia on 9/17/19. +// +#include + +#include "SimpleDynamicPath.h" + +using namespace wecook::planner::detail; + +SimpleDynamicPath::SimpleDynamicPath() { + +} + +void SimpleDynamicPath::Init(const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration) { + m_maxVelocity = maxVelocity; + m_maxAcceleration = maxAcceleration; +} + +void SimpleDynamicPath::SetMilestones(const std::vector &milestones, + const std::vector &velocities) { + m_milestones = milestones; + m_velocities = velocities; +} + +void SimpleDynamicPath::SetMilestones(const std::vector &milestones) { + m_milestones = milestones; +} + +void SimpleDynamicPath::Shortcut(int i1, int i2) { + std::cout << "Shortcutting!!! " << i1 + 1 << " " << i2 + 1 << " " << m_milestones.size() << std::endl; + m_milestones.erase(m_milestones.begin() + i1 + 1, m_milestones.begin() + i2); + + if (!m_velocities.empty()) { + m_velocities.erase(m_velocities.begin() + i1 + 1, m_velocities.begin() + i2); + } + std::cout << "Finished!!! " << m_milestones.size() << std::endl; +} + + diff --git a/src/planner/SimpleDynamicPath.h b/src/planner/SimpleDynamicPath.h new file mode 100644 index 0000000..b0e4b1b --- /dev/null +++ b/src/planner/SimpleDynamicPath.h @@ -0,0 +1,38 @@ +// +// Created by hejia on 9/17/19. +// + +#ifndef WECOOK_SIMPLEDYNAMICPATH_H +#define WECOOK_SIMPLEDYNAMICPATH_H + +#include + +namespace wecook { +namespace planner { +namespace detail { +class SimpleDynamicPath { + public: + SimpleDynamicPath(); + void Init(const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration); + void SetMilestones(const std::vector& milestones, const std::vector& velocities); + void SetMilestones(const std::vector& milestones); + std::vector getWaypoints() const { + return m_milestones; + } + Eigen::VectorXd getWaypoint(int i) const { + return m_milestones[i]; + } + void Shortcut(int i1, int i2); + + private: + std::vector m_milestones; + std::vector m_velocities; + Eigen::VectorXd m_maxVelocity; + Eigen::VectorXd m_maxAcceleration; +}; +} +} +} + +#endif //WECOOK_SIMPLEDYNAMICPATH_H diff --git a/src/planner/SmootherHelpers.cpp b/src/planner/SmootherHelpers.cpp new file mode 100644 index 0000000..913a8af --- /dev/null +++ b/src/planner/SmootherHelpers.cpp @@ -0,0 +1,174 @@ +// +// Created by hejia on 9/16/19. +// +#include "SmootherUtil.h" +#include "wecook/planner/SmootherHelpers.h" + +namespace wecook { +namespace planner { +/* + * Simple short cutting + */ +aikido::trajectory::InterpolatedPtr simpleSmoothPath(const std::shared_ptr &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(metaSkeletonPtr); + Eigen::VectorXd accelerationLimits = ada->getAccelerationLimits(metaSkeletonPtr); + auto interpolated = dynamic_cast(path); + if (!interpolated) { + auto spline = dynamic_cast(path); + if (!spline) { + throw std::invalid_argument("Path should be either spline or Interpolated."); + } + interpolated = detail::SplineToInterpolated(*spline); + } + return simpleDoShortcut(metaStateSpace, + metaSkeletonPtr, + *interpolated, + *(ada->cloneRNG().get()), + constraint, + velocityLimits, + accelerationLimits); +} + +aikido::trajectory::InterpolatedPtr simpleDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit, + double checkResolution, + double tolerance) { + auto stateSpace = inputInterpolated.getStateSpace(); + + auto dynamicPath = + detail::InterpolatedToSimpleDynamicPath(inputInterpolated, + maxVelocity, + maxAcceleration, + metaStateSpace, + metaSkeletonPtr); + + detail::simpleDoShortcut(*dynamicPath, collisionTestable, timeLimit, checkResolution, tolerance, rng); + + auto outputTraj = detail::SimpleDynamicPathToInterpolated(*dynamicPath, + stateSpace); + + return outputTraj; +} + +/* + * Smoothing with Kris Hauser's parabolic smoother + */ +aikido::trajectory::UniqueSplinePtr hauserSmoothPath(const std::shared_ptr &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(metaSkeletonPtr); + Eigen::VectorXd accelerationLimits = ada->getAccelerationLimits(metaSkeletonPtr); + auto interpolated = dynamic_cast(path); + if (!interpolated) { + auto spline = dynamic_cast(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 hauserDoShortcut(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit, + double checkResolution, + double tolerance) { + auto stateSpace = inputInterpolated.getStateSpace(); + + double startTime = inputInterpolated.getStartTime(); + + auto dynamicPath = + detail::InterpolatedToHauserDynamicPath(inputInterpolated, + maxVelocity, + maxAcceleration, + metaStateSpace, + metaSkeletonPtr); + + detail::hauserDoShortcut(*dynamicPath, collisionTestable, timeLimit, checkResolution, tolerance, rng); + + auto outputTraj = detail::HauserDynamicPathToSpline(*dynamicPath, + startTime, + stateSpace); + + return outputTraj; +} + +aikido::trajectory::InterpolatedPtr hauserSmoothPathInterpolated(const std::shared_ptr &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(metaSkeletonPtr); + Eigen::VectorXd accelerationLimits = ada->getAccelerationLimits(metaSkeletonPtr); + auto interpolated = dynamic_cast(path); + if (!interpolated) { + auto spline = dynamic_cast(path); + if (!spline) { + throw std::invalid_argument("Path should be either spline or Interpolated."); + } + interpolated = detail::SplineToInterpolated(*spline); + } + return hauserDoShortcutInterpolated(metaStateSpace, + metaSkeletonPtr, + *interpolated, + *(ada->cloneRNG().get()), + constraint, + velocityLimits, + accelerationLimits); +} + +aikido::trajectory::InterpolatedPtr hauserDoShortcutInterpolated(const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + const aikido::trajectory::Interpolated &inputInterpolated, + aikido::common::RNG &rng, + const aikido::constraint::TestablePtr &collisionTestable, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + double timeLimit, + double checkResolution, + double tolerance) { + auto stateSpace = inputInterpolated.getStateSpace(); + + double startTime = inputInterpolated.getStartTime(); + + auto dynamicPath = + detail::InterpolatedToHauserDynamicPath(inputInterpolated, + maxVelocity, + maxAcceleration, + metaStateSpace, + metaSkeletonPtr); + + detail::hauserDoShortcut(*dynamicPath, collisionTestable, timeLimit, checkResolution, tolerance, rng); + + auto outputTraj = detail::HauserDynamicPathToInterpolated(*dynamicPath, + startTime, + stateSpace); + + return outputTraj; +} +} +} diff --git a/src/planner/SmootherUtil.cpp b/src/planner/SmootherUtil.cpp new file mode 100644 index 0000000..b2bd10e --- /dev/null +++ b/src/planner/SmootherUtil.cpp @@ -0,0 +1,484 @@ +// +// Created by hejia on 9/17/19. +// +#include +#include +#include +#include +#include + +#include "aikido/common/Spline.hpp" +#include "dart/dynamics/MetaSkeleton.hpp" + +#include "SmootherUtil.h" + +using CubicSplineProblem = aikido::common::SplineProblem; +using SegmentSplineProblem = aikido::common::SplineProblem; + +namespace wecook { +namespace planner { +namespace detail { + +class SimpleFeasibilityCheckerBase { + public: + virtual ~SimpleFeasibilityCheckerBase() {} + virtual bool ConfigFeasible(const Eigen::VectorXd &x) = 0; + virtual bool SegmentFeasible(const Eigen::VectorXd &a, const Eigen::VectorXd &b) = 0; +}; + +class SimpleSmootherFeasibilityCheckerBase + : public SimpleFeasibilityCheckerBase { + public: + SimpleSmootherFeasibilityCheckerBase( + aikido::constraint::TestablePtr testable, double checkResolution) + : mTestable(std::move(testable)), + mCheckResolution(checkResolution), + mStateSpace(mTestable->getStateSpace()), + mInterpolator(mStateSpace) { + // Do nothing + } + + bool ConfigFeasible(const Eigen::VectorXd &eigX) override { + auto state = mStateSpace->createState(); + mStateSpace->expMap(eigX, state); + return mTestable->isSatisfied(state); + } + + bool SegmentFeasible(const Eigen::VectorXd &eigA, const Eigen::VectorXd &eigB) override { + auto testState = mStateSpace->createState(); + auto startState = mStateSpace->createState(); + auto goalState = mStateSpace->createState(); + mStateSpace->expMap(eigA, startState); + mStateSpace->expMap(eigB, goalState); + + auto dist = (eigA - eigB).norm(); + auto checkRes = 0.005 / dist; + aikido::common::VanDerCorput vdc{1, false, false, checkRes}; + + for (const auto alpha : vdc) { + mInterpolator.interpolate(startState, goalState, alpha, testState); + if (!mTestable->isSatisfied(testState)) { + Eigen::VectorXd testV; + mStateSpace->logMap(testState, testV); + return false; + } + } + return true; + } + + private: + aikido::constraint::TestablePtr mTestable; + double mCheckResolution; + aikido::statespace::ConstStateSpacePtr mStateSpace; + aikido::statespace::GeodesicInterpolator mInterpolator; +}; + +class SimpleFeasibilityChecker { + public: + SimpleFeasibilityChecker(SimpleFeasibilityCheckerBase *feas, double tol) : feas(feas), tol(tol), maxiters(0) { + + } + bool Check(const Eigen::VectorXd &a, const Eigen::VectorXd &b) { + return feas->SegmentFeasible(a, b); + } + + SimpleFeasibilityCheckerBase *feas; + double tol; + int maxiters; +}; + +class HauserSmootherFeasibilityCheckerBase + : public ParabolicRamp::FeasibilityCheckerBase { + public: + HauserSmootherFeasibilityCheckerBase(aikido::constraint::TestablePtr testable, double checkResolution) + : mTestable(std::move(testable)), + mCheckResolution(checkResolution), + mStateSpace(mTestable->getStateSpace()), + mInterpolator(mStateSpace) { + // Do nothing + } + + bool ConfigFeasible(const ParabolicRamp::Vector& x) override { + Eigen::VectorXd eigX = toEigen(x); + auto state = mStateSpace->createState(); + mStateSpace->expMap(eigX, state); + return mTestable->isSatisfied(state); + } + + bool SegmentFeasible(const ParabolicRamp::Vector& a, const ParabolicRamp::Vector& b) override { + Eigen::VectorXd eigA = toEigen(a); + Eigen::VectorXd eigB = toEigen(b); + + auto testState = mStateSpace->createState(); + auto startState = mStateSpace->createState(); + auto goalState = mStateSpace->createState(); + mStateSpace->expMap(eigA, startState); + mStateSpace->expMap(eigB, goalState); + + auto dist = (eigA - eigB).norm(); + auto checkRes = 0.01 / dist; + + // both ends of the segment have already been checked by calling + // ConfigFeasible(), + // thus it is no longer needed to check in SegmentFeasible() + aikido::common::VanDerCorput vdc{1, false, false, checkRes}; + + for (const auto alpha : vdc) + { + mInterpolator.interpolate(startState, goalState, alpha, testState); + if (!mTestable->isSatisfied(testState)) + { + return false; + } + } + return true; + } + + private: + aikido::constraint::TestablePtr mTestable; + double mCheckResolution; + aikido::statespace::ConstStateSpacePtr mStateSpace; + aikido::statespace::GeodesicInterpolator mInterpolator; +}; + +/* + * Utility functions + */ + +//============================================================================== +ParabolicRamp::Vector toVector(const Eigen::VectorXd &_x) { + ParabolicRamp::Vector output(_x.size()); + + for (int i = 0; i < _x.size(); ++i) + output[i] = _x[i]; + + return output; +} + +//============================================================================== +Eigen::VectorXd toEigen(const ParabolicRamp::Vector &_x) { + Eigen::VectorXd output(_x.size()); + + for (std::size_t i = 0; i < _x.size(); ++i) + output[i] = _x[i]; + + return output; +} + +//============================================================================== +void evaluateAtTime( + const ParabolicRamp::DynamicPath &_path, + double _t, + Eigen::VectorXd &_position, + Eigen::VectorXd &_velocity) { + ParabolicRamp::Vector positionVector; + _path.Evaluate(_t, positionVector); + _position = toEigen(positionVector); + + ParabolicRamp::Vector velocityVector; + _path.Derivative(_t, velocityVector); + _velocity = toEigen(velocityVector); +} + +std::unique_ptr InterpolatedToSimpleDynamicPath(const aikido::trajectory::Interpolated &inputInterpolated, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + bool preserveWaypointVelocity) { + auto stateSpace = inputInterpolated.getStateSpace(); + const auto numWaypoints = inputInterpolated.getNumWaypoints(); + + std::vector milestones; + std::vector velocities; + milestones.reserve(numWaypoints); + velocities.reserve(numWaypoints); + + Eigen::VectorXd tangentVector, currVec; + + for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint) { + auto currentState = inputInterpolated.getWaypoint(iwaypoint); + + stateSpace->logMap(currentState, currVec); + milestones.emplace_back(currVec); + + // TODO check later, + // for now we are not using velocity +// inputInterpolated.getWaypointDerivative(iwaypoint, 1, tangentVector); +// velocities.emplace_back(tangentVector); + } + + auto outputPath = std::make_unique(); + outputPath->Init(maxVelocity, maxAcceleration); + if (preserveWaypointVelocity) { + outputPath->SetMilestones(milestones, velocities); + } else { + outputPath->SetMilestones(milestones); + } +// if (!outputPath->IsValid()) +// throw std::runtime_error("Converted DynamicPath is not valid"); + return outputPath; +} + +std::unique_ptr InterpolatedToHauserDynamicPath(const aikido::trajectory::Interpolated &inputInterpolated, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + bool preserveWaypointVelocity) { + auto stateSpace = inputInterpolated.getStateSpace(); + const auto numWaypoints = inputInterpolated.getNumWaypoints(); + + std::vector milestones; + std::vector velocities; + milestones.reserve(numWaypoints); + velocities.reserve(numWaypoints); + + Eigen::VectorXd tangentVector, currVec; + + for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint) { +// auto currentState = stateSpace->createState(); + auto currentState = inputInterpolated.getWaypoint(iwaypoint); + + stateSpace->logMap(currentState, currVec); + milestones.emplace_back(toVector(currVec)); + +// inputInterpolated.getWaypointDerivative(iwaypoint, 1, tangentVector); +// velocities.emplace_back(tangentVector); + } + + auto outputPath = std::make_unique(); + outputPath->Init(toVector(maxVelocity), toVector(maxAcceleration)); + if (preserveWaypointVelocity) { + outputPath->SetMilestones(milestones, velocities); + } else { + outputPath->SetMilestones(milestones); + } +// if (!outputPath->IsValid()) +// throw std::runtime_error("Converted DynamicPath is not valid"); + return outputPath; +} + +std::unique_ptr HauserDynamicPathToSpline(const ParabolicRamp::DynamicPath &_inputPath, + double _startTime, + const aikido::statespace::ConstStateSpacePtr &_stateSpace) { + const auto dimension = _stateSpace->getDimension(); + + // Construct a list of all ramp transition points. + double t = 0.; + std::set transitionTimes; + transitionTimes.insert(t); + + for (const auto &rampNd : _inputPath.ramps) { + for (const auto &ramp1d : rampNd.ramps) { + transitionTimes.insert(t + ramp1d.tswitch1); + transitionTimes.insert(t + ramp1d.tswitch2); + } + + t += rampNd.endTime; + transitionTimes.insert(t); + } + + // Convert the output to a spline with a knot at each transition time. + assert(!transitionTimes.empty()); + const auto startIt = std::begin(transitionTimes); + double timePrev = *startIt; + transitionTimes.erase(startIt); + + Eigen::VectorXd positionPrev, velocityPrev; + evaluateAtTime(_inputPath, timePrev, positionPrev, velocityPrev); + + auto _outputTrajectory = std::make_unique(_stateSpace, timePrev + _startTime); + auto segmentStartState = _stateSpace->createState(); + + for (const auto timeCurr : transitionTimes) { + Eigen::VectorXd positionCurr, velocityCurr; + evaluateAtTime(_inputPath, timeCurr, positionCurr, velocityCurr); + + SegmentSplineProblem problem(Eigen::Vector2d(0, timeCurr - timePrev), 2, dimension); +// CubicSplineProblem problem(Eigen::Vector2d(0, timeCurr - timePrev), 4, dimension); + problem.addConstantConstraint(0, 0, Eigen::VectorXd::Zero(dimension)); +// problem.addConstantConstraint(0, 1, velocityPrev); + problem.addConstantConstraint(1, 0, positionCurr - positionPrev); +// problem.addConstantConstraint(1, 1, velocityCurr); + const auto spline = problem.fit(); + + _stateSpace->expMap(positionPrev, segmentStartState); + + // Add the ramp to the output trajectory + assert(spline.getCoefficients().size() == 1); + const auto &coefficients = spline.getCoefficients().front(); + _outputTrajectory->addSegment( + coefficients, timeCurr - timePrev, segmentStartState); + + timePrev = timeCurr; + positionPrev = positionCurr; + velocityPrev = velocityCurr; + } + return _outputTrajectory; +} + +aikido::trajectory::InterpolatedPtr HauserDynamicPathToInterpolated(const ParabolicRamp::DynamicPath &_inputPath, + double _startTime, + const aikido::statespace::ConstStateSpacePtr &_stateSpace) { + /* + * Used for debugging + */ + std::shared_ptr + interpolator = std::make_shared(_stateSpace); + auto traj = std::make_shared(_stateSpace, interpolator); + + // Construct a list of all ramp transition points. + double t = 0.; + std::set transitionTimes; + transitionTimes.insert(t); + + for (const auto &rampNd : _inputPath.ramps) { + for (const auto &ramp1d : rampNd.ramps) { + transitionTimes.insert(t + ramp1d.tswitch1); + transitionTimes.insert(t + ramp1d.tswitch2); + } + + t += rampNd.endTime; + transitionTimes.insert(t); + } + + // Convert the output to a spline with a knot at each transition time. + assert(!transitionTimes.empty()); + + for (const auto timeCurr : transitionTimes) { + Eigen::VectorXd positionCurr, velocityCurr; + evaluateAtTime(_inputPath, timeCurr, positionCurr, velocityCurr); + auto currState = _stateSpace->createState(); + _stateSpace->expMap(positionCurr, currState); + traj->addWaypoint(timeCurr, currState); + } + return traj; +} + +aikido::trajectory::InterpolatedPtr SimpleDynamicPathToInterpolated(const SimpleDynamicPath &_inputPath, + const aikido::statespace::ConstStateSpacePtr &_stateSpace) { + std::shared_ptr + interpolator = std::make_shared(_stateSpace); + auto traj = std::make_shared(_stateSpace, interpolator); + + auto waypoints = _inputPath.getWaypoints(); + + // Convert the output to a spline with a knot at each transition time. + assert(!waypoints.empty()); + + auto segmentStartState = _stateSpace->createState(); + + double timeCurr = 0.; + double delta = 0.2; + for (const auto waypoint : waypoints) { + _stateSpace->expMap(waypoint, segmentStartState); + + traj->addWaypoint(timeCurr, segmentStartState); + + timeCurr += delta; + } + + return traj; +} + +aikido::trajectory::Interpolated *SplineToInterpolated(const aikido::trajectory::Spline &inputSpline) { + auto stateSpace = inputSpline.getStateSpace(); + const auto numWaypoints = inputSpline.getNumWaypoints(); + + std::shared_ptr + interpolator = std::make_shared(stateSpace); + auto traj = std::make_shared(stateSpace, interpolator); + + double timeCurr = 0.; + double delta = 0.2; + for (std::size_t iwaypoint = 0; iwaypoint < numWaypoints; ++iwaypoint) { + auto currentState = stateSpace->createState(); + inputSpline.getWaypoint(iwaypoint, currentState); + traj->addWaypoint(timeCurr, currentState); + timeCurr += delta; + } + + return traj.get(); +} + +void hauserDoShortcut(ParabolicRamp::DynamicPath &hauserDynamicPath, + aikido::constraint::TestablePtr testable, + double timelimit, + double checkResolution, + double tolerance, + aikido::common::RNG &rng) { + if (timelimit < 0.0) + throw std::invalid_argument("Timelimit should be non-negative"); + if (checkResolution <= 0.0) + throw std::invalid_argument("Check resolution should be positive"); + if (tolerance < 0.0) + throw std::invalid_argument("Tolerance should be non-negative"); + + HauserSmootherFeasibilityCheckerBase base(testable, checkResolution); + ParabolicRamp::RampFeasibilityChecker feasibilityChecker(&base, tolerance); + + std::chrono::time_point startTime + = std::chrono::system_clock::now(); + double elapsedTime = 0; + + while (elapsedTime < timelimit && hauserDynamicPath.ramps.size() > 3) { + std::uniform_real_distribution<> dist(0.0, hauserDynamicPath.GetTotalTime()); + double t1 = dist(rng); + double t2 = dist(rng); + if (hauserDynamicPath.TryShortcut(t1, t2, feasibilityChecker)) { + } + + elapsedTime = std::chrono::duration_cast>( + std::chrono::system_clock::now() - startTime) + .count(); + } +} + +void simpleDoShortcut(SimpleDynamicPath &dynamicPath, + aikido::constraint::TestablePtr testable, + double timelimit, + double checkResolution, + double tolerance, + aikido::common::RNG &rng) { + if (timelimit < 0.0) + throw std::invalid_argument("Timelimit should be non-negative"); + if (checkResolution <= 0.0) + throw std::invalid_argument("Check resolution should be positive"); + if (tolerance < 0.0) + throw std::invalid_argument("Tolerance should be non-negative"); + + SimpleSmootherFeasibilityCheckerBase base(testable, checkResolution); + SimpleFeasibilityChecker feasibilityChecker(&base, tolerance); + + std::chrono::time_point startTime + = std::chrono::system_clock::now(); + double elapsedTime = 0; + + while (elapsedTime < timelimit && dynamicPath.getWaypoints().size() > 3) { + // closed interval + std::uniform_int_distribution<> dist(0, dynamicPath.getWaypoints().size() - 1); + int i1 = dist(rng); + int i2 = dist(rng); + + if (i1 > i2) { + auto tmp = i1; + i1 = i2; + i2 = tmp; + } + + if (i1 == i2 || i1 - i2 == -1) { + continue; + } + + if (feasibilityChecker.Check(dynamicPath.getWaypoint(i1), dynamicPath.getWaypoint(i2))) { + dynamicPath.Shortcut(i1, i2); + } + + elapsedTime = + std::chrono::duration_cast>(std::chrono::system_clock::now() - startTime).count(); + } +} +} +} +} \ No newline at end of file diff --git a/src/planner/SmootherUtil.h b/src/planner/SmootherUtil.h new file mode 100644 index 0000000..3fc4d67 --- /dev/null +++ b/src/planner/SmootherUtil.h @@ -0,0 +1,93 @@ +// +// Created by hejia on 9/17/19. +// + +#ifndef WECOOK_SMOOTHERUTIL_H +#define WECOOK_SMOOTHERUTIL_H + +#include +#include +#include +#include +#include + +#include "SimpleDynamicPath.h" + +namespace wecook { +namespace planner { +namespace detail { +/* + * Utility functions + */ +/// Convert a Eigen vector to a ParabolicRamp vector +/// \param _x an Eigen vector +/// \return a ParabolicRamp vector +ParabolicRamp::Vector toVector(const Eigen::VectorXd &_x); + +/// Convert a ParabolicRamp vector to a Eigen vector +/// \param _x a ParabolicRamp vector +/// \return an Eigen vector +Eigen::VectorXd toEigen(const ParabolicRamp::Vector &_x); + +/// Evaluate the position and the velocity of a dynamic path +/// given time t +/// \param _path a dynamic path +/// \param _t time +/// \param[out] position at time \c _t +/// \param[out] velocity at time \c _t +void evaluateAtTime(ParabolicRamp::DynamicPath &_path, double _t, + Eigen::VectorXd &_position, Eigen::VectorXd &_velocity); + +std::unique_ptr InterpolatedToSimpleDynamicPath(const aikido::trajectory::Interpolated &inputInterpolated, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + bool preserveWaypointVelocity = false); + +std::unique_ptr InterpolatedToHauserDynamicPath(const aikido::trajectory::Interpolated &inputInterpolated, + const Eigen::VectorXd &maxVelocity, + const Eigen::VectorXd &maxAcceleration, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &metaStateSpace, + const dart::dynamics::MetaSkeletonPtr &metaSkeletonPtr, + bool preserveWaypointVelocity = false); + +std::unique_ptr HauserDynamicPathToSpline(const ParabolicRamp::DynamicPath &_inputPath, + double _startTime, + const aikido::statespace::ConstStateSpacePtr &_stateSpace); + +aikido::trajectory::InterpolatedPtr HauserDynamicPathToInterpolated(const ParabolicRamp::DynamicPath &_inputPath, + double _startTime, + const aikido::statespace::ConstStateSpacePtr &_stateSpace); + +aikido::trajectory::InterpolatedPtr SimpleDynamicPathToInterpolated(const SimpleDynamicPath &_inputPath, + const aikido::statespace::ConstStateSpacePtr &_stateSpace); + +aikido::trajectory::Interpolated *SplineToInterpolated(const aikido::trajectory::Spline &inputSpline); + +//===================================================================================================================== + +/* + * simple shortcutting + */ +void simpleDoShortcut(SimpleDynamicPath &dynamicPath, + aikido::constraint::TestablePtr testable, + double timelimit, + double checkResolution, + double tolerance, + aikido::common::RNG &rng); + +/* + * smoothing with Kris Hauser's smoother + */ +void hauserDoShortcut(ParabolicRamp::DynamicPath &hauserDynamicPath, + aikido::constraint::TestablePtr testable, + double timelimit, + double checkResolution, + double tolerancce, + aikido::common::RNG &rng); +} +} +} + +#endif //WECOOK_SMOOTHERUTIL_H diff --git a/wecook.rosinstall b/wecook.rosinstall index 04a178b..32cd14e 100644 --- a/wecook.rosinstall +++ b/wecook.rosinstall @@ -1,45 +1,24 @@ - git: local-name: ada_description - uri: https://github.com/usc-csci-545/ada_description.git + uri: https://github.com/icaros-usc/ada_description.git version: wecook - git: local-name: ada_launch - uri: https://github.com/usc-csci-545/ada_launch.git + uri: https://github.com/icaros-usc/ada_launch.git - git: local-name: aikido - uri: https://github.com/usc-csci-545/aikido.git + uri: https://github.com/icaros-usc/aikido.git version: wecook -- git: - local-name: aikidopy - uri: https://github.com/usc-csci-545/aikidopy.git -- git: - local-name: jaco_hardware - uri: https://github.com/usc-csci-545/jaco_hardware.git - git: local-name: libada - uri: https://github.com/usc-csci-545/libada.git + uri: https://github.com/icaros-usc/libada.git version: wecook -- git: - local-name: pr_assets - uri: https://github.com/usc-csci-545/pr_assets.git - git: local-name: pr_control_msgs - uri: https://github.com/usc-csci-545/pr_control_msgs.git -- git: - local-name: pr_hardware_interfaces - uri: https://github.com/usc-csci-545/pr_hardware_interfaces.git -- git: - local-name: pr_ros_controllers - uri: https://github.com/usc-csci-545/pr_ros_controllers.git + uri: https://github.com/icaros-usc/pr_control_msgs.git - git: local-name: pr_tsr - uri: https://github.com/usc-csci-545/pr_tsr.git -- git: - local-name: rewd_controllers - uri: https://github.com/usc-csci-545/rewd_controllers.git -- git: - local-name: roscpp_initializer - uri: https://github.com/usc-csci-545/roscpp_initializer.git + uri: https://github.com/icaros-usc/pr_tsr.git - git: local-name: wecook uri: https://github.com/icaros-usc/wecook.git @@ -50,4 +29,4 @@ version: master - git: local-name: pybind11_catkin - uri: https://github.com/usc-csci-545/pybind11_catkin.git \ No newline at end of file + uri: https://github.com/icaros-usc/pybind11_catkin.git \ No newline at end of file