Refactor populateVelocities

play_motion/include/play_motion/play_motion.h

@@ -126,13 +126,6 @@ namespace play_motion
     void updateControllersCb(const ControllerUpdater::ControllerStates& states,
                              const ControllerUpdater::ControllerJoints& joints);
 
-    /// \brief Populate velocities to trajectory waypoints not specifying them.
-    /// \param traj_in Input trajectory
-    /// \param traj_out Output trajectory. Has complete velocity specification.
-    /// \note It's allowed to alias \p traj_in and \traj_out; that is, to provide the same trajectory instance
-    /// as input and output parameters for in-place, work.
-    void populateVelocities(const Trajectory& traj_in, Trajectory& traj_out);
-
     ros::NodeHandle                  nh_;
     ControllerList                   move_joint_groups_;
     std::map<std::string, double>    joint_states_;

play_motion/include/play_motion/play_motion_helpers.h

@@ -106,8 +106,46 @@ namespace play_motion
                       const JointNames &sourceJoints, const TrajPoint &sourcePoint,
                       double tolerance = 0.15);
 
+  /**
+   * @brief Populate joint velocity information of a trajectory waypoint.
+   *
+   * Joint velocities are computed by numeric differentiation of position information, except in the following cases,
+   * where it is set to zero:
+   *
+   * - \f$p_i = p_{i-1}\f$
+   * - \f$p_i < p_{i-1}\f$ and \f$p_i \leq p_{i+1}\f$
+   * - \f$p_i > p_{i-1}\f$ and \f$p_i \geq p_{i+1}\f$
+   *
+   * where \f$p_{i-1}\f$, \f$p_i\f$ and \f$p_{i+1}\f$ are the positions of a joint at the previous, current and next
+   * waypoint. This heuristic is well suited for direction reversals and position holding without overshoot, and
+   * produces better results than pure numeric differentiation.
+   *
+   * If the input waypoint already contains a valid velocity specification, it will \e not be overwritten, that is, this
+   * method will be a no-op.
+   *
+   * @param[in]  point_prev Previous trajectory waypoint.
+   * @param[in]  point_next Next trajectory waypoint.
+   * @param[out] point_curr Trajectory waypoint to which velocity information will be added.
+   *
+   * \sa populateVelocities(const Trajectory&, Trajectory&)
+   */
+  void populateVelocities(const TrajPoint& point_prev,
+                          const TrajPoint& point_next,
+                                TrajPoint& point_curr);
 
-
+  /**
+   * @brief Populate joint velocity information of a trajectory.
+   *
+   * Joint velocities will be computed for all waypoints not containing a valid velocity specification. Waypoints with
+   * an existing velocity specification will not be modified. If the trajectory endpoints don't specify velocites, they
+   * will be set to zero.
+   *
+   * @param[in] traj_in Input trajectory. Some waypoints may have a velocity specification (or not at all).
+   * @param[out] traj_out Output trajectory. All waypoints have a velocity specification. Can be the same instance as
+   * \c traj_in.
+   *
+   * \sa populateVelocities(const TrajPoint&, const TrajPoint&, TrajPoint&)
+   */
   void populateVelocities(const Trajectory& traj_in, Trajectory& traj_out);
 
   void getMotion(const ros::NodeHandle &nh, const std::string &motion_id,

play_motion/src/play_motion.cpp

@@ -276,12 +276,6 @@ next_joint:;
     }
   }
 
-  void PlayMotion::populateVelocities(const Trajectory& traj_in, Trajectory& traj_out)
-  {
-    ::play_motion::populateVelocities (traj_in, traj_out);
-  }
-
-
   bool PlayMotion::run(const std::string& motion_name, const ros::Duration& duration,
                        GoalHandle& goal_hdl, const Callback& cb)
   {

play_motion/src/play_motion_helpers.cpp

@@ -114,73 +114,65 @@ namespace play_motion
     }
   }
 
