mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (#458)
MoveIt passes the effort field of the last trajectory point as the max_effort for a GripperCommand. Thus we pass the max_effort property to the effort field of the trajectory's last waypoint.
This commit is contained in:
parent
b346e7eb78
commit
7f10292ab3
@ -51,6 +51,8 @@ using namespace trajectory_processing;
|
||||
JointInterpolationPlanner::JointInterpolationPlanner() {
|
||||
auto& p = properties();
|
||||
p.declare<double>("max_step", 0.1, "max joint step");
|
||||
// allow passing max_effort to GripperCommand actions via
|
||||
p.declare<double>("max_effort", "max_effort for GripperCommand actions");
|
||||
}
|
||||
|
||||
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||
@ -95,6 +97,20 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||
props.get<double>("max_acceleration_scaling_factor"));
|
||||
|
||||
// set max_effort on first and last waypoint (first, because we might reverse the trajectory)
|
||||
const auto& max_effort = properties().get("max_effort");
|
||||
if (!max_effort.empty()) {
|
||||
double effort = boost::any_cast<double>(max_effort);
|
||||
for (const auto* jm : jmg->getActiveJointModels()) {
|
||||
if (jm->getVariableCount() != 1)
|
||||
continue;
|
||||
result->getFirstWayPointPtr()->dropAccelerations();
|
||||
result->getFirstWayPointPtr()->setJointEfforts(jm, &effort);
|
||||
result->getLastWayPointPtr()->dropAccelerations();
|
||||
result->getLastWayPointPtr()->setJointEfforts(jm, &effort);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user