Namespaces | |
namespace | msg |
Classes | |
class | ClampedCubicSplineSmoother |
Generates a clamped cubic spline trajectory from the input waypoints. More... | |
class | CubicParameterizedSplineVelocityScaler |
Scales the time intervals stretching them if necessary so that the trajectory conforms to velocity limits. More... | |
class | CubicParameterizedTrajectory |
class | CubicSplineVelocityScaler |
Scales the time intervals stretching them if necessary so that the trajectory conforms to velocity limits. More... | |
class | CubicTrajectory |
class | FritschButlandSplineSmoother |
Implements the Fritsch-Butland algorithm for monotonic cubic spline interpolation. More... | |
class | LinearSplineVelocityScaler |
Scales the time intervals stretching them if necessary so that the trajectory conforms to velocity limits. More... | |
class | LinearTrajectory |
Generates velocities at waypoints by finite differencing. Accelerations are set to zero. More... | |
struct | LSPBSplineCoefficients_ |
class | LSPBTrajectory |
Generates velocities at waypoints by finite differencing. Accelerations are set to zero. More... | |
struct | LSPBTrajectoryMsg_ |
struct | LSPBTrajectorySegmentMsg_ |
class | NumericalDifferentiationSplineSmoother |
Generates velocities at waypoints by finite differencing. Accelerations are set to zero. More... | |
struct | SplineCoefficients_ |
class | SplineSmoother |
Abstract base class for spline smoothing. More... | |
struct | SplineTrajectory_ |
struct | SplineTrajectorySegment_ |
Typedefs | |
typedef ::spline_smoother::LSPBSplineCoefficients_ < std::allocator< void > > | LSPBSplineCoefficients |
typedef boost::shared_ptr < ::spline_smoother::LSPBSplineCoefficients const > | LSPBSplineCoefficientsConstPtr |
typedef boost::shared_ptr < ::spline_smoother::LSPBSplineCoefficients > | LSPBSplineCoefficientsPtr |
typedef ::spline_smoother::LSPBTrajectoryMsg_ < std::allocator< void > > | LSPBTrajectoryMsg |
typedef boost::shared_ptr < ::spline_smoother::LSPBTrajectoryMsg const > | LSPBTrajectoryMsgConstPtr |
typedef boost::shared_ptr < ::spline_smoother::LSPBTrajectoryMsg > | LSPBTrajectoryMsgPtr |
typedef ::spline_smoother::LSPBTrajectorySegmentMsg_ < std::allocator< void > > | LSPBTrajectorySegmentMsg |
typedef boost::shared_ptr < ::spline_smoother::LSPBTrajectorySegmentMsg const > | LSPBTrajectorySegmentMsgConstPtr |
typedef boost::shared_ptr < ::spline_smoother::LSPBTrajectorySegmentMsg > | LSPBTrajectorySegmentMsgPtr |
typedef ::spline_smoother::SplineCoefficients_ < std::allocator< void > > | SplineCoefficients |
typedef boost::shared_ptr < ::spline_smoother::SplineCoefficients const > | SplineCoefficientsConstPtr |
typedef boost::shared_ptr < ::spline_smoother::SplineCoefficients > | SplineCoefficientsPtr |
typedef ::spline_smoother::SplineTrajectory_ < std::allocator< void > > | SplineTrajectory |
typedef boost::shared_ptr < ::spline_smoother::SplineTrajectory const > | SplineTrajectoryConstPtr |
typedef boost::shared_ptr < ::spline_smoother::SplineTrajectory > | SplineTrajectoryPtr |
typedef ::spline_smoother::SplineTrajectorySegment_ < std::allocator< void > > | SplineTrajectorySegment |
typedef boost::shared_ptr < ::spline_smoother::SplineTrajectorySegment const > | SplineTrajectorySegmentConstPtr |
typedef boost::shared_ptr < ::spline_smoother::SplineTrajectorySegment > | SplineTrajectorySegmentPtr |
Functions | |
template<typename T > | |
bool | checkTrajectoryConsistency (T &waypoint_traj) |
Ensures the consistency of a WaypointTrajWithLimits message, and resizes vel and acc arrays. | |
template<typename T > | |
void | differentiate (const std::vector< T > &x, std::vector< T > &xd) |
bool | findSplineSegment (const spline_smoother::SplineTrajectory &spline, const double &time, spline_smoother::SplineTrajectorySegment &spline_segment, double &segment_time, int start_index=0, int end_index=-1) |
Internal function that helps determine which spline segment corresponds to an input time. | |
bool | findSplineSegment (const spline_smoother::LSPBTrajectoryMsg &spline, const double &time, spline_smoother::LSPBTrajectorySegmentMsg &spline_segment, double &segment_time, int start_index=0, int end_index=-1) |
static void | generatePowers (int n, double x, double *powers) |
void | getCubicSplineCoefficients (double start_pos, double start_vel, double end_pos, double end_vel, double time, std::vector< double > &coefficients) |
Calculates cubic spline coefficients given the start and end way-points. | |
void | getQuinticSplineCoefficients (double start_pos, double start_vel, double start_acc, double end_pos, double end_vel, double end_acc, double time, std::vector< double > &coefficients) |
Calculates quintic spline coefficients given the start and end way-points. | |
bool | getTotalTime (const spline_smoother::SplineTrajectory &spline, double &t) |
Get the total time for the provided spline trajectory. | |
bool | getTotalTime (const spline_smoother::LSPBTrajectoryMsg &spline, double &t) |
Get the total time for the provided spline trajectory. | |
double | jointDiff (const double &from, const double &to, const arm_navigation_msgs::JointLimits &limit) |
An internal helper function that uses information about the joint to compute proper angular differences if the joint is a rotary joint with limits. | |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::SplineCoefficients_< ContainerAllocator > &v) |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::LSPBTrajectorySegmentMsg_< ContainerAllocator > &v) |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::SplineTrajectorySegment_< ContainerAllocator > &v) |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::LSPBSplineCoefficients_< ContainerAllocator > &v) |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::LSPBTrajectoryMsg_< ContainerAllocator > &v) |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::spline_smoother::SplineTrajectory_< ContainerAllocator > &v) |
void | sampleCubicSpline (const std::vector< double > &coefficients, double time, double &position, double &velocity, double &acceleration) |
Samples a cubic spline segment at a particular time. | |
void | sampleQuinticSpline (const std::vector< double > &coefficients, double time, double &position, double &velocity, double &acceleration) |
Samples a quintic spline segment at a particular time. | |
bool | sampleSplineTrajectory (const spline_smoother::SplineTrajectorySegment &spline, const double &input_time, trajectory_msgs::JointTrajectoryPoint &point_out) |
bool | sampleSplineTrajectory (const spline_smoother::LSPBTrajectorySegmentMsg &spline, const double &input_time, trajectory_msgs::JointTrajectoryPoint &point_out) |
bool | sampleSplineTrajectory (const spline_smoother::SplineTrajectory &spline, const std::vector< double > ×, trajectory_msgs::JointTrajectory &traj_out) |
bool | sampleSplineTrajectory (const spline_smoother::LSPBTrajectoryMsg &spline, const std::vector< double > ×, trajectory_msgs::JointTrajectory &traj_out) |
template<typename T > | |
void | tridiagonalSolve (std::vector< T > &a, std::vector< T > &b, std::vector< T > &c, std::vector< T > &d, std::vector< T > &x) |
Solves the tridiagonal system of equations, Ax = d A is an n by n square matrix which consists of: diagonal b (0 ... n-1) upper diagonal c (0 ... n-2) lower diagonal a (0 ... n-1) | |
bool | write (const spline_smoother::SplineTrajectory &spline, const std::vector< double > ×, const std::string &filename) |
Write the trajectory out to a file after sampling at the specified times. | |
bool | write (const spline_smoother::LSPBTrajectoryMsg &spline, const std::vector< double > ×, const std::string &filename) |
Write the trajectory out to a file after sampling at the specified times. | |
bool | writeSpline (const spline_smoother::SplineTrajectory &spline, const std::string &filename) |
Write the spline representation of the trajectory out. | |
bool | writeSpline (const spline_smoother::LSPBTrajectoryMsg &spline, const std::string &filename) |
Write the spline representation of the trajectory out. | |
Variables | |
static const double | MAX_ALLOWABLE_TIME = FLT_MAX |
typedef ::spline_smoother::LSPBSplineCoefficients_<std::allocator<void> > spline_smoother::LSPBSplineCoefficients |
Definition at line 52 of file LSPBSplineCoefficients.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBSplineCoefficients const> spline_smoother::LSPBSplineCoefficientsConstPtr |
Definition at line 55 of file LSPBSplineCoefficients.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBSplineCoefficients> spline_smoother::LSPBSplineCoefficientsPtr |
Definition at line 54 of file LSPBSplineCoefficients.h.
typedef ::spline_smoother::LSPBTrajectoryMsg_<std::allocator<void> > spline_smoother::LSPBTrajectoryMsg |
Definition at line 54 of file LSPBTrajectoryMsg.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBTrajectoryMsg const> spline_smoother::LSPBTrajectoryMsgConstPtr |
Definition at line 57 of file LSPBTrajectoryMsg.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBTrajectoryMsg> spline_smoother::LSPBTrajectoryMsgPtr |
Definition at line 56 of file LSPBTrajectoryMsg.h.
typedef ::spline_smoother::LSPBTrajectorySegmentMsg_<std::allocator<void> > spline_smoother::LSPBTrajectorySegmentMsg |
Definition at line 48 of file LSPBTrajectorySegmentMsg.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBTrajectorySegmentMsg const> spline_smoother::LSPBTrajectorySegmentMsgConstPtr |
Definition at line 51 of file LSPBTrajectorySegmentMsg.h.
typedef boost::shared_ptr< ::spline_smoother::LSPBTrajectorySegmentMsg> spline_smoother::LSPBTrajectorySegmentMsgPtr |
Definition at line 50 of file LSPBTrajectorySegmentMsg.h.
typedef ::spline_smoother::SplineCoefficients_<std::allocator<void> > spline_smoother::SplineCoefficients |
Definition at line 42 of file SplineCoefficients.h.
typedef boost::shared_ptr< ::spline_smoother::SplineCoefficients const> spline_smoother::SplineCoefficientsConstPtr |
Definition at line 45 of file SplineCoefficients.h.
typedef boost::shared_ptr< ::spline_smoother::SplineCoefficients> spline_smoother::SplineCoefficientsPtr |
Definition at line 44 of file SplineCoefficients.h.
typedef ::spline_smoother::SplineTrajectory_<std::allocator<void> > spline_smoother::SplineTrajectory |
Definition at line 54 of file SplineTrajectory.h.
typedef boost::shared_ptr< ::spline_smoother::SplineTrajectory const> spline_smoother::SplineTrajectoryConstPtr |
Definition at line 57 of file SplineTrajectory.h.
typedef boost::shared_ptr< ::spline_smoother::SplineTrajectory> spline_smoother::SplineTrajectoryPtr |
Definition at line 56 of file SplineTrajectory.h.
typedef ::spline_smoother::SplineTrajectorySegment_<std::allocator<void> > spline_smoother::SplineTrajectorySegment |
Definition at line 48 of file SplineTrajectorySegment.h.
typedef boost::shared_ptr< ::spline_smoother::SplineTrajectorySegment const> spline_smoother::SplineTrajectorySegmentConstPtr |
Definition at line 51 of file SplineTrajectorySegment.h.
typedef boost::shared_ptr< ::spline_smoother::SplineTrajectorySegment> spline_smoother::SplineTrajectorySegmentPtr |
Definition at line 50 of file SplineTrajectorySegment.h.
bool spline_smoother::checkTrajectoryConsistency | ( | T & | waypoint_traj | ) |
Ensures the consistency of a WaypointTrajWithLimits message, and resizes vel and acc arrays.
Ensures that the number of (joint) names matches the number of positions in each waypoint Resizes the velocities and accelerations for every waypoint, filling in zeros if necessary Ensures that time is strictly increasing
Definition at line 122 of file spline_smoother_utils.h.
void spline_smoother::differentiate | ( | const std::vector< T > & | x, |
std::vector< T > & | xd | ||
) |
Definition at line 77 of file spline_smoother_utils.h.
bool spline_smoother::findSplineSegment | ( | const spline_smoother::SplineTrajectory & | spline, |
const double & | time, | ||
spline_smoother::SplineTrajectorySegment & | spline_segment, | ||
double & | segment_time, | ||
int | start_index = 0 , |
||
int | end_index = -1 |
||
) |
Internal function that helps determine which spline segment corresponds to an input time.
spline | The input spline representation of the trajectory |
time | Time at which trajectory needs to be sampled. time = 0.0 corresponds to start of the trajectory. |
spline_segment | Reference to output spline segment containing the waypoint corresponding to the desired time |
segment_time | Reference to output segment time corresponding to the waypoint in trajectory at the (absolute) input time. segment_time = 0.0 corresponds to the start of this spline segment. |
start_index | (optional) parameter to specify the starting index to start the (linear) search for required input time |
end_index | (optional) parameter to specify the end index for the (linear) search for required input time |
Definition at line 55 of file spline_smoother_utils.cpp.
bool spline_smoother::findSplineSegment | ( | const spline_smoother::LSPBTrajectoryMsg & | spline, |
const double & | time, | ||
spline_smoother::LSPBTrajectorySegmentMsg & | spline_segment, | ||
double & | segment_time, | ||
int | start_index = 0 , |
||
int | end_index = -1 |
||
) |
Definition at line 91 of file spline_smoother_utils.cpp.
static void spline_smoother::generatePowers | ( | int | n, |
double | x, | ||
double * | powers | ||
) | [inline, static] |
void spline_smoother::getCubicSplineCoefficients | ( | double | start_pos, |
double | start_vel, | ||
double | end_pos, | ||
double | end_vel, | ||
double | time, | ||
std::vector< double > & | coefficients | ||
) | [inline] |
Calculates cubic spline coefficients given the start and end way-points.
The input to this function is the start and end way-point, with position, velocity and the duration of the spline segment. (assumes that the spline runs from 0 to time)
Returns 4 coefficients of the quintic polynomial in the "coefficients" vector. The spline can then be sampled as: x = coefficients[0]*t^0 + coefficients[1]*t^1 ... coefficients[3]*t^3
void spline_smoother::getQuinticSplineCoefficients | ( | double | start_pos, |
double | start_vel, | ||
double | start_acc, | ||
double | end_pos, | ||
double | end_vel, | ||
double | end_acc, | ||
double | time, | ||
std::vector< double > & | coefficients | ||
) | [inline] |
Calculates quintic spline coefficients given the start and end way-points.
The input to this function is the start and end way-point, with position, velocity and acceleration, and the duration of the spline segment. (assumes that the spline runs from 0 to time)
Returns 6 coefficients of the quintic polynomial in the "coefficients" vector. The spline can then be sampled as: x = coefficients[0]*t^0 + coefficients[1]*t^1 ... coefficients[5]*t^5
bool spline_smoother::getTotalTime | ( | const spline_smoother::SplineTrajectory & | spline, |
double & | t | ||
) |
Get the total time for the provided spline trajectory.
spline | The input spline representation of the trajectory. |
t | A reference that will be filled in with the total time for the input spline trajectory. |
Definition at line 134 of file spline_smoother_utils.cpp.
bool spline_smoother::getTotalTime | ( | const spline_smoother::LSPBTrajectoryMsg & | spline, |
double & | t | ||
) |
Get the total time for the provided spline trajectory.
spline | The input spline representation of the trajectory. |
t | A reference that will be filled in with the total time for the input spline trajectory. |
Definition at line 152 of file spline_smoother_utils.cpp.
double spline_smoother::jointDiff | ( | const double & | from, |
const double & | to, | ||
const arm_navigation_msgs::JointLimits & | limit | ||
) | [inline] |
An internal helper function that uses information about the joint to compute proper angular differences if the joint is a rotary joint with limits.
from | The start position of the joint |
to | The end position of the joint |
limit | A set of position, velocity and acceleration limits for this particular joint |
Definition at line 169 of file spline_smoother_utils.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::SplineCoefficients_< ContainerAllocator > & | v | ||
) |
Definition at line 49 of file SplineCoefficients.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::LSPBTrajectorySegmentMsg_< ContainerAllocator > & | v | ||
) |
Definition at line 55 of file LSPBTrajectorySegmentMsg.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::SplineTrajectorySegment_< ContainerAllocator > & | v | ||
) |
Definition at line 55 of file SplineTrajectorySegment.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::LSPBSplineCoefficients_< ContainerAllocator > & | v | ||
) |
Definition at line 59 of file LSPBSplineCoefficients.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::LSPBTrajectoryMsg_< ContainerAllocator > & | v | ||
) |
Definition at line 61 of file LSPBTrajectoryMsg.h.
std::ostream& spline_smoother::operator<< | ( | std::ostream & | s, |
const ::spline_smoother::SplineTrajectory_< ContainerAllocator > & | v | ||
) |
Definition at line 61 of file SplineTrajectory.h.
void spline_smoother::sampleCubicSpline | ( | const std::vector< double > & | coefficients, |
double | time, | ||
double & | position, | ||
double & | velocity, | ||
double & | acceleration | ||
) | [inline] |
void spline_smoother::sampleQuinticSpline | ( | const std::vector< double > & | coefficients, |
double | time, | ||
double & | position, | ||
double & | velocity, | ||
double & | acceleration | ||
) | [inline] |
bool spline_smoother::sampleSplineTrajectory | ( | const spline_smoother::SplineTrajectorySegment & | spline, |
const double & | input_time, | ||
trajectory_msgs::JointTrajectoryPoint & | point_out | ||
) |
Definition at line 164 of file spline_smoother_utils.cpp.
bool spline_smoother::sampleSplineTrajectory | ( | const spline_smoother::LSPBTrajectorySegmentMsg & | spline, |
const double & | input_time, | ||
trajectory_msgs::JointTrajectoryPoint & | point_out | ||
) |
Definition at line 196 of file spline_smoother_utils.cpp.
bool spline_smoother::sampleSplineTrajectory | ( | const spline_smoother::SplineTrajectory & | spline, |
const std::vector< double > & | times, | ||
trajectory_msgs::JointTrajectory & | traj_out | ||
) |
Definition at line 245 of file spline_smoother_utils.cpp.
bool spline_smoother::sampleSplineTrajectory | ( | const spline_smoother::LSPBTrajectoryMsg & | spline, |
const std::vector< double > & | times, | ||
trajectory_msgs::JointTrajectory & | traj_out | ||
) |
Definition at line 267 of file spline_smoother_utils.cpp.
void spline_smoother::tridiagonalSolve | ( | std::vector< T > & | a, |
std::vector< T > & | b, | ||
std::vector< T > & | c, | ||
std::vector< T > & | d, | ||
std::vector< T > & | x | ||
) |
Solves the tridiagonal system of equations, Ax = d A is an n by n square matrix which consists of: diagonal b (0 ... n-1) upper diagonal c (0 ... n-2) lower diagonal a (0 ... n-1)
The solution goes into x. Time complexity: O(n)
WARNING: modifies input arrays!!
Definition at line 88 of file spline_smoother_utils.h.
bool spline_smoother::write | ( | const spline_smoother::SplineTrajectory & | spline, |
const std::vector< double > & | times, | ||
const std::string & | filename | ||
) |
Write the trajectory out to a file after sampling at the specified times.
spline | The input spline representation of the trajectory. |
times | The set of times (in seconds) where the trajectory needs to be sampled, time = 0.0 corresponds to the start of the trajectory. |
filename | The string representation of the name of the file where the trajectory should be written |
Definition at line 296 of file spline_smoother_utils.cpp.
bool spline_smoother::write | ( | const spline_smoother::LSPBTrajectoryMsg & | spline, |
const std::vector< double > & | times, | ||
const std::string & | filename | ||
) |
Write the trajectory out to a file after sampling at the specified times.
spline | The input spline representation of the trajectory. |
times | The set of times (in seconds) where the trajectory needs to be sampled, time = 0.0 corresponds to the start of the trajectory. |
filename | The string representation of the name of the file where the trajectory should be written |
Definition at line 378 of file spline_smoother_utils.cpp.
bool spline_smoother::writeSpline | ( | const spline_smoother::SplineTrajectory & | spline, |
const std::string & | filename | ||
) |
Write the spline representation of the trajectory out.
spline | The input spline representation of the trajectory. |
filename | The string representation of the name of the file where the spline representation of the trajectory should be written |
Definition at line 345 of file spline_smoother_utils.cpp.
bool spline_smoother::writeSpline | ( | const spline_smoother::LSPBTrajectoryMsg & | spline, |
const std::string & | filename | ||
) |
Write the spline representation of the trajectory out.
spline | The input spline representation of the trajectory. |
filename | The string representation of the name of the file where the spline representation of the trajectory should be written |
Definition at line 427 of file spline_smoother_utils.cpp.
const double spline_smoother::MAX_ALLOWABLE_TIME = FLT_MAX [static] |
Definition at line 285 of file spline_smoother_utils.h.