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() {
|
JointInterpolationPlanner::JointInterpolationPlanner() {
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("max_step", 0.1, "max joint step");
|
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*/) {}
|
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"),
|
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||||
props.get<double>("max_acceleration_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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user