KDL  1.4.0
Public Types | Public Member Functions | Static Public Member Functions | Private Attributes | List of all members
KDL::Path_Line Class Reference

A path representing a line from A to B. More...

#include <src/path_line.hpp>

Inheritance diagram for KDL::Path_Line:
Inheritance graph
[legend]
Collaboration diagram for KDL::Path_Line:
Collaboration graph
[legend]

Public Types

enum  IdentifierType {
  ID_LINE =1, ID_CIRCLE =2, ID_COMPOSITE =3, ID_ROUNDED_COMPOSITE =4,
  ID_POINT =5, ID_CYCLIC_CLOSED =6
}
 

Public Member Functions

 Path_Line (const Frame &F_base_start, const Frame &F_base_end, RotationalInterpolation *orient, double eqradius, bool _aggregate=true)
 Constructs a Line Path F_base_start and F_base_end give the begin and end frame wrt the base orient gives the method of rotation interpolation eqradius : equivalent radius : serves to compare rotations and translations. More...
 
 Path_Line (const Frame &F_base_start, const Twist &twist_in_base, RotationalInterpolation *orient, double eqradius, bool _aggregate=true)
 
double LengthToS (double length)
 LengthToS() converts a physical length along the trajectory to the parameter s used in Pos, Vel and Acc. More...
 
virtual double PathLength ()
 Returns the total path length of the trajectory (has dimension LENGTH) This is not always a physical length , ie when dealing with rotations that are dominant. More...
 
virtual Frame Pos (double s) const
 Returns the Frame at the current path length s. More...
 
virtual Twist Vel (double s, double sd) const
 Returns the velocity twist at path length s theta and with derivative of s == sd. More...
 
virtual Twist Acc (double s, double sd, double sdd) const
 Returns the acceleration twist at path length s and with derivative of s == sd, and 2nd derivative of s == sdd. More...
 
virtual void Write (std::ostream &os)
 Writes one of the derived objects to the stream. More...
 
virtual PathClone ()
 Virtual constructor, constructing by copying, Returns a deep copy of this Path Object. More...
 
virtual IdentifierType getIdentifier () const
 gets an identifier indicating the type of this Path object More...
 
virtual ~Path_Line ()
 

Static Public Member Functions

static PathRead (std::istream &is)
 Reads one of the derived objects from the stream and returns a pointer (factory method) More...
 

Private Attributes

RotationalInterpolationorient
 
Vector V_base_start
 
Vector V_base_end
 
Vector V_start_end
 
double eqradius
 
double pathlength
 
double scalelin
 
double scalerot
 
bool aggregate
 

Detailed Description

A path representing a line from A to B.

Member Enumeration Documentation

§ IdentifierType

enum KDL::Path::IdentifierType
inherited
Enumerator
ID_LINE 
ID_CIRCLE 
ID_COMPOSITE 
ID_ROUNDED_COMPOSITE 
ID_POINT 
ID_CYCLIC_CLOSED 

Constructor & Destructor Documentation

§ Path_Line() [1/2]

KDL::Path_Line::Path_Line ( const Frame F_base_start,
const Frame F_base_end,
RotationalInterpolation orient,
double  eqradius,
bool  _aggregate = true 
)

Constructs a Line Path F_base_start and F_base_end give the begin and end frame wrt the base orient gives the method of rotation interpolation eqradius : equivalent radius : serves to compare rotations and translations.

the "amount of motion"(pos,vel,acc) of the rotation is taken to be the amount motion of a point at distance eqradius from the rotation axis.

Eqradius is introduced because it is unavoidable that you have to compare rotations and translations : e.g. : You can have motions that only contain rotation, and motions that only contain translations. The motion planning goes as follows :

  • translation is planned with the given parameters
  • rotation is planned planned with the parameters calculated with eqradius.
  • The longest of the previous two remains unchanged, the shortest in duration is scaled to take as long as the longest. This guarantees that the geometric path in 6D space remains independent of the motion profile parameters.

RotationalInterpolation_SingleAxis() has the advantage that it is independent of the frame in which you express your path. Other implementations for RotationalInterpolations COULD be (not implemented) (yet) : 1) quaternion interpolation : but this is more difficult for the human to interprete 2) 3-axis interpolation : express the orientation of the frame in e.g. euler zyx angles alfa,beta, gamma and interpolate these parameters. But this is dependent of the frame you choose as a reference and their can occur representation singularities.

References KDL::RotationalInterpolation::Angle(), eqradius, KDL::Frame::M, KDL::Vector::Normalize(), orient, pathlength, scalelin, scalerot, KDL::RotationalInterpolation::SetStartEnd(), V_base_end, V_base_start, and V_start_end.

