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
e6c9f19d92
commit
09c8bd49ce
@ -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
|
||||
|
@ -99,22 +99,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. ::
|
||||
@ -124,12 +108,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<>());
|
||||
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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") };
|
||||
|
Loading…
Reference in New Issue
Block a user