Trajectory Interpolator (Against Kinetic Devel)

descartes_utilities/CMakeLists.txt

@@ -12,7 +12,10 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES descartes_utilities
-  CATKIN_DEPENDS descartes_core descartes_trajectory trajectory_msgs
+  CATKIN_DEPENDS 
+    descartes_core 
+    descartes_trajectory 
+    trajectory_msgs
 )
 
 include_directories(
@@ -22,7 +25,10 @@ include_directories(
 
 add_library(${PROJECT_NAME}
   src/ros_conversions.cpp
+  src/spline_interpolator.cpp
+  src/parameterization.cpp
 )
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 add_dependencies(${PROJECT_NAME} 
   ${catkin_EXPORTED_TARGETS}
@@ -42,3 +48,12 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
 )
+
+if(CATKIN_ENABLE_TESTING)
+  set(UTEST_SRC_FILES 
+      test/utest.cpp
+      test/parameterization_tests.cpp
+  )
+  catkin_add_gtest(${PROJECT_NAME}_utest ${UTEST_SRC_FILES})
+  target_link_libraries(${PROJECT_NAME}_utest ${PROJECT_NAME})
+endif()

descartes_utilities/include/descartes_utilities/parameterization.h

@@ -0,0 +1,86 @@
+/*
+ * Software License Agreement (Apache License)
+ *
+ * Copyright (c) 2016, Southwest Research Institute
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/*
+ *  parameterization.h
+ *
+ *  Created on: March 28, 2016
+ *  Author: Jonathan Meyer
+ */
+
+#ifndef DESCARTES_PARAMETERIZATION_H
+#define DESCARTES_PARAMETERIZATION_H
+
+#include <trajectory_msgs/JointTrajectory.h>
+#include "descartes_utilities/spline_interpolator.h"
+
+namespace descartes_utilities
+{
+
+/**
+ * @brief Given an input trajectory with positions and time_from_start specified,
+ *        generates a set of cubic splines that describe the behavior of each 
+ *        joint.
+ *        
+ * @param traj Input trajectory with positions and times specified.  
+ * @param splines If successful, this field will be populated with splines for each
+ *                joint.
+ * @return True on success, false otherwise
+ */
+bool toCubicSplines(const std::vector<trajectory_msgs::JointTrajectoryPoint>& traj,
+                    std::vector<SplineInterpolator>& splines);
+
+/**
+ * Calculates velocity and acceleration values for a joint trajectory based on natural cubic splines
+ * fit to the input trajectory. This function assumes that the 'positions' field of the trajectory
+ * has been filled out for each point and that the degrees of freedom does not change.
+ * @param  traj The input trajectory on which to act; assumed to have 'positions' & 'time_from_start'
+ *              fields filled out. This function will populate 'velocities' and 'accelerations'.
+ * @return      True if the operation succeeded. False otherwise.
+ */ 
+bool setDerivatesFromSplines(std::vector<trajectory_msgs::JointTrajectoryPoint>& traj);
+
+/**
+ * Calculates velocity and acceleration values for a joint trajectory based on natural cubic splines
+ * provided by the caller. This function assumes that the 'positions' field of the trajectory
+ * has been filled out for each point and that the degrees of freedom does not change.
+ * @param  splines Set of splines describing the behaviour of each joint as it moves through the
+ *                 input trajectory.
+ * @param  traj The input trajectory on which to act; assumed to have 'positions' & 'time_from_start'
+ *              fields filled out. This function will populate 'velocities' and 'accelerations'.
+ * @return      True if the operation succeeded. False otherwise.
+ */
+bool setDerivatesFromSplines(const std::vector<SplineInterpolator>& splines,
+                             std::vector<trajectory_msgs::JointTrajectoryPoint>& traj);
+/**
+ * Given a set of splines describing the motion of each joint as it moves through a trajectory,
+ * sample this spline at regular intervals to generate a new trajectory.
+ * @param  splines  Sequence of splines describing the motion of each joint; 
+ *                  perhaps calculated by 'toCubicSplines'
+ * @param  start_tm The time value of the trajectory at which to start sampling
+ * @param  end_tm   The time value of the trajectory at which to end sampling
+ * @param  tm_step  The interval at which to sample times between start & end
+ * @param  traj     The new, resampled, output trajectory
+ * @return          True on success; If false, 'traj' is untouched.
+ */
+bool resampleTrajectory(const std::vector<SplineInterpolator>& splines,
+                        double start_tm, double end_tm, double tm_step,
+                        std::vector<trajectory_msgs::JointTrajectoryPoint>& traj);
+
+}
+
+#endif

descartes_utilities/include/descartes_utilities/ros_conversions.h

@@ -34,15 +34,26 @@ namespace descartes_utilities
  * @brief Converts a sequence of Descartes joint trajectory points to ROS trajectory points.
  *        Copies timing if specified, and sets vel/acc/effort fields to zeros.
  * @param model Descartes robot model associated with the Descartes joint trajectory
- * @param joint_traj Sequence of 'joint trajectory points' as returned by Dense/Sparse planner
+ * @param joint_traj Sequence of 'joint trajectory points' as returned by Dense/Sparse planner 
  * @param default_joint_vel If a point, does not have timing specified, this value (in rads/s)
  *                          is used to calculate a 'default' time. Must be > 0 & less than 100.
  * @param out Buffer in which to store the resulting ROS trajectory. Only overwritten on success.
  * @return True if the conversion succeeded. False otherwise.
  */
 bool toRosJointPoints(const descartes_core::RobotModel& model,
-                      const std::vector<descartes_core::TrajectoryPtPtr>& joint_traj, double default_joint_vel,
+                      const std::vector<descartes_core::TrajectoryPtPtr>& joint_traj,
+                      double default_joint_vel,
                       std::vector<trajectory_msgs::JointTrajectoryPoint>& out);
+
+/**
+ * Calculates velocity and acceleration values for a joint trajectory based on natural cubic splines
+ * fit to the input trajectory. This function assumes that the 'positions' field of the trajectory
+ * has been filled out for each point and that the degrees of freedom does not change.
+ * @param  traj The input trajectory on which to act; assumed to have 'positions' field filled out.
+ *              This function will populate 'velocities' and 'accelerations'.
+ * @return      True if the operation succeeded. False otherwise.
+ */
+bool parameterizeVelocityAcceleration(std::vector<trajectory_msgs::JointTrajectoryPoint>& traj);
 }
 
 #endif

descartes_utilities/include/descartes_utilities/spline_interpolator.h

@@ -0,0 +1,162 @@
+/*
+ * Software License Agreement (Apache License)
+ *
+ * Copyright (c) 2016, Southwest Research Institute
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/*
+ *  spline_interpolator.h
+ *
+ *  Created on: March 25, 2016
+ *  Author: Jonathan Meyer
+ */
+
+#ifndef DESCARTES_SPLINE_INTERPOLATOR_H
+#define DESCARTES_SPLINE_INTERPOLATOR_H
+
+#include <vector>
+
+namespace descartes_utilities
+{
+
+/**
+ * Helper struct to store the parameters of a cubic spline:
+ *   f(x) = a + bx + cx^2 + dx^3
+ */
+struct CubicSplineParams
+{
+  CubicSplineParams() = default;
+  CubicSplineParams(double a, double b, double c, double d)
+    : coeffs {a,b,c,d}
+  {}
+
+  double coeffs[4];
+
+  double a() const { return coeffs[0]; }
+  double b() const { return coeffs[1]; }
+  double c() const { return coeffs[2]; }
+  double d() const { return coeffs[3]; }
+
+  /**
+   * Computes f(x) at given x
+   */
+  double position(double x) const;
+
+  /**
+   * Computes df/dx at given x
+   */
+  double velocity(double x) const;
+
+  /**
+   * Computes ddf/ddx at given x
+   */
+  double acceleration(double x) const;
+
+  /**
+   * Computes the 'x' position that maximizes the velocity
+   */
+  double xMaxVelocity() const;
+
+  /**
+   * Computes the 'x' position that maximizes the acceleration
+   */
+  double xMaxAcceleration() const;
+};
+
+/**
+ * Builds a sequence of cubic splines between each pair of sequential inputs
+ * such that positions are obeyed absolutely, velocities are smooth & continous
+ * and accelerations are continous.
+ */
+class SplineInterpolator
+{
+public:
+  /**
+   * The constructor builds the set of cubic splines for an input function defined by
+   * 'x_data', and 'y_data'. 'x_data' and 'y_data' are assumed to be the same length (> 1).
+   * 'x_data' must be sorted in ascending order.  
+   */
+  SplineInterpolator(const std::vector<double>& x_data, const std::vector<double>& y_data,
+                     double init_velocity = 0.0, double final_velocity = 0.0);
+
+  /**
+   * Query the position of the curve at a given time or 'x' coordinate. Times outside the
+   * bounds determined by the first/last point in 'x_data' are interpolated based on the
+   * first/last cubic spline segment respectively.
+   */
+  double position(double x) const;
+
+  /**
+   * Query the velocity of the curve at a given time or 'x' coordinate. Times outside the
+   * bounds determined by the first/last point in 'x_data' are interpolated based on the
+   * first/last cubic spline segment respectively.
+   */
+  double velocity(double x) const;
+
+  /**
+   * Query the acceleration of the curve at a given time or 'x' coordinate. Times outside the
+   * bounds determined by the first/last point in 'x_data' are interpolated based on the
+   * first/last cubic spline segment respectively.
+   */
+  double acceleration(double x) const;
+
+  /**
+   * Returns the maximum velocity in the spline-interpolated trajectory.
+   * @return A std::pair where the first element is the magnitude of the maximum velocity
+   *         and the second value is the trajectory segment in which this velocity occurs.
+   */
+  std::pair<double, unsigned> maxVelocity() const;
+
+  /**
+   * Returns the maximum acceleration in the spline-interpolated trajectory.
+   * @return A std::pair where the first element is the magnitude of the maximum acceleration
+   *         and the second value is the trajectory segment in which this velocity occurs.
+   */  
+  std::pair<double, unsigned> maxAcceleration() const;
+
+  /**
+   * Returns a reference to the 'x_data' used to construct the cubic splines.
+   */
+  const std::vector<double> xData() const { return x_data_; }
+
+  /**
+   * Returns a reference to the cubic spline segments constructed from 'x_data' and 'y_data'. 
+   */
+  const std::vector<CubicSplineParams> splineParams() const { return spline_params_; }
+
+private:
+  /**
+   * Returns the cubic segment in which 'x' resides. If x is greater than the
+   * last x value in the input, the last segment is returned. If x is less than
+   * the x value in the input, the first is returned.
+   */
+  unsigned search(double x) const;
+
+  /**
+   * Returns the maximum velocity for a given spline segment.
+   */
+  double maxSegmentVelocity(unsigned idx) const;
+
+  /**
+   * Returns the maximum acceleration for a given spline segment.
+   */
+  double maxSegmentAcceleration(unsigned idx) const;
+
+  std::vector<double> x_data_;
+  std::vector<CubicSplineParams> spline_params_;
+};
+
+}
+
+#endif

descartes_utilities/package.xml

@@ -11,6 +11,7 @@
   <maintainer email="[email protected]">Jonathan Meyer</maintainer>
 
   <license>Apache 2.0</license>
+  <license>BSD</license>
 
   <buildtool_depend>catkin</buildtool_depend>