From 13116a146cb60a7e22aa86b58b0817194f633001 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Fri, 24 Jul 2020 00:25:12 +0200
Subject: [PATCH] ensure section trajectory starts at actual position
---
src/TimedPlanner.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 9a94fbd..96108e3 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -121,6 +121,7 @@ void TimedPlanner::doMotion() {
ROS_WARN_STREAM("terminating current trajectory");
return;
}
+ group->getCurrentState()->copyJointGroupPositions("panda_arm", trajectory.joint_trajectory.points[0].positions);
ROS_INFO_STREAM(
"Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);
--
GitLab