$search
#include <trajectory.h>
Classes | |
struct | TCoeff |
struct | TPoint |
Public Member Functions | |
void | addPoint (const TPoint) |
Add a point to the trajectory. | |
void | clear () |
clear the trajectory | |
int | findTrajectorySegment (double time) |
finds the trajectory segment corresponding to a particular time | |
int | getDuration (int index, double &duration) |
int | getDuration (std::vector< double > &duration) |
int | getNumberPoints () |
Get the number of points in the trajectory. | |
int | getTimeStamps (std::vector< double > ×tamps) |
double | getTotalTime () |
Get the total time for the trajectory. | |
void | getTrajectory (std::vector< trajectory::Trajectory::TPoint > &traj, double dT) |
int | minimizeSegmentTimes () |
Minimize segment times in the trajectory Timings for the trajectory segments are automatically calculated using max rate and/or max accn information based on the current value of interp_method_;. | |
int | sample (TPoint &tp, double time) |
Sample the trajectory at a certain point in time. | |
void | setInterpolationMethod (std::string interp_method) |
Set the interpolation method. | |
void | setJointWraps (int index) |
int | setMaxAcc (std::vector< double > max_acc) |
int | setMaxRates (std::vector< double > max_rate) |
set the max rates (velocities) | |
int | setTrajectory (const std::vector< double > &p, const std::vector< double > &pdot, const std::vector< double > &time, int numPoints) |
int | setTrajectory (const std::vector< double > &p, int numPoints) |
Set the trajectory using a vector of values, timestamps are not specified and so autocalc_timing_ is set to true within this function. Max rates and max accelerations should be set before this function is called. | |
int | setTrajectory (const std::vector< double > &p, const std::vector< double > &time, int numPoints) |
Set the trajectory using a vector of values and timestamps. | |
int | setTrajectory (const std::vector< TPoint > &tp) |
Set the trajectory using a vector of trajectory points. | |
Trajectory (int dimension) | |
Constructor for instantiation of the class by specifying the dimension. | |
int | write (std::string filename, double dT) |
virtual | ~Trajectory () |
Destructor. | |
Public Attributes | |
bool | autocalc_timing_ |
Protected Attributes | |
std::string | interp_method_ |
Private Member Functions | |
double | blendTime (double aa, double bb, double cc) |
double | calculateMinimumTimeCubic (const TPoint &start, const TPoint &end) |
double | calculateMinimumTimeLinear (const TPoint &start, const TPoint &end) |
calculate minimum time for a trajectory segment using a linear interpolation | |
double | calculateMinimumTimeLSPB (const TPoint &start, const TPoint &end) |
calculate minimum time for a trajectory segment using LSPB. | |
double | calculateMinTimeCubic (double q0, double q1, double v0, double v1, double vmax, int index) |
double | calculateMinTimeLSPB (double q0, double q1, double vmax, double amax, int index) |
void | init (int num_points, int dimension) |
double | jointDiff (double from, double to, int index) |
const TPoint & | lastPoint () |
int | minimizeSegmentTimesWithBlendedLinearInterpolation () |
calculate a minimum time trajectory using blended linear interpolation Timings for the trajectory are automatically calculated using max rate information. | |
int | minimizeSegmentTimesWithCubicInterpolation () |
calculate a minimum time trajectory using cubic interpolation Timings for the trajectory are automatically calculated using max rate information. | |
int | minimizeSegmentTimesWithLinearInterpolation () |
calculate a minimum time trajectory using linear interpolation Timings for the trajectory are automatically calculated using max rate information. | |
int | parameterize () |
calculate the coefficients for interpolation between trajectory points If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate and/or max accn information Thus, the time duration for any trajectory segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints. | |
int | parameterizeBlendedLinear () |
calculate the coefficients for interpolation between trajectory points using blended linear interpolation. If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate and max acceleration information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints. | |
int | parameterizeCubic () |
calculate the coefficients for interpolation between trajectory points using cubic interpolation. If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints. | |
int | parameterizeLinear () |
calculate the coefficients for interpolation between trajectory points using linear interpolation If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints. | |
void | sampleBlendedLinear (TPoint &tp, double time, const TCoeff &tc, double segment_start_time) |
Sample the trajectory based on a cubic interpolation. | |
void | sampleCubic (TPoint &tp, double time, const TCoeff &tc, double segment_start_time) |
Sample the trajectory based on a cubic interpolation. | |
void | sampleLinear (TPoint &tp, double time, const TCoeff &tc, double segment_start_time) |
Sample the trajectory based on a linear interpolation. | |
Private Attributes | |
int | dimension_ |
std::vector< bool > | joint_wraps_ |
std::vector< double > | max_acc_ |
bool | max_acc_set_ |
std::vector< double > | max_limit_ |
std::vector< double > | max_rate_ |
bool | max_rate_set_ |
std::vector< double > | min_limit_ |
int | num_points_ |
std::vector< TCoeff > | tc_ |
std::vector< TPoint > | tp_ |
Definition at line 56 of file trajectory.h.
Trajectory::Trajectory | ( | int | dimension | ) |
Constructor for instantiation of the class by specifying the dimension.
Definition at line 44 of file trajectory.cpp.
virtual trajectory::Trajectory::~Trajectory | ( | ) | [inline, virtual] |
Destructor.
Definition at line 125 of file trajectory.h.
void Trajectory::addPoint | ( | const TPoint | tp | ) |
Add a point to the trajectory.
Definition at line 250 of file trajectory.cpp.
double Trajectory::blendTime | ( | double | aa, | |
double | bb, | |||
double | cc | |||
) | [private] |
Definition at line 555 of file trajectory.cpp.
Definition at line 711 of file trajectory.cpp.
double Trajectory::calculateMinimumTimeLinear | ( | const TPoint & | start, | |
const TPoint & | end | |||
) | [private] |
calculate minimum time for a trajectory segment using a linear interpolation
Definition at line 689 of file trajectory.cpp.
calculate minimum time for a trajectory segment using LSPB.
Definition at line 772 of file trajectory.cpp.
double Trajectory::calculateMinTimeCubic | ( | double | q0, | |
double | q1, | |||
double | v0, | |||
double | v1, | |||
double | vmax, | |||
int | index | |||
) | [private] |
Definition at line 732 of file trajectory.cpp.
double Trajectory::calculateMinTimeLSPB | ( | double | q0, | |
double | q1, | |||
double | vmax, | |||
double | amax, | |||
int | index | |||
) | [private] |
Definition at line 792 of file trajectory.cpp.
void Trajectory::clear | ( | ) |
clear the trajectory
Definition at line 77 of file trajectory.cpp.
int Trajectory::findTrajectorySegment | ( | double | time | ) |
finds the trajectory segment corresponding to a particular time
input | time (in seconds) |
Definition at line 261 of file trajectory.cpp.
int Trajectory::getDuration | ( | int | index, | |
double & | duration | |||
) |
Definition at line 662 of file trajectory.cpp.
int Trajectory::getDuration | ( | std::vector< double > & | duration | ) |
Definition at line 649 of file trajectory.cpp.
int Trajectory::getNumberPoints | ( | ) |
Get the number of points in the trajectory.
if true, the max rates are used to compute trajectory timings, if false trajectory timings must be input by the user.
Definition at line 644 of file trajectory.cpp.
int Trajectory::getTimeStamps | ( | std::vector< double > & | timestamps | ) |
Definition at line 676 of file trajectory.cpp.
double Trajectory::getTotalTime | ( | ) |
Get the total time for the trajectory.
Definition at line 272 of file trajectory.cpp.
void Trajectory::getTrajectory | ( | std::vector< trajectory::Trajectory::TPoint > & | traj, | |
double | dT | |||
) |
Definition at line 1073 of file trajectory.cpp.
void Trajectory::init | ( | int | num_points, | |
int | dimension | |||
) | [private] |
Definition at line 51 of file trajectory.cpp.
double Trajectory::jointDiff | ( | double | from, | |
double | to, | |||
int | index | |||
) | [private] |
Definition at line 173 of file trajectory.cpp.
const Trajectory::TPoint & Trajectory::lastPoint | ( | ) | [private] |
Definition at line 282 of file trajectory.cpp.
int Trajectory::minimizeSegmentTimes | ( | ) |
Minimize segment times in the trajectory Timings for the trajectory segments are automatically calculated using max rate and/or max accn information based on the current value of interp_method_;.
Definition at line 381 of file trajectory.cpp.
int Trajectory::minimizeSegmentTimesWithBlendedLinearInterpolation | ( | ) | [private] |
calculate a minimum time trajectory using blended linear interpolation Timings for the trajectory are automatically calculated using max rate information.
Definition at line 497 of file trajectory.cpp.
int Trajectory::minimizeSegmentTimesWithCubicInterpolation | ( | ) | [private] |
calculate a minimum time trajectory using cubic interpolation Timings for the trajectory are automatically calculated using max rate information.
Definition at line 444 of file trajectory.cpp.
int Trajectory::minimizeSegmentTimesWithLinearInterpolation | ( | ) | [private] |
calculate a minimum time trajectory using linear interpolation Timings for the trajectory are automatically calculated using max rate information.
Definition at line 396 of file trajectory.cpp.
int Trajectory::parameterize | ( | ) | [private] |
calculate the coefficients for interpolation between trajectory points If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate and/or max accn information Thus, the time duration for any trajectory segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints.
boolean specifying if the joint wraps
Definition at line 816 of file trajectory.cpp.
int Trajectory::parameterizeBlendedLinear | ( | ) | [private] |
calculate the coefficients for interpolation between trajectory points using blended linear interpolation. If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate and max acceleration information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints.
Definition at line 972 of file trajectory.cpp.
int Trajectory::parameterizeCubic | ( | ) | [private] |
calculate the coefficients for interpolation between trajectory points using cubic interpolation. If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints.
Definition at line 903 of file trajectory.cpp.
int Trajectory::parameterizeLinear | ( | ) | [private] |
calculate the coefficients for interpolation between trajectory points using linear interpolation If autocalc_timing_ is true, timings for the trajectories are automatically calculated using max rate information. Thus, the time duration for any segment is the maximum of two times: the time duration specified by the user and the time duration dictated by the constraints.
Definition at line 833 of file trajectory.cpp.
int Trajectory::sample | ( | TPoint & | tp, | |
double | time | |||
) |
Sample the trajectory at a certain point in time.
reference | to a pre-allocated struct of type TPoint | |
time | at which trajectory is to be sampled |
Definition at line 287 of file trajectory.cpp.
void Trajectory::sampleBlendedLinear | ( | TPoint & | tp, | |
double | time, | |||
const TCoeff & | tc, | |||
double | segment_start_time | |||
) | [private] |
Sample the trajectory based on a cubic interpolation.
reference | to pre-allocated output trajectory point | |
time | at which trajectory is being sample | |
polynomial | coefficients for this segment of the trajectory | |
segment | start time |
Definition at line 591 of file trajectory.cpp.
void Trajectory::sampleCubic | ( | TPoint & | tp, | |
double | time, | |||
const TCoeff & | tc, | |||
double | segment_start_time | |||
) | [private] |
Sample the trajectory based on a cubic interpolation.
reference | to pre-allocated output trajectory point | |
time | at which trajectory is being sample | |
polynomial | coefficients for this segment of the trajectory | |
segment | start time |
Definition at line 628 of file trajectory.cpp.
void Trajectory::sampleLinear | ( | TPoint & | tp, | |
double | time, | |||
const TCoeff & | tc, | |||
double | segment_start_time | |||
) | [private] |
Sample the trajectory based on a linear interpolation.
reference | to pre-allocated output trajectory point | |
time | at which trajectory is being sample | |
polynomial | coefficients for this segment of the trajectory | |
segment | start time |
Definition at line 571 of file trajectory.cpp.
void Trajectory::setInterpolationMethod | ( | std::string | interp_method | ) |
Set the interpolation method.
std::string | interpolation method |
Definition at line 810 of file trajectory.cpp.
void Trajectory::setJointWraps | ( | int | index | ) |
Definition at line 163 of file trajectory.cpp.
int Trajectory::setMaxAcc | ( | std::vector< double > | max_acc | ) |
Definition at line 365 of file trajectory.cpp.
int Trajectory::setMaxRates | ( | std::vector< double > | max_rate | ) |
set the max rates (velocities)
std::vector | of size dimension_ containing the max rates for the degrees of freedom in the trajectory |
Definition at line 349 of file trajectory.cpp.
int Trajectory::setTrajectory | ( | const std::vector< double > & | p, | |
const std::vector< double > & | pdot, | |||
const std::vector< double > & | time, | |||
int | numPoints | |||
) |
Definition at line 219 of file trajectory.cpp.
int Trajectory::setTrajectory | ( | const std::vector< double > & | p, | |
int | numPoints | |||
) |
Set the trajectory using a vector of values, timestamps are not specified and so autocalc_timing_ is set to true within this function. Max rates and max accelerations should be set before this function is called.
std::vector | of size dimension x number of points - specifies a list of waypoints to initialize the trajectory with | |
number | of points in the trajectory |
Definition at line 130 of file trajectory.cpp.
int Trajectory::setTrajectory | ( | const std::vector< double > & | p, | |
const std::vector< double > & | time, | |||
int | numPoints | |||
) |
Set the trajectory using a vector of values and timestamps.
std::vector | of size dimension x number of points - specifies a list of waypoints to initialize the trajectory with | |
std::vector | of time stamps | |
number | of points in the trajectory |
Definition at line 187 of file trajectory.cpp.
int Trajectory::setTrajectory | ( | const std::vector< TPoint > & | tp | ) |
Set the trajectory using a vector of trajectory points.
std::vector | of trajectory points |
Definition at line 87 of file trajectory.cpp.
int Trajectory::write | ( | std::string | filename, | |
double | dT | |||
) |
Definition at line 1046 of file trajectory.cpp.
Definition at line 197 of file trajectory.h.
int trajectory::Trajectory::dimension_ [private] |
number of points in the trajectory
Definition at line 247 of file trajectory.h.
std::string trajectory::Trajectory::interp_method_ [protected] |
Definition at line 233 of file trajectory.h.
std::vector<bool> trajectory::Trajectory::joint_wraps_ [private] |
vector of max accelerations on the n DOFs of the trajectory
Definition at line 261 of file trajectory.h.
std::vector<double> trajectory::Trajectory::max_acc_ [private] |
vector of max rates on the n DOFs of the trajectory
Definition at line 259 of file trajectory.h.
bool trajectory::Trajectory::max_acc_set_ [private] |
string representation of interpolation method
Definition at line 237 of file trajectory.h.
std::vector<double> trajectory::Trajectory::max_limit_ [private] |
vector of polynomial coefficients for use to define the trajectory segments
Definition at line 253 of file trajectory.h.
std::vector<double> trajectory::Trajectory::max_rate_ [private] |
vector of min limits on the n DOFs of the trajectory
Definition at line 257 of file trajectory.h.
bool trajectory::Trajectory::max_rate_set_ [private] |
Definition at line 239 of file trajectory.h.
std::vector<double> trajectory::Trajectory::min_limit_ [private] |
vector of max limits on the n DOFs of the trajectory
Definition at line 255 of file trajectory.h.
int trajectory::Trajectory::num_points_ [private] |
Definition at line 245 of file trajectory.h.
std::vector<TCoeff> trajectory::Trajectory::tc_ [private] |
vector of TPoints in the trajectory
Definition at line 251 of file trajectory.h.
std::vector<TPoint> trajectory::Trajectory::tp_ [private] |
dimension of the trajectory
Definition at line 249 of file trajectory.h.