spline_smoother Namespace Reference

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::LSPBTrajectoryMsg &spline, const double &time, spline_smoother::LSPBTrajectorySegmentMsg &spline_segment, double &segment_time, int start_index=0, int end_index=-1)
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.
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::LSPBTrajectoryMsg &spline, double &t)
 Get the total time for the provided spline trajectory.
bool getTotalTime (const spline_smoother::SplineTrajectory &spline, double &t)
 Get the total time for the provided spline trajectory.
double jointDiff (const double &from, const double &to, const motion_planning_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::SplineTrajectorySegment_< ContainerAllocator > &v)
template<typename ContainerAllocator >
std::ostream & operator<< (std::ostream &s, const ::spline_smoother::SplineTrajectory_< ContainerAllocator > &v)
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::LSPBTrajectoryMsg_< ContainerAllocator > &v)
template<typename ContainerAllocator >
std::ostream & operator<< (std::ostream &s, const ::spline_smoother::LSPBSplineCoefficients_< 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::LSPBTrajectoryMsg &spline, const std::vector< double > &times, trajectory_msgs::JointTrajectory &traj_out)
bool sampleSplineTrajectory (const spline_smoother::SplineTrajectory &spline, const std::vector< double > &times, trajectory_msgs::JointTrajectory &traj_out)
bool sampleSplineTrajectory (const spline_smoother::LSPBTrajectorySegmentMsg &spline, const double &input_time, trajectory_msgs::JointTrajectoryPoint &point_out)
bool sampleSplineTrajectory (const spline_smoother::SplineTrajectorySegment &spline, const double &input_time, trajectory_msgs::JointTrajectoryPoint &point_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::LSPBTrajectoryMsg &spline, const std::vector< double > &times, const std::string &filename)
 Write the trajectory out to a file after sampling at the specified times.
bool 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.
bool writeSpline (const spline_smoother::LSPBTrajectoryMsg &spline, const std::string &filename)
 Write the spline representation of the trajectory out.
bool writeSpline (const spline_smoother::SplineTrajectory &spline, const std::string &filename)
 Write the spline representation of the trajectory out.

Variables

static const double MAX_ALLOWABLE_TIME = FLT_MAX

Detailed Description

Author:
Mrinal Kalakrishnan
Sachin Chitta

Typedef Documentation

Definition at line 103 of file LSPBSplineCoefficients.h.

Definition at line 106 of file LSPBSplineCoefficients.h.

Definition at line 105 of file LSPBSplineCoefficients.h.

Definition at line 138 of file LSPBTrajectoryMsg.h.

Definition at line 141 of file LSPBTrajectoryMsg.h.

Definition at line 140 of file LSPBTrajectoryMsg.h.

Definition at line 101 of file LSPBTrajectorySegmentMsg.h.

Definition at line 104 of file LSPBTrajectorySegmentMsg.h.

Definition at line 103 of file LSPBTrajectorySegmentMsg.h.

Definition at line 86 of file SplineCoefficients.h.

Definition at line 89 of file SplineCoefficients.h.

Definition at line 88 of file SplineCoefficients.h.

Definition at line 137 of file SplineTrajectory.h.

Definition at line 140 of file SplineTrajectory.h.

Definition at line 139 of file SplineTrajectory.h.

Definition at line 100 of file SplineTrajectorySegment.h.

Definition at line 103 of file SplineTrajectorySegment.h.

Definition at line 102 of file SplineTrajectorySegment.h.


Function Documentation

template<typename T >
bool spline_smoother::checkTrajectoryConsistency ( T &  waypoint_traj  )  [inline]

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.

template<typename T >
void spline_smoother::differentiate ( const std::vector< T > &  x,
std::vector< T > &  xd 
) [inline]

Definition at line 77 of file spline_smoother_utils.h.

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 97 of file spline_smoother_utils.cpp.

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.

Returns:
true on success, false if any failure occurs
Parameters:
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.

static void spline_smoother::generatePowers ( int  n,
double  x,
double *  powers 
) [inline, static]

Definition at line 88 of file splines.h.

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

