spline_smoother_utils.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#ifndef SPLINE_SMOOTHER_UTILS_H_
#define SPLINE_SMOOTHER_UTILS_H_

#include <arm_navigation_msgs/JointTrajectoryWithLimits.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <arm_navigation_msgs/JointLimits.h>
#include <spline_smoother/LSPBTrajectoryMsg.h>
#include <spline_smoother/SplineTrajectory.h>
#include <angles/angles.h>
#include <ros/ros.h>
#include <float.h>
namespace spline_smoother
{


template <typename T>
void differentiate(const std::vector<T>& x, std::vector<T>& xd);

template <typename T>
void tridiagonalSolve(std::vector<T>& a,
                      std::vector<T>& b,
                      std::vector<T>& c,
                      std::vector<T>& d,
                      std::vector<T>& x);


template <typename T>
void differentiate(const std::vector<T>& x, std::vector<T>& xd)
{
  int size = x.size();
  xd.resize(size-1);
  for (int i=0; i<size-1; ++i)
  {
    xd[i] = x[i+1] - x[i];
  }
}

template <typename T>
void tridiagonalSolve(std::vector<T>& a,
                      std::vector<T>& b,
                      std::vector<T>& c,
                      std::vector<T>& d,
                      std::vector<T>& x)
{
  int n = (int)d.size();

  x.resize(n);

  // forward elimination
  for (int i=1; i<n; i++)
  {
    double m = a[i] / b[i-1];
    b[i] -= m*c[i-1];
    d[i] -= m*d[i-1];
  }

  // backward substitution
  x[n-1] = d[n-1]/b[n-1];
  for (int i=n-2; i>=0; i--)
  {
    x[i] = (d[i] - c[i]*x[i+1])/b[i];
  }
}

template <typename T>
bool checkTrajectoryConsistency(T& waypoint_traj)
{
  unsigned int length = waypoint_traj.request.trajectory.points.size();
  unsigned int num_joints = waypoint_traj.request.trajectory.joint_names.size();

  double prev_time = -1.0;

  for (unsigned int i=0; i<length; i++)
  {
    if (waypoint_traj.request.trajectory.points[i].positions.size() != num_joints)
    {
      ROS_ERROR("Number of positions (%d) at trajectory index %d doesn't match number of joint names (%d)",
                (int) waypoint_traj.request.trajectory.points[i].positions.size(), (int) i, (int) num_joints);
      return false;
    }
    if (waypoint_traj.request.trajectory.points[i].time_from_start.toSec() < prev_time)
    {
      ROS_ERROR("Time of waypoint at trajectory index %d (%f) is not greater than or equal to the previous time (%f)",
                (int) i, waypoint_traj.request.trajectory.points[i].time_from_start.toSec(), prev_time);
      return false;
    }
    if(waypoint_traj.request.trajectory.points[i].time_from_start.toSec() < 0.0)
    {
      ROS_ERROR("Time of waypoint at trajectory index %d (%f) is negative",
                (int) i, waypoint_traj.request.trajectory.points[i].time_from_start.toSec());
      return false;
    }      
    prev_time = waypoint_traj.request.trajectory.points[i].time_from_start.toSec();
    if(waypoint_traj.request.trajectory.points[i].velocities.size() != waypoint_traj.request.trajectory.points[i].positions.size()) 
      waypoint_traj.request.trajectory.points[i].velocities.resize(num_joints, 0.0);
    if(waypoint_traj.request.trajectory.points[i].accelerations.size() != waypoint_traj.request.trajectory.points[i].positions.size()) 
      waypoint_traj.request.trajectory.points[i].accelerations.resize(num_joints, 0.0);
  }
  return true;
}

inline double jointDiff(const double &from, const double &to, const arm_navigation_msgs::JointLimits &limit)
{
  if(!limit.has_position_limits)
  {
    return angles::shortest_angular_distance(from,to);
  }
  else
  {
    return (to-from);
  }
}





bool findSplineSegment(const spline_smoother::SplineTrajectory &spline,
                       const double& time, 
                       spline_smoother::SplineTrajectorySegment& spline_segment,
                       double& segment_time, 
                       int start_index=0, 
                       int end_index=-1);

bool findSplineSegment(const spline_smoother::LSPBTrajectoryMsg &spline,
                       const double& time, 
                       spline_smoother::LSPBTrajectorySegmentMsg& spline_segment,
                       double& segment_time, 
                       int start_index=0, 
                       int end_index=-1);


bool getTotalTime(const spline_smoother::SplineTrajectory &spline, 
                  double &t);


bool getTotalTime(const spline_smoother::LSPBTrajectoryMsg &spline, 
                  double &t);

bool sampleSplineTrajectory(const spline_smoother::SplineTrajectorySegment &spline, 
                            const double& input_time, 
                            trajectory_msgs::JointTrajectoryPoint &point_out);

bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectorySegmentMsg &spline, 
                            const double& input_time, 
                            trajectory_msgs::JointTrajectoryPoint &point_out);

bool sampleSplineTrajectory(const spline_smoother::SplineTrajectory& spline, 
                            const std::vector<double> &times, 
                            trajectory_msgs::JointTrajectory& traj_out);

bool sampleSplineTrajectory(const spline_smoother::LSPBTrajectoryMsg& spline, 
                            const std::vector<double> &times, 
                            trajectory_msgs::JointTrajectory& traj_out);

bool write(const spline_smoother::SplineTrajectory &spline, 
           const std::vector<double> &times, 
           const std::string &filename);

bool writeSpline(const spline_smoother::SplineTrajectory &spline, 
                 const std::string &filename);

bool write(const spline_smoother::LSPBTrajectoryMsg &spline, 
           const std::vector<double> &times, 
           const std::string &filename);

bool writeSpline(const spline_smoother::LSPBTrajectoryMsg &spline, 
                 const std::string &filename);

    static const double MAX_ALLOWABLE_TIME = FLT_MAX;
}
#endif /* SPLINE_SMOOTHER_UTILS_H_ */