From 52c409ad36896f984f512e666df06d585717b9ae Mon Sep 17 00:00:00 2001 From: John Morris Date: Thu, 17 Sep 2020 16:29:52 -0500 Subject: [PATCH] Reset trajectory generator after instantiating object --- .../src/follow_joint_trajectory_controller_handle.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index 3862eaa878..5d8705df1e 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -160,6 +160,10 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro kMaxDuration, trackjt_current_joint_states[point], trackjt_goal_joint_states[point], limits, kPositionTolerance, kUseHighSpeedMode); + traj_gen.reset(kTimestep, trackjt_desired_durations[point], + kMaxDuration, trackjt_current_joint_states[point], + trackjt_goal_joint_states[point], limits, kPositionTolerance, + kUseHighSpeedMode); auto start = std::chrono::system_clock::now(); error_code = traj_gen.generateTrajectories(&output_trajectories);