Definition at line 146 of file splines.h.

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

Definition at line 97 of file splines.h.

bool spline_smoother::getTotalTime ( const spline_smoother::LSPBTrajectoryMsg spline,
double &  t 
)

Get the total time for the provided spline trajectory.

Returns:
true on success, false if any failure occurs
Parameters:
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 158 of file spline_smoother_utils.cpp.

bool spline_smoother::getTotalTime ( const spline_smoother::SplineTrajectory spline,
double &  t 
)

Get the total time for the provided spline trajectory.

Returns:
true on success, false if any failure occurs
Parameters:
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 140 of file spline_smoother_utils.cpp.

double spline_smoother::jointDiff ( const double &  from,
const double &  to,
const motion_planning_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.

Returns:
if(limit.angle_wraparound) return angles::shortest_angular_distance(from,to) else return (to-from)
Parameters:
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.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::SplineTrajectorySegment_< ContainerAllocator > &  v 
) [inline]

Definition at line 107 of file SplineTrajectorySegment.h.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::SplineTrajectory_< ContainerAllocator > &  v 
) [inline]

Definition at line 144 of file SplineTrajectory.h.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::SplineCoefficients_< ContainerAllocator > &  v 
) [inline]

Definition at line 93 of file SplineCoefficients.h.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::LSPBTrajectorySegmentMsg_< ContainerAllocator > &  v 
) [inline]

Definition at line 108 of file LSPBTrajectorySegmentMsg.h.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::LSPBTrajectoryMsg_< ContainerAllocator > &  v 
) [inline]

Definition at line 145 of file LSPBTrajectoryMsg.h.

template<typename ContainerAllocator >
std::ostream& spline_smoother::operator<< ( std::ostream &  s,
const ::spline_smoother::LSPBSplineCoefficients_< ContainerAllocator > &  v 
) [inline]

Definition at line 110 of file LSPBSplineCoefficients.h.

void spline_smoother::sampleCubicSpline ( const std::vector< double > &  coefficients,
double  time,
double &  position,
double &  velocity,
double &  acceleration 
) [inline]

Samples a cubic spline segment at a particular time.

Definition at line 160 of file splines.h.

void spline_smoother::sampleQuinticSpline ( const std::vector< double > &  coefficients,
double  time,
double &  position,
double &  velocity,
double &  acceleration 
) [inline]

Samples a quintic spline segment at a particular time.

Definition at line 120 of file splines.h.

bool spline_smoother::sampleSplineTrajectory ( const spline_smoother::LSPBTrajectoryMsg spline,
const std::vector< double > &  times,
trajectory_msgs::JointTrajectory &  traj_out 
)

Definition at line 276 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 251 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 202 of file spline_smoother_utils.cpp.

bool spline_smoother::sampleSplineTrajectory ( const spline_smoother::SplineTrajectorySegment spline,
const double &  input_time,
trajectory_msgs::JointTrajectoryPoint &  point_out 
)

Definition at line 170 of file spline_smoother_utils.cpp.

template<typename T >
void spline_smoother::tridiagonalSolve ( std::vector< T > &  a,
std::vector< T > &  b,
std::vector< T > &  c,
std::vector< T > &  d,
std::vector< T > &  x 
) [inline]

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::LSPBTrajectoryMsg spline,
const std::vector< double > &  times,
const std::string &  filename 
)

Write the trajectory out to a file after sampling at the specified times.

Returns:
true on success, false if any failure occurs
Parameters:
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 390 of file spline_smoother_utils.cpp.

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.

Returns:
true on success, false if any failure occurs
Parameters:
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 308 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.

Returns:
true on success, false if any failure occurs
Parameters:
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 439 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.

Returns:
true on success, false if any failure occurs
Parameters:
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 357 of file spline_smoother_utils.cpp.


Variable Documentation

const double spline_smoother::MAX_ALLOWABLE_TIME = FLT_MAX [static]

Definition at line 285 of file spline_smoother_utils.h.

 All Classes Namespaces Files Functions Variables Typedefs


spline_smoother
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Fri Jan 11 09:15:18 2013