-  void populateVelocities(const Trajectory& traj_in, Trajectory& traj_out)
+  void populateVelocities(const TrajPoint& point_prev,
+                          const TrajPoint& point_next,
+                                TrajPoint& point_curr)
   {
-    if (traj_in.empty()) {return;}
+    const int num_joints = point_curr.positions.size();
+    assert(num_joints == point_prev.positions.size() && num_joints == point_next.positions.size());
 
-    const int num_waypoints = traj_in.size();
-    const int num_joints    = traj_in.front().positions.size();
-
-    // Initialize first and last points with zero velocity, if unspecified or not properly sized:
-    TrajPoint& point_first = traj_out.front();
-    TrajPoint& point_last  = traj_out.back();
+    // Do nothing if waypoint contains a valid velocity specification
+    if (int(point_curr.velocities.size()) == num_joints) {return;}
 
-    if (int(point_first.velocities.size()) != num_joints)
-    {
-      point_first.velocities.resize(num_joints, 0.0);
-    }
-    if (int(point_last.velocities.size())  != num_joints)
-    {
-      point_last.velocities.resize(num_joints, 0.0);
-    }
+    // Initialize joint velocities to zero
+    std::vector<double>& vel_out = point_curr.velocities;
+    vel_out.resize(num_joints, 0.0);
 
-    // Iterate over all waypoints except the first and last
-    for (int i = 1; i < num_waypoints - 1; ++i)
+    // Set individual joint velocities
+    for (int i = 0; i < num_joints; ++i)
     {
-      std::vector<double>& vel_out = traj_out[i].velocities;
-      const TrajPoint& point_curr = traj_in[i];
-      const TrajPoint& point_prev = traj_in[i - 1];
-      const TrajPoint& point_next = traj_in[i + 1];
+      const double pos_curr = point_curr.positions[i];
+      const double pos_prev = point_prev.positions[i];
+      const double pos_next = point_next.positions[i];
 
-      // Do nothing if waypoint contains a velocity specification, otherwise initialize to zero and continue
-      if (int(point_curr.velocities.size()) != num_joints)
+      if ( (pos_curr == pos_prev)                        ||
+           (pos_curr < pos_prev && pos_curr <= pos_next) ||
+           (pos_curr > pos_prev && pos_curr >= pos_next) )
       {
-        vel_out.resize(num_joints, 0.0);
+        vel_out[i] = 0.0; // Special cases where zero velocity is enforced
       }
-      else // Waypoint already specifies a velocity vector of the appropriate size
+      else
       {
-        return;
-      }
+        // General case using numeric differentiation
+        const double t_prev = point_curr.time_from_start.toSec() - point_prev.time_from_start.toSec();
+        const double t_next = point_next.time_from_start.toSec() - point_curr.time_from_start.toSec();
 
-      // Iterate over all joints in a waypoint
-      for (int j = 0; j < num_joints; ++j)
-      {
-        const double pos_curr = point_curr.positions[j];
-        const double pos_prev = point_prev.positions[j];
-        const double pos_next = point_next.positions[j];
-
-        if ( (pos_curr == pos_prev)                        ||
-             (pos_curr < pos_prev && pos_curr <= pos_next) ||
-             (pos_curr > pos_prev && pos_curr >= pos_next) )
-        {
-          // Special case where zero velocity is enforced
-          vel_out[j] = 0.0;
-        }
-        else
-        {
-          // General case using numeric differentiation
-          const double t_prev = point_curr.time_from_start.toSec() - point_prev.time_from_start.toSec();
-          const double t_next = point_next.time_from_start.toSec() - point_curr.time_from_start.toSec();
-
-          const double v_prev = (pos_curr - pos_prev)/t_prev;
-          const double v_next = (pos_next - pos_curr)/t_next;
-
-          vel_out[j] = 0.5*(v_prev + v_next);
-        }
+        const double v_prev = (pos_curr - pos_prev)/t_prev;
+        const double v_next = (pos_next - pos_curr)/t_next;
+
+        vel_out[i] = 0.5 * (v_prev + v_next);
       }
     }
   }
 
+  void populateVelocities(const Trajectory& traj_in, Trajectory& traj_out)
+  {
+    if (traj_in.empty()) {return;}
+
+    const int num_waypoints = traj_in.size();
+    const int num_joints    = traj_in.front().positions.size();
+
+    // Initialize first and last points with zero velocity, if unspecified or not properly sized
+    TrajPoint& point_first = traj_out.front();
+    TrajPoint& point_last  = traj_out.back();
+
+    if (int(point_first.velocities.size()) != num_joints) {point_first.velocities.resize(num_joints, 0.0);}
+    if (int(point_last.velocities.size())  != num_joints) {point_last.velocities.resize(num_joints, 0.0);}
+
+    // Populate velocities for remaining points (all but first and last)
+    for (int i = 1; i < num_waypoints - 1; ++i) {populateVelocities(traj_in[i - 1], traj_in[i + 1], traj_out[i]);}
+  }
+
   ros::Duration getMotionDuration(const ros::NodeHandle &nh, const std::string &motion_id)
   {
     Trajectory traj;

play_motion/src/play_motion_main.cpp

@@ -43,16 +43,24 @@
 int main(int argc, char** argv)
 {
   typedef boost::shared_ptr<play_motion::PlayMotion> PlayMotionPtr;
+
   // Init the ROS node
   ros::init(argc, argv, "play_motion");
 
   // Node handle scoped to where the poses are specified
   ros::NodeHandle nh;
 
-  ROS_INFO("starting");
-  // Initialize the actionlib server
-  play_motion::PlayMotionServer pms(nh, PlayMotionPtr(new play_motion::PlayMotion(nh)));
+  try
+  {
+    // Initialize the actionlib server
+    play_motion::PlayMotionServer pms(nh, PlayMotionPtr(new play_motion::PlayMotion(nh)));
 
-  // guruguru
-  ros::spin();
+    // guruguru
+    ros::spin();
+  }
+  catch(const ros::Exception& ex)
+  {
+    ROS_FATAL_STREAM(ex.what());
+    return EXIT_FAILURE;
+  }
 }