It looks like the controller is calling `shortest_angular_distance`

on the difference between the current and targeted position (from https://github.com/ros/angles or /opt/ros/kinetic/include/angles/angles.h)

```
ros_controllers$ grep -r shortest_angular_distance
joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_segment.h: Scalar dist = angles::shortest_angular_distance(prev_position, next_position);
joint_trajectory_controller/test/joint_trajectory_controller_test.cpp: // that it has to wrap when in fact it doesn't. This comes from a call to angles::shortest_angular_distance, which
effort_controllers/src/joint_position_controller.cpp: angles::shortest_angular_distance_with_limits(
effort_controllers/src/joint_position_controller.cpp: error = angles::shortest_angular_distance(current_position, command_position);
velocity_controllers/src/joint_position_controller.cpp: angles::shortest_angular_distance_with_limits(
velocity_controllers/src/joint_position_controller.cpp: error = angles::shortest_angular_distance(current_position, command_position);
```

angles.h:

```
static inline double normalize_angle(double angle)
{
double a = normalize_angle_positive(angle);
if (a > M_PI)
a -= 2.0 *M_PI;
return a;
}
/*!
* \function
* \brief shortest_angular_distance
*
* Given 2 angles, this returns the shortest angular
* difference. The inputs and ouputs are of course radians.
*
* The result
* would always be -pi <= result <= pi. Adding the result
* to "from" will always get you an equivelent angle to "to".
*/
static inline double shortest_angular_distance(double from, double to)
{
return normalize_angle(to-from);
}
```

I think a solution would involve creating a new joint type that doesn't do the the wrapping.

Started an issue here:
https://github.com/ros-controls/ros_c...

Hi, were you able to get a solution? Facing the same problem in my setup (Ubuntu 16.04, Gazebo 7). Thanks

No I haven't looked into it. I suspect this needs to be fixed in the source code https://github.com/ros-controls/ros_c...