Add KinematicsQueryOptions property in CartesianPath solver (#370)

This commit is contained in:
Wyatt Rees 2022-06-03 10:39:51 -06:00 committed by GitHub
parent d2918f130d
commit 60229db010
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -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