harmonizePositions()

moveit_core/robot_model/include/moveit/robot_model/joint_model.h

@@ -297,6 +297,14 @@ class JointModel
       Return true if changes were made. */
   virtual bool enforcePositionBounds(double* values, const Bounds& other_bounds) const = 0;
 
+  /** Harmonize position of revolute joints, adding/subtracting multiples of 2*Pi to bring them back into bounds.
+   *  Return true if changes were made. */
+  bool harmonizePosition(double *values, const Bounds &other_bounds) const;
+  bool harmonizePosition(double *values) const
+  {
+    return harmonizePosition(values, variable_bounds_);
+  }
+
   /** \brief Check if the set of velocities for the variables of this joint are within bounds. */
   bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
   {

moveit_core/robot_model/src/joint_model.cpp

@@ -90,6 +90,31 @@ int JointModel::getLocalVariableIndex(const std::string& variable) const
   return it->second;
 }
 
+bool JointModel::harmonizePosition(double* values, const Bounds& other_bounds) const
+{
+  if (type_ != REVOLUTE)
+    return false;
+
+  bool modified = false;
+  if (*values < other_bounds[0].min_position_)
+  {
+    while (*values + 2 * M_PI <= other_bounds[0].max_position_)
+    {
+      *values += 2 * M_PI;
+      modified = true;
+    }
+  }
+  else if (*values > other_bounds[0].max_position_)
+  {
+    while (*values - 2 * M_PI >= other_bounds[0].min_position_)
+    {
+      *values -= 2 * M_PI;
+      modified = true;
+    }
+  }
+  return modified;
+}
+
 bool JointModel::enforceVelocityBounds(double* values, const Bounds& other_bounds) const
 {
   bool change = false;
@@ -234,4 +259,4 @@ std::ostream& operator<<(std::ostream& out, const VariableBounds& b)
 }
 
 }  // end of namespace core
-}  // end of namespace moveit
+}  // end of namespace moveit

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1511,6 +1511,15 @@ as the new values that correspond to the group */
       updateMimicJoint(joint);
     }
   }
+  void harmonizePositions();
+  void harmonizePositions(const JointModelGroup* joint_group);
+  void harmonizePosition(const JointModel *joint)
+  {
+    if (joint->harmonizePosition(position_ + joint->getFirstVariableIndex()))
+      // no need to mark transforms dirty, as the transform hasn't changed
+      updateMimicJoint(joint);
+  }
+
   void enforceVelocityBounds(const JointModel* joint)
   {
     joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());

moveit_core/robot_state/src/robot_state.cpp

@@ -736,6 +736,18 @@ void RobotState::enforceBounds(const JointModelGroup* joint_group)
     enforceBounds(jm[i]);
 }
 
+void RobotState::harmonizePositions()
+{
+  for (const JointModel* jm : robot_model_->getActiveJointModels())
+    harmonizePosition(jm);
+}
+
+void RobotState::harmonizePositions(const JointModelGroup* joint_group)
+{
+  for (const JointModel* jm : joint_group->getActiveJointModels())
+    harmonizePosition(jm);
+}
+
 std::pair<double, const JointModel*> RobotState::getMinDistanceToPositionBounds() const
 {
   return getMinDistanceToPositionBounds(robot_model_->getActiveJointModels());

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints.cpp

@@ -369,6 +369,7 @@ void MotionPlanningFrameJointsWidget::jogNullspace(double value)
   model->getRobotState().copyJointGroupPositions(model->getJointModelGroup(), values);
   values += value * nullspace_.col(index);
   model->getRobotState().setJointGroupPositions(model->getJointModelGroup(), values);
+  model->getRobotState().harmonizePositions(model->getJointModelGroup());
   triggerUpdate(model);
 }