add API for passing RNG to setToRandomPositionsNearBy (#2799)

Co-authored-by: Peter <peter@armnova>

MIGRATION.md

@@ -7,6 +7,7 @@ API changes in MoveIt releases
     - `getFrameTransform()` now returns this pose instead of the first shape's pose.
     - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest.
     - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required.
+- add API for passing RNG to setToRandomPositionsNearBy
 - Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
 - Planned trajectories are *slow* by default.

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

@@ -1315,6 +1315,14 @@ class RobotState
    *  Resulting values are clamped within default bounds. */
   void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);
 
+  /** \brief Set all joints in \e group to random values near the value in \near, using a specified random number generator.
+   *  \e distance is the maximum amount each joint value will vary from the
+   *  corresponding value in \e near.  \distance represents meters for
+   *  prismatic/postitional joints and radians for revolute/orientation joints.
+   *  Resulting values are clamped within default bounds. */
+  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
+                                  random_numbers::RandomNumberGenerator& rng);
+
   /** \brief Set all joints in \e group to random values near the value in \near.
    *  \e distances \b MUST have the same size as \c
    *  group.getActiveJointModels().  Each value in \e distances is the maximum
@@ -1325,6 +1333,16 @@ class RobotState
   void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
                                   const std::vector<double>& distances);
 
+  /** \brief Set all joints in \e group to random values near the value in \near, using a specified random number generator.
+   *  \e distances \b MUST have the same size as \c
+   *  group.getActiveJointModels().  Each value in \e distances is the maximum
+   *  amount the corresponding active joint in \e group will vary from the
+   *  corresponding value in \e near.  \distance represents meters for
+   *  prismatic/postitional joints and radians for revolute/orientation joints.
+   *  Resulting values are clamped within default bounds. */
+  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
+                                  const std::vector<double>& distances, random_numbers::RandomNumberGenerator& rng);
+
   /** @} */
 
   /** \name Updating and getting transforms

moveit_core/robot_state/src/robot_state.cpp

@@ -311,6 +311,13 @@ void RobotState::setToRandomPositionsNearBy(const JointModelGroup* group, const
   // we do not make calls to RobotModel for random number generation because mimic joints
   // could trigger updates outside the state of the group itself
   random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
+  setToRandomPositionsNearBy(group, seed, distances, rng);
+}
+
+void RobotState::setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
+                                            const std::vector<double>& distances,
+                                            random_numbers::RandomNumberGenerator& rng)
+{
   const std::vector<const JointModel*>& joints = group->getActiveJointModels();
   assert(distances.size() == joints.size());
   for (std::size_t i = 0; i < joints.size(); ++i)
@@ -327,6 +334,12 @@ void RobotState::setToRandomPositionsNearBy(const JointModelGroup* group, const
   // we do not make calls to RobotModel for random number generation because mimic joints
   // could trigger updates outside the state of the group itself
   random_numbers::RandomNumberGenerator& rng = getRandomNumberGenerator();
+  setToRandomPositionsNearBy(group, seed, distance, rng);
+}
+
+void RobotState::setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
+                                            random_numbers::RandomNumberGenerator& rng)
+{
   const std::vector<const JointModel*>& joints = group->getActiveJointModels();
   for (const JointModel* joint : joints)
   {