mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Revert "Update API: JumpThreshold -> CartesianPrecision (#611)"
This reverts commit 99ccc115e0
.
This commit is contained in:
parent
679a1c98f4
commit
d9109870b1
@ -39,7 +39,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||||
#include <moveit/robot_state/cartesian_interpolator.hpp>
|
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
@ -58,11 +57,7 @@ public:
|
|||||||
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
|
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
|
||||||
|
|
||||||
void setStepSize(double step_size) { setProperty("step_size", step_size); }
|
void setStepSize(double step_size) { setProperty("step_size", step_size); }
|
||||||
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
|
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
|
||||||
template <typename T = float>
|
|
||||||
void setJumpThreshold(double /*unused*/) {
|
|
||||||
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
|
|
||||||
}
|
|
||||||
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
|
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
|
||||||
|
|
||||||
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
|
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
|
||||||
|
@ -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")
|
.property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
|
||||||
.def(py::init<>());
|
.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"(
|
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
|
||||||
Perform linear interpolation between Cartesian poses.
|
Perform linear interpolation between Cartesian poses.
|
||||||
Fails on collision along the interpolation path. There is no obstacle avoidance. ::
|
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
|
# Instantiate Cartesian-space interpolation planner
|
||||||
cartesianPlanner = core.CartesianPath()
|
cartesianPlanner = core.CartesianPath()
|
||||||
cartesianPlanner.step_size = 0.01
|
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 "
|
.property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
|
||||||
"In contrast to joint-space interpolation, the Cartesian planner can also "
|
"In contrast to joint-space interpolation, the Cartesian planner can also "
|
||||||
"succeed when only a fraction of the linear path was feasible.")
|
"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.")
|
.property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
|
||||||
.def(py::init<>());
|
.def(py::init<>());
|
||||||
|
|
||||||
|
@ -60,8 +60,7 @@ CartesianPath::CartesianPath() {
|
|||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
|
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<double>("step_size", 0.01, "step size between consecutive waypoints");
|
||||||
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
|
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
|
||||||
"precision of linear path");
|
|
||||||
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
|
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
|
||||||
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
|
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
|
||||||
"KinematicsQueryOptions to pass to CartesianInterpolator");
|
"KinematicsQueryOptions to pass to CartesianInterpolator");
|
||||||
@ -121,7 +120,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
|
|||||||
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
||||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
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::KinematicsQueryOptions>("kinematics_options"),
|
||||||
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
|
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
|
||||||
|
|
||||||
|
@ -34,6 +34,7 @@ int main(int argc, char** argv) {
|
|||||||
|
|
||||||
// setup solvers
|
// setup solvers
|
||||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||||
|
cartesian->setJumpThreshold(2.0);
|
||||||
|
|
||||||
const auto ptp = [&node]() {
|
const auto ptp = [&node]() {
|
||||||
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
|
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP") };
|
||||||
|
Loading…
Reference in New Issue
Block a user