Revision history [back]

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.

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.

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_controllers/issues/261