KDL
1.4.0
|
A path representing a line from A to B. More...
#include <src/path_line.hpp>
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 Path * | Clone () |
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 Path * | Read (std::istream &is) |
Reads one of the derived objects from the stream and returns a pointer (factory method) More... | |
Private Attributes | |
RotationalInterpolation * | orient |
Vector | V_base_start |
Vector | V_base_end |
Vector | V_start_end |
double | eqradius |
double | pathlength |
double | scalelin |
double | scalerot |
bool | aggregate |
A path representing a line from A to B.
|
inherited |
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 :
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().
KDL::Path_Line::Path_Line | ( | const Frame & | F_base_start, |
const Twist & | twist_in_base, | ||
RotationalInterpolation * | orient, | ||
double | eqradius, | ||
bool | _aggregate = true |
||
) |
References KDL::RotationalInterpolation::Angle(), eqradius, KDL::Frame::M, KDL::Vector::Norm(), KDL::Vector::Normalize(), orient, pathlength, KDL::Rotation::Rot(), KDL::Twist::rot, scalelin, scalerot, KDL::RotationalInterpolation::SetStartEnd(), V_base_end, V_base_start, V_start_end, and KDL::Twist::vel.
|
virtual |
References aggregate, and orient.
Referenced by getIdentifier().
|
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.
|
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.
|
inlinevirtual |
gets an identifier indicating the type of this Path object
Implements KDL::Path.
References KDL::Path::ID_LINE, and ~Path_Line().
|
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.
|
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.
|
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.
|
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().
|
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().
|
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().
|
private |
Referenced by Clone(), and ~Path_Line().
|
private |
Referenced by Clone(), Path_Line(), and Write().
|
private |
Referenced by Acc(), Clone(), Path_Line(), Pos(), Vel(), Write(), and ~Path_Line().
|
private |
Referenced by Clone(), Path_Line(), PathLength(), and Write().
|
private |
Referenced by Acc(), LengthToS(), Path_Line(), Pos(), and Vel().
|
private |
|
private |
Referenced by Clone(), Path_Line(), and Write().
|
private |
Referenced by Clone(), Path_Line(), Pos(), and Write().
|
private |
Referenced by Acc(), Path_Line(), Pos(), and Vel().