Add trajectory_parameterization parameter to CartesianPath & JointInterpolationPlanner

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -39,6 +39,7 @@
 #pragma once
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
 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_processing::TimeParameterization> trajectory_parameterization =
+	                           std::make_unique<trajectory_processing::TimeOptimalTrajectoryGeneration>());
 
 	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_processing::TimeParameterization> trajectory_parameterization_;
 };
 }  // namespace solvers
 }  // namespace task_constructor

core/include/moveit/task_constructor/solvers/joint_interpolation.h

@@ -40,6 +40,7 @@
 
 #include <moveit/task_constructor/solvers/planner_interface.h>
 #include <moveit/macros/class_forward.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
 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_processing::TimeParameterization> trajectory_parameterization =
+	        std::make_unique<trajectory_processing::TimeOptimalTrajectoryGeneration>());
 
 	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_processing::TimeParameterization> trajectory_parameterization_;
 };
 }  // namespace solvers
 }  // namespace task_constructor

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_processing::TimeParameterization> trajectory_parameterization)
+  : trajectory_parameterization_(std::move(trajectory_parameterization)) {
 	auto& p = properties();
 	p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
 	p.declare<double>("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<double>("max_velocity_scaling_factor"),
-	                         props.get<double>("max_acceleration_scaling_factor"));
+	trajectory_parameterization_->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
+	                                                props.get<double>("max_acceleration_scaling_factor"));
 
 	return achieved_fraction >= props.get<double>("min_fraction");
 }

core/src/solvers/joint_interpolation.cpp

@@ -38,15 +38,16 @@
 
 #include <moveit/task_constructor/solvers/joint_interpolation.h>
 #include <moveit/planning_scene/planning_scene.h>
-#include <moveit/trajectory_processing/iterative_time_parameterization.h>
 
 #include <chrono>
 
 namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
-JointInterpolationPlanner::JointInterpolationPlanner() {
+JointInterpolationPlanner::JointInterpolationPlanner(
+    std::unique_ptr<trajectory_processing::TimeParameterization> trajectory_parameterization)
+  : trajectory_parameterization_(std::move(trajectory_parameterization)) {
 	auto& p = properties();
 	p.declare<double>("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<double>("max_velocity_scaling_factor"),
-	                         props.get<double>("max_acceleration_scaling_factor"));
+	trajectory_parameterization_->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
+	                                                props.get<double>("max_acceleration_scaling_factor"));
 
 	return true;
 }