Revert "Update API: JumpThreshold -> CartesianPrecision (#611)"
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jazzy-source]) (push) Has been cancelled
Format / pre-commit (push) Has been cancelled

This reverts commit 99ccc115e0.
This commit is contained in:
Robert Haschke 2024-09-17 11:57:44 +02:00
parent 679a1c98f4
commit d9109870b1
4 changed files with 9 additions and 27 deletions

View File

@ -39,7 +39,6 @@
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_state/cartesian_interpolator.hpp>
namespace moveit {
namespace task_constructor {
@ -58,11 +57,7 @@ public:
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
template <typename T = float>
void setJumpThreshold(double /*unused*/) {
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
}
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off

View File

@ -97,22 +97,6 @@ void export_solvers(py::module& m) {
.property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
.def(py::init<>());
const moveit::core::CartesianPrecision default_precision;
py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
.def(py::init([](double translational, double rotational, double max_resolution) {
return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
}),
py::arg("translational") = default_precision.translational,
py::arg("rotational") = default_precision.rotational,
py::arg("max_resolution") = default_precision.max_resolution)
.def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
.def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
.def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)
.def("__str__", [](const moveit::core::CartesianPrecision& self) {
return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}",
self.translational, self.rotational, self.max_resolution);
});
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
Perform linear interpolation between Cartesian poses.
Fails on collision along the interpolation path. There is no obstacle avoidance. ::
@ -122,12 +106,15 @@ void export_solvers(py::module& m) {
# Instantiate Cartesian-space interpolation planner
cartesianPlanner = core.CartesianPath()
cartesianPlanner.step_size = 0.01
cartesianPlanner.precision.translational = 0.001
cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold.
)")
.property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
"In contrast to joint-space interpolation, the Cartesian planner can also "
"succeed when only a fraction of the linear path was feasible.")
.property<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
.property<double>(
"jump_threshold",
"float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. "
"This values specifies the fraction of mean acceptable joint motion per step.")
.property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
.def(py::init<>());

View File

@ -60,8 +60,7 @@ CartesianPath::CartesianPath() {
auto& p = properties();
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
"precision of linear path");
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");
@ -121,7 +120,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")),
props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);

View File

@ -34,6 +34,7 @@ int main(int argc, char** argv) {
// setup solvers
auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setJumpThreshold(2.0);
const auto ptp = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };