Skip to content

Commit 55b65e0

Browse files
committed
Address review
1 parent 0356a4a commit 55b65e0

File tree

3 files changed

+14
-8
lines changed

3 files changed

+14
-8
lines changed

core/include/moveit/task_constructor/solvers/cartesian_path.h

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -59,14 +59,16 @@ class CartesianPath : public PlannerInterface
5959

6060
void setStepSize(double step_size) { setProperty("step_size", step_size); }
6161
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
62-
[[deprecated("Replace with setPrecision")]] // clang-format off
63-
void setJumpThreshold(double /*jump_threshold*/) {}
62+
template <typename T = float>
63+
void setJumpThreshold(double) {
64+
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
65+
}
6466
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
6567

6668
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
67-
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }
69+
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on
6870
[[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off
69-
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }
71+
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on
7072

7173
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
7274

@@ -75,8 +77,8 @@ class CartesianPath : public PlannerInterface
7577
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
7678

7779
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
78-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
79-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
80+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
81+
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
8082
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
8183
};
8284
} // namespace solvers

core/python/bindings/src/solvers.cpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -100,11 +100,14 @@ void export_solvers(py::module& m) {
100100
.property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
101101
.def(py::init<>());
102102

103+
const moveit::core::CartesianPrecision default_precision;
103104
py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
104105
.def(py::init([](double translational, double rotational, double max_resolution) {
105106
return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
106107
}),
107-
py::arg("translational") = 0.001, py::arg("rotational") = 0.01, py::arg("max_resolution") = 1e-5)
108+
py::arg("translational") = default_precision.translational,
109+
py::arg("rotational") = default_precision.rotational,
110+
py::arg("max_resolution") = default_precision.max_resolution)
108111
.def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
109112
.def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
110113
.def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)

core/src/solvers/cartesian_path.cpp

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,8 @@ CartesianPath::CartesianPath() {
5454
auto& p = properties();
5555
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
5656
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
57-
p.declare<moveit::core::CartesianPrecision>("precision", { 1e-3, 1e-2, 1e-4 }, "precision of linear path");
57+
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
58+
"precision of linear path");
5859
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
5960
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
6061
"KinematicsQueryOptions to pass to CartesianInterpolator");

0 commit comments

Comments
 (0)