trajectory.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TRAJECTORY_H
33 #define TRAJECTORY_H
34 
35 #include <iostream>
36 #include <iomanip>
37 #include <cmath>
38 #include <vector>
39 #include <sstream>
40 #include <map>
41 
42 #include "ros/console.h"
43 #include <boost/thread/condition.hpp>
44 
45 namespace trajectory
46 {
56  class Trajectory
57  {
58  public:
59 
63  struct TPoint
64  {
65 
66  TPoint() {}
68  TPoint(int dimension){setDimension(dimension);};
70  TPoint(const std::vector<double>& q, double time);
72  std::vector<double> q_;
74  std::vector<double> qdot_;
76  double time_;
78  int dimension_;
80  friend class Trajectory;
81 
86  void setDimension(int dimension){
87  dimension_ = dimension;
88  q_.resize(dimension_);
89  qdot_.resize(dimension_);
90  }
91  };
92 
96  struct TCoeff
97  {
98  TCoeff() {}
99 
100  /*
101  \brief Get the coefficient corresponding to a degree in the polynomial and a dimension index */
102  inline double get_coefficient(int degree, int dim_index);
103 
104  private:
105 
106  int degree_;
110  double duration_;
112  std::vector<std::vector<double> > coeff_;
114  friend class Trajectory;
115  };
116 
120  Trajectory(int dimension);
121 
125  virtual ~Trajectory() {}
126 
130  void clear();
131 
135  void addPoint(const TPoint);
136 
141  int setTrajectory(const std::vector<TPoint>& tp);
142 
149  int setTrajectory(const std::vector<double> &p, const std::vector<double> &time, int numPoints);
150 
157  int setTrajectory(const std::vector<double> &p, int numPoints);
158 
159  int setTrajectory(const std::vector<double> &p, const std::vector<double> &pdot, const std::vector<double> &time, int numPoints);
160 
161  int setMaxAcc(std::vector<double> max_acc);
162 
163 
168  //inline double getTotalTime();
169  double getTotalTime();
170 
177  int sample(TPoint &tp, double time);
178 
179 // void sample(std::vector<TPoint> &tp, double dT);
180 
181 // void sample(std::vector<TPoint> &tp, double start_time, double end_time, double dT);
182 
183 // std::vector<TPoint>& getPoints() const;
184 
189  void setInterpolationMethod(std::string interp_method);
190 
195  int setMaxRates(std::vector<double> max_rate);
196 
203  int getNumberPoints();
204 
210  int minimizeSegmentTimes();
211 
212  int getDuration(std::vector<double> &duration);
213 
214  int getDuration(int index, double &duration);
215 
216  int getTimeStamps(std::vector<double> &timestamps);
217 
218  int write(std::string filename, double dT);
219 
220  void setJointWraps(int index);
221 
227  int findTrajectorySegment(double time);
228 
229  void getTrajectory(std::vector<trajectory::Trajectory::TPoint> &traj, double dT);
230 
231  protected:
232 
233  std::string interp_method_;
235  private:
236 
238 
240 
241  const TPoint& lastPoint();
242 
243  void init(int num_points, int dimension);
244 
249  std::vector<TPoint> tp_;
251  std::vector<TCoeff> tc_;
253  std::vector<double> max_limit_;
255  std::vector<double> min_limit_;
257  std::vector<double> max_rate_;
259  std::vector<double> max_acc_;
261  std::vector<bool> joint_wraps_;
269  int parameterize();
270 
277  int parameterizeLinear();
278 
286 
293  int parameterizeCubic();
294 
300 
306 
312 
320  void sampleLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
321 
329  void sampleCubic(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
330 
338  void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time);
339 
340  double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index);
341 
347  double calculateMinimumTimeLinear(const TPoint &start, const TPoint &end);
348 
354  double calculateMinimumTimeLSPB(const TPoint &start, const TPoint &end);
355 
356  double calculateMinTimeLSPB(double q0, double q1, double vmax, double amax, int index);
357 
358  double calculateMinimumTimeCubic(const TPoint &start, const TPoint &end);
359 
360  double blendTime(double aa,double bb,double cc);
361 
362  double jointDiff(double from, double to, int index);
363 
364  };
365 }
366 
367 #endif
void clear()
clear the trajectory
Definition: trajectory.cpp:77
double calculateMinimumTimeLSPB(const TPoint &start, const TPoint &end)
calculate minimum time for a trajectory segment using LSPB.
Definition: trajectory.cpp:772
int parameterizeBlendedLinear()
calculate the coefficients for interpolation between trajectory points using blended linear interpola...
Definition: trajectory.cpp:972
std::vector< bool > joint_wraps_
Definition: trajectory.h:261
std::vector< double > max_rate_
Definition: trajectory.h:257
double calculateMinTimeLSPB(double q0, double q1, double vmax, double amax, int index)
Definition: trajectory.cpp:792
int minimizeSegmentTimes()
Minimize segment times in the trajectory Timings for the trajectory segments are automatically calcul...
Definition: trajectory.cpp:381
int parameterize()
calculate the coefficients for interpolation between trajectory points If autocalc_timing_ is true...
Definition: trajectory.cpp:816
void sampleCubic(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
Sample the trajectory based on a cubic interpolation.
Definition: trajectory.cpp:628
double getTotalTime()
Get the total time for the trajectory.
Definition: trajectory.cpp:272
std::vector< double > q_
Definition: trajectory.h:72
void sampleBlendedLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
Sample the trajectory based on a cubic interpolation.
Definition: trajectory.cpp:591
double calculateMinimumTimeLinear(const TPoint &start, const TPoint &end)
calculate minimum time for a trajectory segment using a linear interpolation
Definition: trajectory.cpp:689
int parameterizeCubic()
calculate the coefficients for interpolation between trajectory points using cubic interpolation...
Definition: trajectory.cpp:903
int minimizeSegmentTimesWithCubicInterpolation()
calculate a minimum time trajectory using cubic interpolation Timings for the trajectory are automati...
Definition: trajectory.cpp:444
int getDuration(std::vector< double > &duration)
Definition: trajectory.cpp:649
int parameterizeLinear()
calculate the coefficients for interpolation between trajectory points using linear interpolation If ...
Definition: trajectory.cpp:833
double calculateMinimumTimeCubic(const TPoint &start, const TPoint &end)
Definition: trajectory.cpp:711
int write(std::string filename, double dT)
void sampleLinear(TPoint &tp, double time, const TCoeff &tc, double segment_start_time)
Sample the trajectory based on a linear interpolation.
Definition: trajectory.cpp:571
void addPoint(const TPoint)
Add a point to the trajectory.
Definition: trajectory.cpp:250
double blendTime(double aa, double bb, double cc)
Definition: trajectory.cpp:555
std::vector< std::vector< double > > coeff_
Definition: trajectory.h:112
void init(int num_points, int dimension)
Definition: trajectory.cpp:51
std::vector< TCoeff > tc_
Definition: trajectory.h:251
int minimizeSegmentTimesWithLinearInterpolation()
calculate a minimum time trajectory using linear interpolation Timings for the trajectory are automat...
Definition: trajectory.cpp:396
virtual ~Trajectory()
Destructor.
Definition: trajectory.h:125
double calculateMinTimeCubic(double q0, double q1, double v0, double v1, double vmax, int index)
Definition: trajectory.cpp:732
void setDimension(int dimension)
Set the dimension of a trajectory point. This resizes the internal vectors to the right sizes...
Definition: trajectory.h:86
int getNumberPoints()
Get the number of points in the trajectory.
Definition: trajectory.cpp:644
void setJointWraps(int index)
Definition: trajectory.cpp:163
double jointDiff(double from, double to, int index)
Definition: trajectory.cpp:173
std::vector< double > max_limit_
Definition: trajectory.h:253
std::vector< double > min_limit_
Definition: trajectory.h:255
int getTimeStamps(std::vector< double > &timestamps)
Definition: trajectory.cpp:676
std::vector< double > max_acc_
Definition: trajectory.h:259
const TPoint & lastPoint()
Definition: trajectory.cpp:282
int sample(TPoint &tp, double time)
Sample the trajectory at a certain point in time.
Definition: trajectory.cpp:287
int findTrajectorySegment(double time)
finds the trajectory segment corresponding to a particular time
Definition: trajectory.cpp:261
std::string interp_method_
Definition: trajectory.h:233
void getTrajectory(std::vector< trajectory::Trajectory::TPoint > &traj, double dT)
std::vector< double > qdot_
Definition: trajectory.h:74
std::vector< TPoint > tp_
Definition: trajectory.h:249
int setMaxAcc(std::vector< double > max_acc)
Definition: trajectory.cpp:365
void setInterpolationMethod(std::string interp_method)
Set the interpolation method.
Definition: trajectory.cpp:810
int minimizeSegmentTimesWithBlendedLinearInterpolation()
calculate a minimum time trajectory using blended linear interpolation Timings for the trajectory are...
Definition: trajectory.cpp:497
int setMaxRates(std::vector< double > max_rate)
set the max rates (velocities)
Definition: trajectory.cpp:349
int setTrajectory(const std::vector< TPoint > &tp)
Set the trajectory using a vector of trajectory points.
Definition: trajectory.cpp:87


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Wed Jun 5 2019 19:34:08