From 258a407a835df9a1b4d6588b7110e8aa0559cbb6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 6 Jan 2022 15:54:23 +0300 Subject: [PATCH 1/2] Add trajectory_parameterization parameter to CartesianPath & JointInterpolationPlanner --- .../moveit/task_constructor/solvers/cartesian_path.h | 7 ++++++- .../task_constructor/solvers/joint_interpolation.h | 8 +++++++- core/src/solvers/cartesian_path.cpp | 8 ++++---- core/src/solvers/joint_interpolation.cpp | 11 +++++------ 4 files changed, 22 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 72e63fe3a..d969ef78d 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,6 +39,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { @@ -50,7 +51,8 @@ MOVEIT_CLASS_FORWARD(CartesianPath); class CartesianPath : public PlannerInterface { public: - CartesianPath(); + explicit CartesianPath(std::unique_ptr trajectory_parameterization = + std::make_unique()); void setStepSize(double step_size) { setProperty("step_size", step_size); } void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } @@ -69,6 +71,9 @@ class CartesianPath : public PlannerInterface const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + +private: + std::unique_ptr trajectory_parameterization_; }; } // namespace solvers } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 6f27c240e..a4b28aa33 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -40,6 +40,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -54,7 +55,9 @@ MOVEIT_CLASS_FORWARD(JointInterpolationPlanner); class JointInterpolationPlanner : public PlannerInterface { public: - JointInterpolationPlanner(); + explicit JointInterpolationPlanner( + std::unique_ptr trajectory_parameterization = + std::make_unique()); void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -66,6 +69,9 @@ class JointInterpolationPlanner : public PlannerInterface const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + +private: + std::unique_ptr trajectory_parameterization_; }; } // namespace solvers } // namespace task_constructor diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e15306ab8..60bad6710 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -49,7 +49,8 @@ namespace moveit { namespace task_constructor { namespace solvers { -CartesianPath::CartesianPath() { +CartesianPath::CartesianPath(std::unique_ptr trajectory_parameterization) + : trajectory_parameterization_(std::move(trajectory_parameterization)) { auto& p = properties(); p.declare("step_size", 0.01, "step size between consecutive waypoints"); p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); @@ -107,9 +108,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + trajectory_parameterization_->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return achieved_fraction >= props.get("min_fraction"); } diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 35b3935f2..d4fee72fa 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -38,7 +38,6 @@ #include #include -#include #include @@ -46,7 +45,9 @@ namespace moveit { namespace task_constructor { namespace solvers { -JointInterpolationPlanner::JointInterpolationPlanner() { +JointInterpolationPlanner::JointInterpolationPlanner( + std::unique_ptr trajectory_parameterization) + : trajectory_parameterization_(std::move(trajectory_parameterization)) { auto& p = properties(); p.declare("max_step", 0.1, "max joint step"); } @@ -89,10 +90,8 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr if (from->isStateColliding(to_state, jmg->getName())) return false; - // add timing, TODO: use a generic method to add timing via plugins - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); + trajectory_parameterization_->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return true; } From a536bc252b632f96efaef2c239c79828bc10b0b6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Fri, 7 Jan 2022 10:27:23 +0300 Subject: [PATCH 2/2] Make TOTG default parameterization --- core/include/moveit/task_constructor/solvers/cartesian_path.h | 4 ++-- .../moveit/task_constructor/solvers/joint_interpolation.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index d969ef78d..76421adaf 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace moveit { namespace task_constructor { @@ -52,7 +52,7 @@ class CartesianPath : public PlannerInterface { public: explicit CartesianPath(std::unique_ptr trajectory_parameterization = - std::make_unique()); + std::make_unique()); void setStepSize(double step_size) { setProperty("step_size", step_size); } void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index a4b28aa33..2c99d5f10 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -40,7 +40,7 @@ #include #include -#include +#include namespace moveit { namespace task_constructor { @@ -57,7 +57,7 @@ class JointInterpolationPlanner : public PlannerInterface public: explicit JointInterpolationPlanner( std::unique_ptr trajectory_parameterization = - std::make_unique()); + std::make_unique()); void init(const moveit::core::RobotModelConstPtr& robot_model) override;