JointTrajectoryController: setHoldPosition results in unexpected movement when restarting controller

[lprobsth]

I'm using ros_controllers in ROS melodic for controlling a robotic arm in position mode with position_controllers/JointTrajectoryController. The Controller works as expected until I switch the controllers (e.g. to velocity controller).

When the JointTrajectoryController is stopped and I switch back from a velocity controller, the TrajectoryController immediately starts moving in direction of the last known Position.

(red = actual position; blue = desired position)

I inspected the TrajectoryController with debugging and can confirm that my hardware interface sets the correct state and that the TrajectoryController uses this state in the Starting function:

ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

Lines 142 to 146 in 95ebe05

	for (unsigned int i = 0; i < getNumberOfJoints(); ++i)
	{
	desired_state_.position[i] = joints_[i].getPosition();
	desired_state_.velocity[i] = joints_[i].getVelocity();
	}

I think that setHoldPosition induces the problem - it seems to me that hold_traj_builder_ works and creates a new segment with the current state as the next point in the trajectory. But instead of replacing the current trajectory in the trajectory box with the hold trajectory, the old trajectory with the last known position from when the controller was stopped is kept in place and the new point from the hold trajectory gets placed in the future.

ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

Line 738 in 9bbdedd

curr_trajectory_box_.set(hold_trajectory_ptr_);

Then the last known state and the new hold position get interpolated by the sample function. I would have guessed that by resetting the uptime in

ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h

Lines 136 to 139 in 9bbdedd

	TimeData time_data;
	time_data.time = time;
	time_data.uptime = ros::Time(0.0);
	time_data_.initRT(time_data);

the sample function would immediatly start with the new point from the hold trajectory.

Am I missing something?
How can I reinitialize the current trajectory so that the movement starts at the current state when restarting the controller? (possible workaround: unload & reload)

[AndyZe]

I haven't dug into the code at all on a deep level, but is there any spline fitting going on for setHoldPosition? It could easily cause something like this, even if there's only a tiny position difference.