mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Allow max Cartesian link speed in PlannerInterface (#277)
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run
This commit is contained in:
parent
ac4b92fff9
commit
7384702448
@ -89,6 +89,10 @@ public:
|
|||||||
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
|
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
|
||||||
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
|
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
|
||||||
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
|
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
|
||||||
|
|
||||||
|
void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); }
|
||||||
|
void setCartesianSpeedLimitedLink(const std::string& link) { setProperty("cartesian_speed_limited_link", link); }
|
||||||
|
|
||||||
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
|
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
|
||||||
properties_.set("time_parameterization", tp);
|
properties_.set("time_parameterization", tp);
|
||||||
}
|
}
|
||||||
|
@ -54,6 +54,8 @@ void export_solvers(py::module& m) {
|
|||||||
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
|
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
|
||||||
.property<double>("max_acceleration_scaling_factor",
|
.property<double>("max_acceleration_scaling_factor",
|
||||||
"float: Reduce the maximum acceleration by scaling between (0,1]")
|
"float: Reduce the maximum acceleration by scaling between (0,1]")
|
||||||
|
.property<double>("max_cartesian_speed", "float: maximum cartesian speed")
|
||||||
|
.property<std::string>("cartesian_speed_limited_link", "str: link with limited cartesian speed")
|
||||||
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
|
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
|
||||||
py::return_value_policy::reference_internal, "Properties of the planner");
|
py::return_value_policy::reference_internal, "Properties of the planner");
|
||||||
|
|
||||||
|
@ -40,6 +40,7 @@
|
|||||||
#include <moveit/task_constructor/utils.h>
|
#include <moveit/task_constructor/utils.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||||
|
#include <moveit/trajectory_processing/limit_cartesian_speed.h>
|
||||||
#include <moveit/kinematics_base/kinematics_base.h>
|
#include <moveit/kinematics_base/kinematics_base.h>
|
||||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||||
#include <tf2_eigen/tf2_eigen.h>
|
#include <tf2_eigen/tf2_eigen.h>
|
||||||
@ -121,8 +122,19 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
|
|||||||
for (const auto& waypoint : trajectory)
|
for (const auto& waypoint : trajectory)
|
||||||
result->addSuffixWayPoint(waypoint, 0.0);
|
result->addSuffixWayPoint(waypoint, 0.0);
|
||||||
|
|
||||||
|
double max_cartesian_speed = props.get<double>("max_cartesian_speed");
|
||||||
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
|
||||||
if (timing)
|
// compute timing to move the eef with constant speed
|
||||||
|
if (max_cartesian_speed > 0.0) {
|
||||||
|
if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed,
|
||||||
|
props.get<std::string>("cartesian_speed_limited_link"))) {
|
||||||
|
ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
|
||||||
|
<< props.get<std::string>("cartesian_speed_limited_link"));
|
||||||
|
} else {
|
||||||
|
ROS_ERROR_STREAM("failed to set max speed for link_ "
|
||||||
|
<< props.get<std::string>("cartesian_speed_limited_link"));
|
||||||
|
}
|
||||||
|
} else if (timing)
|
||||||
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||||
props.get<double>("max_acceleration_scaling_factor"));
|
props.get<double>("max_acceleration_scaling_factor"));
|
||||||
|
|
||||||
|
@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
|
|||||||
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
|
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
|
||||||
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
|
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
|
||||||
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
|
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
|
||||||
|
req.cartesian_speed_limited_link = p.get<std::string>("cartesian_speed_limited_link");
|
||||||
|
req.max_cartesian_speed = p.get<double>("max_cartesian_speed");
|
||||||
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
|
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() {
|
|||||||
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
|
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
|
||||||
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
|
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
|
||||||
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
|
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
|
||||||
|
p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
|
||||||
|
p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
|
||||||
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
|
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
|
||||||
}
|
}
|
||||||
} // namespace solvers
|
} // namespace solvers
|
||||||
|
@ -18,6 +18,8 @@ group = "panda_arm"
|
|||||||
# [cartesianTut2]
|
# [cartesianTut2]
|
||||||
# Cartesian and joint-space interpolation planners
|
# Cartesian and joint-space interpolation planners
|
||||||
cartesian = core.CartesianPath()
|
cartesian = core.CartesianPath()
|
||||||
|
cartesian.max_cartesian_speed = 0.1 # m/s
|
||||||
|
cartesian.cartesian_speed_limited_link = "panda_hand"
|
||||||
jointspace = core.JointInterpolationPlanner()
|
jointspace = core.JointInterpolationPlanner()
|
||||||
# [cartesianTut2]
|
# [cartesianTut2]
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user