From 60229db010ea305296bc1c90d04faa3e4dacd976 Mon Sep 17 00:00:00 2001
From: Wyatt Rees <wyatt.rees@gmail.com>
Date: Fri, 3 Jun 2022 10:39:51 -0600
Subject: [PATCH] Add KinematicsQueryOptions property in CartesianPath solver
 (#370)

---
 core/src/solvers/cartesian_path.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp
index 6044c8309..3caecd757 100644
--- a/core/src/solvers/cartesian_path.cpp
+++ b/core/src/solvers/cartesian_path.cpp
@@ -41,6 +41,7 @@
 
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/trajectory_processing/time_parameterization.h>
+#include <moveit/kinematics_base/kinematics_base.h>
 #if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
 #include <moveit/robot_state/cartesian_interpolator.h>
 #endif
@@ -56,6 +57,8 @@ CartesianPath::CartesianPath() {
 	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");
 	p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
+	p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
+	                                              "KinematicsQueryOptions to pass to CartesianInterpolator");
 }
 
 void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
@@ -97,11 +100,12 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
 	double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
 	    &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
 	    moveit::core::MaxEEFStep(props.get<double>("step_size")),
-	    moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid);
+	    moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
+	    props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
 #else
 	double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
 	    jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
-	    is_valid);
+	    is_valid, props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
 #endif
 
 	assert(!trajectory.empty());  // there should be at least the start state