Referenced by Clone().

§ Path_Line() [2/2]

KDL::Path_Line::Path_Line ( const Frame F_base_start,
const Twist twist_in_base,
RotationalInterpolation orient,
double  eqradius,
bool  _aggregate = true 
)

§ ~Path_Line()

KDL::Path_Line::~Path_Line ( )
virtual

References aggregate, and orient.

Referenced by getIdentifier().

Member Function Documentation

§ Acc()

Twist KDL::Path_Line::Acc ( double  s,
double  sd,
double  sdd 
) const
virtual

Returns the acceleration twist at path length s and with derivative of s == sd, and 2nd derivative of s == sdd.

Implements KDL::Path.

References KDL::RotationalInterpolation::Acc(), orient, scalelin, scalerot, and V_start_end.

§ Clone()

Path * KDL::Path_Line::Clone ( )
virtual

Virtual constructor, constructing by copying, Returns a deep copy of this Path Object.

Implements KDL::Path.

References aggregate, KDL::RotationalInterpolation::Clone(), eqradius, orient, Path_Line(), pathlength, KDL::RotationalInterpolation::Pos(), scalerot, V_base_end, and V_base_start.

§ getIdentifier()

virtual IdentifierType KDL::Path_Line::getIdentifier ( ) const
inlinevirtual

gets an identifier indicating the type of this Path object

Implements KDL::Path.

References KDL::Path::ID_LINE, and ~Path_Line().

§ LengthToS()

double KDL::Path_Line::LengthToS ( double  length)
virtual

LengthToS() converts a physical length along the trajectory to the parameter s used in Pos, Vel and Acc.

This is used because in cases with large rotations the parameter s does NOT correspond to the lineair length along the trajectory. User should be sure that the lineair distance travelled by this path object is NOT zero, when using this method ! (e.g. the case of only rotational change) throws Error_MotionPlanning_Not_Applicable if used on composed path objects.

Implements KDL::Path.

References scalelin.

§ PathLength()

double KDL::Path_Line::PathLength ( )
virtual

Returns the total path length of the trajectory (has dimension LENGTH) This is not always a physical length , ie when dealing with rotations that are dominant.

Implements KDL::Path.

References pathlength.

§ Pos()

Frame KDL::Path_Line::Pos ( double  s) const
virtual

Returns the Frame at the current path length s.

Implements KDL::Path.

References orient, KDL::RotationalInterpolation::Pos(), scalelin, scalerot, V_base_start, and V_start_end.

§ Read()

Path * KDL::Path::Read ( std::istream &  is)
staticinherited

Reads one of the derived objects from the stream and returns a pointer (factory method)

References KDL::RotationalInterpolation::Read().

Referenced by KDL::Trajectory::Read().

§ Vel()

Twist KDL::Path_Line::Vel ( double  s,
double  sd 
) const
virtual

Returns the velocity twist at path length s theta and with derivative of s == sd.

Implements KDL::Path.

References orient, scalelin, scalerot, V_start_end, and KDL::RotationalInterpolation::Vel().

§ Write()

void KDL::Path_Line::Write ( std::ostream &  os)
virtual

Writes one of the derived objects to the stream.

Implements KDL::Path.

References eqradius, orient, pathlength, KDL::RotationalInterpolation::Pos(), scalerot, V_base_end, V_base_start, and KDL::RotationalInterpolation::Write().

Member Data Documentation

§ aggregate

bool KDL::Path_Line::aggregate
private

Referenced by Clone(), and ~Path_Line().

§ eqradius

double KDL::Path_Line::eqradius
private

Referenced by Clone(), Path_Line(), and Write().

§ orient

RotationalInterpolation* KDL::Path_Line::orient
private

§ pathlength

double KDL::Path_Line::pathlength
private

Referenced by Clone(), Path_Line(), PathLength(), and Write().

§ scalelin

double KDL::Path_Line::scalelin
private

Referenced by Acc(), LengthToS(), Path_Line(), Pos(), and Vel().

§ scalerot

double KDL::Path_Line::scalerot
private

Referenced by Acc(), Clone(), Path_Line(), Pos(), Vel(), and Write().

§ V_base_end

Vector KDL::Path_Line::V_base_end
private

Referenced by Clone(), Path_Line(), and Write().

§ V_base_start

Vector KDL::Path_Line::V_base_start
private

Referenced by Clone(), Path_Line(), Pos(), and Write().

§ V_start_end

Vector KDL::Path_Line::V_start_end
private

Referenced by Acc(), Path_Line(), Pos(), and Vel().


The documentation for this class was generated from the following files: