merge: update states only once per waypoint

This commit is contained in:
v4hn 2020-04-02 16:31:06 +02:00 committed by Robert Haschke
parent 8834ce18f7
commit 00331414a6

View File

@ -155,11 +155,11 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
merged_state->update();
}
if (finished)
break;
merged_state->update();
// add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint