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