trajectory.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, 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 Willow Garage, Inc. 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 TRAJECTORY_H
#define TRAJECTORY_H
 
#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include <sstream>
#include <map>
 
#include "ros/console.h"
#include <boost/thread/condition.hpp>
 
namespace trajectory
{
  class Trajectory
  {
    public:
  
    struct TPoint
    {
 
        TPoint() {} 
        TPoint(int dimension){setDimension(dimension);}; 
        TPoint(const std::vector<double>& q, double time); 
        std::vector<double> q_; 
        std::vector<double> qdot_; 
        double time_; 
        int dimension_; 
        friend class Trajectory;
 
        void setDimension(int dimension){
          dimension_ = dimension;
          q_.resize(dimension_);
          qdot_.resize(dimension_);
        }
    };
 
    struct TCoeff
    {
        TCoeff() {}
 
        /* 
           \brief Get the coefficient corresponding to a degree in the polynomial and a dimension index */ 
        inline double get_coefficient(int degree, int dim_index); 
 
        private: 
 
        int degree_; 
        int dimension_; 
        double duration_; 
        std::vector<std::vector<double> > coeff_; 
        friend class Trajectory;
    };
  
    Trajectory(int dimension);
 
    virtual ~Trajectory() {}
 
    void clear();
 
    void addPoint(const TPoint);
 
    int setTrajectory(const std::vector<TPoint>& tp);
 
    int setTrajectory(const std::vector<double> &p, const std::vector<double> &time, int numPoints);
 
    int setTrajectory(const std::vector<double> &p, int numPoints);
 
    int setTrajectory(const std::vector<double> &p, const std::vector<double> &pdot, const std::vector<double> &time, int numPoints);
 
    int setMaxAcc(std::vector<double> max_acc);
 
 
    //inline double getTotalTime();
    double getTotalTime();
 
    int sample(TPoint &tp, double time);
 
//  void sample(std::vector<TPoint> &tp, double dT);
 
//    void sample(std::vector<TPoint> &tp, double start_time, double end_time, double dT); 
 
//  std::vector<TPoint>& getPoints() const;
 
    void setInterpolationMethod(std::string interp_method);
 
    int setMaxRates(std::vector<double> max_rate);
 
    bool autocalc_timing_;
    int getNumberPoints();
 
    int minimizeSegmentTimes();
 
    int getDuration(std::vector<double> &duration);
 
    int getDuration(int index, double &duration);
 
    int getTimeStamps(std::vector<double> &timestamps);
 
    int write(std::string filename, double dT);
 
    void setJointWraps(int index);
 
    int findTrajectorySegment(double time);
 
    void getTrajectory(std::vector<trajectory::Trajectory::TPoint> &traj, double dT);
 
    protected:
 
    std::string interp_method_; 
    private:
 
    bool max_acc_set_;
 
    bool max_rate_set_;
 
    const TPoint& lastPoint();
 
    void init(int num_points, int dimension);
 
    int num_points_; 
    int dimension_; 
    std::vector<TPoint> tp_; 
    std::vector<TCoeff> tc_; 
    std::vector<double> max_limit_;
    std::vector<double> min_limit_;
    std::vector<double> max_rate_;
    std::vector<double> max_acc_;
    std::vector<bool> joint_wraps_; 
    int parameterize();  
 
    int parameterizeLinear();
 
    int parameterizeBlendedLinear();
 
    int parameterizeCubic();
 
    int  minimizeSegmentTimesWithLinearInterpolation();
 
    int  minimizeSegmentTimesWithCubicInterpolation();
 
    int  minimizeSegmentTimesWithBlendedLinearInterpolation();
 
    void sampleLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
 
    void sampleCubic(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
 
    void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
 
    double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index);
 
    double calculateMinimumTimeLinear(const TPoint &start, const TPoint &end);
 
    double calculateMinimumTimeLSPB(const TPoint &start, const TPoint &end);
 
    double calculateMinTimeLSPB(double q0, double q1, double vmax, double amax, int index);
 
    double calculateMinimumTimeCubic(const TPoint &start, const TPoint &end);
 
    double blendTime(double aa,double bb,double cc);
 
    double jointDiff(double from, double to, int index);
 
  };
}
 
#endif