diff --git a/core/src/merge.cpp b/core/src/merge.cpp index db3b0c7c..d7a03fbb 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -155,11 +155,11 @@ merge(const std::vector& 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