spline_smoother: spline_smoother: spline_smoother

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00038 #ifndef SPLINE_SMOOTHER_UTILS_H_
00039 #define SPLINE_SMOOTHER_UTILS_H_
00040 
00041 #include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
00042 #include <trajectory_msgs/JointTrajectory.h>
00043 #include <arm_navigation_msgs/JointLimits.h>
00044 #include <spline_smoother/LSPBTrajectoryMsg.h>
00045 #include <spline_smoother/SplineTrajectory.h>
00046 #include <angles/angles.h>
00047 #include <ros/ros.h>
00048 #include <float.h>
00049 namespace spline_smoother
00050 {
00051 
00052 
00053 template <typename T>
00054 void differentiate(const std::vector<T>& x, std::vector<T>& xd);
00055 
00067 template <typename T>
00068 void tridiagonalSolve(std::vector<T>& a,
00069                       std::vector<T>& b,
00070                       std::vector<T>& c,
00071                       std::vector<T>& d,
00072                       std::vector<T>& x);
00073 
00075 
00076 template <typename T>
00077 void differentiate(const std::vector<T>& x, std::vector<T>& xd)
00078 {
00079   int size = x.size();
00080   xd.resize(size-1);
00081   for (int i=0; i<size-1; ++i)
00082   {
00083     xd[i] = x[i+1] - x[i];
00084   }
00085 }
00086 
00087 template <typename T>
00088 void tridiagonalSolve(std::vector<T>& a,
00089                       std::vector<T>& b,
00090                       std::vector<T>& c,
00091                       std::vector<T>& d,
00092                       std::vector<T>& x)
00093 {
00094   int n = (int)d.size();
00095 
00096   x.resize(n);
00097 
00098   // forward elimination
00099   for (int i=1; i<n; i++)
00100   {
00101     double m = a[i] / b[i-1];
00102     b[i] -= m*c[i-1];
00103     d[i] -= m*d[i-1];
00104   }
00105 
00106   // backward substitution
00107   x[n-1] = d[n-1]/b[n-1];
00108   for (int i=n-2; i>=0; i--)
00109   {
00110     x[i] = (d[i] - c[i]*x[i+1])/b[i];
00111   }
00112 }
00113 
00121 template <typename T>
00122 bool checkTrajectoryConsistency(T& waypoint_traj)
00123 {
00124   unsigned int length = waypoint_traj.trajectory.points.size();
00125   unsigned int num_joints = waypoint_traj.trajectory.joint_names.size();
00126 
00127   double prev_time = -1.0;
00128 
00129   for (unsigned int i=0; i<length; i++)
00130   {
00131     if (waypoint_traj.trajectory.points[i].positions.size() != num_joints)
00132     {
00133       ROS_ERROR("Number of positions (%d) at trajectory index %d doesn't match number of joint names (%d)",
00134                 (int) waypoint_traj.trajectory.points[i].positions.size(), (int) i, (int) num_joints);
00135       return false;
00136     }
00137     if (waypoint_traj.trajectory.points[i].time_from_start.toSec() < prev_time)
00138     {
00139       ROS_ERROR("Time of waypoint at trajectory index %d (%f) is not greater than or equal to the previous time (%f)",
00140                 (int) i, waypoint_traj.trajectory.points[i].time_from_start.toSec(), prev_time);
00141       return false;
00142     }
00143     if(waypoint_traj.trajectory.points[i].time_from_start.toSec() < 0.0)
00144     {
00145       ROS_ERROR("Time of waypoint at trajectory index %d (%f) is negative",
00146                 (int) i, waypoint_traj.trajectory.points[i].time_from_start.toSec());
00147       return false;
00148     }      
00149     prev_time = waypoint_traj.trajectory.points[i].time_from_start.toSec();
00150     if(waypoint_traj.trajectory.points[i].velocities.size() != waypoint_traj.trajectory.points[i].positions.size()) 
00151       waypoint_traj.trajectory.points[i].velocities.resize(num_joints, 0.0);
00152     if(waypoint_traj.trajectory.points[i].accelerations.size() != waypoint_traj.trajectory.points[i].positions.size()) 
00153       waypoint_traj.trajectory.points[i].accelerations.resize(num_joints, 0.0);
00154   }
00155   return true;
00156 }
00157 
00169 inline double jointDiff(const double &from, const double &to, const arm_navigation_msgs::JointLimits &limit)
00170 {
00171   if(!limit.has_position_limits)
00172   {
00173     return angles::shortest_angular_distance(from,to);
00174   }
00175   else
00176   {
00177     return (to-from);
00178   }
00179 }
00180 
00181 
00182 
00183 
00184 
00195 bool findSplineSegment(const spline_smoother::SplineTrajectory &spline,
00196                        const double& time, 
00197                        spline_smoother::SplineTrajectorySegment& spline_segment,
00198                        double& segment_time, 
00199                        int start_index=0, 
00200                        int end_index=-1);
00201 
00202 bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
00203                        const double& time, 
00204                        spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
00205                        double& segment_time, 
00206                        int start_index=0, 
00207                        int end_index=-1);
00208 
00209 
00216 bool getTotalTime(const spline_smoother::SplineTrajectory &spline, 
00217                   double &t);
00218 
00219 
00226 bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline, 
00227                   double &t);
00228 
00229 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline, 
00230                             const double& input_time, 
00231                             trajectory_msgs::JointTrajectoryPoint &point_out);
00232 
00233 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline, 
00234                             const double& input_time, 
00235                             trajectory_msgs::JointTrajectoryPoint &point_out);
00236 
00237 bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline, 
00238                             const std::vector<double> &times, 
00239                             trajectory_msgs::JointTrajectory& traj_out);
00240 
00241 bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline, 
00242                             const std::vector<double> &times, 
00243                             trajectory_msgs::JointTrajectory& traj_out);
00244 
00252 bool write(const spline_smoother::SplineTrajectory &spline, 
00253            const std::vector<double> &times, 
00254            const std::string &filename);
00255 
00262 bool writeSpline(const spline_smoother::SplineTrajectory &spline, 
00263                  const std::string &filename);
00264 
00272 bool write(const spline_smoother::LSPBTrajectoryMsg &spline, 
00273            const std::vector<double> &times, 
00274            const std::string &filename);
00275 
00282 bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline, 
00283                  const std::string &filename);
00284 
00285     static const double MAX_ALLOWABLE_TIME = FLT_MAX;
00287 }
00288 #endif /* SPLINE_SMOOTHER_UTILS_H_ */
spline_smoother_utils.h
