trajectory.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #ifndef TRAJECTORY_H
00033 #define TRAJECTORY_H
00034 
00035 #include <iostream>
00036 #include <iomanip>
00037 #include <cmath>
00038 #include <vector>
00039 #include <sstream>
00040 #include <map>
00041 
00042 #include "ros/console.h"
00043 #include <boost/thread/condition.hpp>
00044 
00045 namespace trajectory
00046 {
00056   class Trajectory
00057   {
00058     public:
00059   
00063     struct TPoint
00064     {
00065 
00066         TPoint() {} 
00068         TPoint(int dimension){setDimension(dimension);}; 
00070         TPoint(const std::vector<double>& q, double time); 
00072         std::vector<double> q_; 
00074         std::vector<double> qdot_; 
00076         double time_; 
00078         int dimension_; 
00080         friend class Trajectory;
00081 
00086         void setDimension(int dimension){
00087           dimension_ = dimension;
00088           q_.resize(dimension_);
00089           qdot_.resize(dimension_);
00090         }
00091     };
00092 
00096     struct TCoeff
00097     {
00098         TCoeff() {}
00099 
00100         /* 
00101            \brief Get the coefficient corresponding to a degree in the polynomial and a dimension index */ 
00102         inline double get_coefficient(int degree, int dim_index); 
00103 
00104         private: 
00105 
00106         int degree_; 
00108         int dimension_; 
00110         double duration_; 
00112         std::vector<std::vector<double> > coeff_; 
00114         friend class Trajectory;
00115     };
00116   
00120     Trajectory(int dimension);
00121 
00125     virtual ~Trajectory() {}
00126 
00130     void clear();
00131 
00135     void addPoint(const TPoint);
00136 
00141     int setTrajectory(const std::vector<TPoint>& tp);
00142 
00149     int setTrajectory(const std::vector<double> &p, const std::vector<double> &time, int numPoints);
00150 
00157     int setTrajectory(const std::vector<double> &p, int numPoints);
00158 
00159     int setTrajectory(const std::vector<double> &p, const std::vector<double> &pdot, const std::vector<double> &time, int numPoints);
00160 
00161     int setMaxAcc(std::vector<double> max_acc);
00162 
00163 
00168     //inline double getTotalTime();
00169     double getTotalTime();
00170 
00177     int sample(TPoint &tp, double time);
00178 
00179 //  void sample(std::vector<TPoint> &tp, double dT);
00180 
00181 //    void sample(std::vector<TPoint> &tp, double start_time, double end_time, double dT); 
00182 
00183 //  std::vector<TPoint>& getPoints() const;
00184 
00189     void setInterpolationMethod(std::string interp_method);
00190  
00195     int setMaxRates(std::vector<double> max_rate);
00196 
00197     bool autocalc_timing_;
00203     int getNumberPoints();
00204 
00210     int minimizeSegmentTimes();
00211 
00212     int getDuration(std::vector<double> &duration);
00213 
00214     int getDuration(int index, double &duration);
00215 
00216     int getTimeStamps(std::vector<double> &timestamps);
00217 
00218     int write(std::string filename, double dT);
00219 
00220     void setJointWraps(int index);
00221 
00227     int findTrajectorySegment(double time);
00228 
00229     void getTrajectory(std::vector<trajectory::Trajectory::TPoint> &traj, double dT);
00230 
00231     protected:
00232 
00233     std::string interp_method_; 
00235     private:
00236 
00237     bool max_acc_set_;
00238 
00239     bool max_rate_set_;
00240 
00241     const TPoint& lastPoint();
00242 
00243     void init(int num_points, int dimension);
00244 
00245     int num_points_; 
00247     int dimension_; 
00249     std::vector<TPoint> tp_; 
00251     std::vector<TCoeff> tc_; 
00253     std::vector<double> max_limit_;
00255     std::vector<double> min_limit_;
00257     std::vector<double> max_rate_;
00259     std::vector<double> max_acc_;
00261     std::vector<bool> joint_wraps_; 
00269     int parameterize();  
00270 
00277     int parameterizeLinear();
00278 
00285     int parameterizeBlendedLinear();
00286 
00293     int parameterizeCubic();
00294 
00299     int  minimizeSegmentTimesWithLinearInterpolation();
00300 
00305     int  minimizeSegmentTimesWithCubicInterpolation();
00306 
00311     int  minimizeSegmentTimesWithBlendedLinearInterpolation();
00312 
00320     void sampleLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
00321 
00329     void sampleCubic(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
00330 
00338     void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
00339 
00340     double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index);
00341 
00347     double calculateMinimumTimeLinear(const TPoint &start, const TPoint &end);
00348 
00354     double calculateMinimumTimeLSPB(const TPoint &start, const TPoint &end);
00355 
00356     double calculateMinTimeLSPB(double q0, double q1, double vmax, double amax, int index);
00357 
00358     double calculateMinimumTimeCubic(const TPoint &start, const TPoint &end);
00359 
00360     double blendTime(double aa,double bb,double cc);
00361 
00362     double jointDiff(double from, double to, int index);
00363 
00364   };
00365 }
00366 
00367 #endif


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52