From 00331414a652c9a2a8aa38ed2a09ac3faba72774 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 2 Apr 2020 16:31:06 +0200 Subject: [PATCH] merge: update states only once per waypoint --- core/src/merge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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