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:
Robert Haschke 2023-05-04 15:29:53 +02:00 committed by GitHub
parent b346e7eb78
commit 7f10292ab3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -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;
}