JointTrajectory: Don't use shortest_angular_distance for linear joints

[bmagyar]

Fixes #432

Port #591 to Noetic

[bmagyar]

@steinmn could you please nod in agreement? ;)

[steinmn]

Nod :)

[steinmn]

On second thought, something doesn't look quite right here

[steinmn]

Suggested change

	if (is_linear_[joint_index])
	{
	state_joint_error_.position[0] = desired_joint_state_.position[0] - current_state_.position[joint_index];
	}
	else
	{
	state_joint_error_.position[0] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]);
	}
	state_joint_error_.velocity[0] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index];
	state_joint_error_.acceleration[0] = 0.0;
	state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]);
	if (is_linear_[joint_index])
	{
	state_error_.position[joint_index] = desired_joint_state_.position[0] - current_state_.position[joint_index];
	}
	else
	{
	state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]);
	}