/*************************************************************************** tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_roundedcomposite.h path_roundedcomposite.h - description ------------------- begin : Mon January 10 2005 copyright : (C) 2005 Erwin Aertbelien email : erwin.aertbelien@mech.kuleuven.ac.be *************************************************************************** * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the Free Software * * Foundation, Inc., 59 Temple Place, * * Suite 330, Boston, MA 02111-1307 USA * * * ***************************************************************************/ /***************************************************************************** * \author * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * * \version * ORO_Geometry V0.2 * * \par History * - $log$ * * \par Release * $Id: path_roundedcomposite.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ * $Name: $ ****************************************************************************/ #ifndef KDL_MOTION_ROUNDEDCOMPOSITE_H #define KDL_MOTION_ROUNDEDCOMPOSITE_H #include "path.hpp" #include "path_composite.hpp" #include "rotational_interpolation.hpp" namespace KDL { /** * The specification of a path, composed of way-points with rounded corners. * * @ingroup Motion */ class Path_RoundedComposite : public Path { /** a Path_Composite is aggregated to hold the rounded trajectory * with circles and lines */ Path_Composite* comp; double radius; double eqradius; RotationalInterpolation* orient; // cached from underlying path objects for generating the rounding : Frame F_base_start; Frame F_base_via; //Frame F_base_end; int nrofpoints; bool aggregate; Path_RoundedComposite(Path_Composite* comp,double radius,double eqradius,RotationalInterpolation* orient, bool aggregate, int nrofpoints); public: /** * @param radius : radius of the rounding circles * @param eqradius : equivalent radius to compare rotations/velocities * @param orient : method of rotational_interpolation interpolation * @param aggregate : if true, this object will own the _orient pointer, i.e. it will delete the _orient pointer * when the destructor of this object is called. */ Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true); /** * Adds a point to this rounded composite, between two adjacent points * a Path_Line will be created, between two lines there will be * rounding with the given radius with a Path_Circle * * The Error_MotionPlanning_Not_Feasible has a type (obtained by GetType) of: * - 3101 if the eq. radius <= 0 * - 3102 if the first segment in a rounding has zero length. * - 3103 if the second segment in a rounding has zero length. * - 3104 if the angle between the first and the second segment is close to PI. * (meaning that the segments are on top of each other) * - 3105 if the distance needed for the rounding is larger then the first segment. * - 3106 if the distance needed for the rounding is larger then the second segment. * * @param F_base_point the pose of a new via point. * @warning Can throw Error_MotionPlanning_Not_Feasible object * @TODO handle the case of error type 3105 and 3106 by skipping segments, such that the class could be applied * with points that are very close to each other. */ void Add(const Frame& F_base_point); /** * to be called after the last line is added to finish up * the work */ void Finish(); virtual double LengthToS(double length); /** * 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. */ virtual double PathLength(); /** * Returns the Frame at the current path length s */ virtual Frame Pos(double s) const; /** * Returns the velocity twist at path length s theta and with * derivative of s == sd */ virtual Twist Vel(double s,double sd) const; /** * Returns the acceleration twist at path length s and with * derivative of s == sd, and 2nd derivative of s == sdd */ virtual Twist Acc(double s,double sd,double sdd) const; /** * virtual constructor, constructing by copying. * In this case it returns the Clone() of the aggregated Path_Composite * because this is all one ever will need. */ virtual Path* Clone(); /** * Writes one of the derived objects to the stream */ virtual void Write(std::ostream& os); /** * returns the number of underlying segments. */ virtual int GetNrOfSegments(); /** * returns a pointer to the underlying Path of the given segment number i. * \param i segment number * \return pointer to the underlying Path * \warning The pointer is still owned by this class and is lifetime depends on the lifetime * of this class. */ virtual Path* GetSegment(int i); /** * gets the length to the end of the given segment. * \param i segment number * \return length to the end of the segment, i.e. the value for s corresponding to the end of * this segment. */ virtual double GetLengthToEndOfSegment(int i); /** * \param s [INPUT] path length variable for the composite. * \param segment_number [OUTPUT] segments that corresponds to the path length variable s. * \param inner_s [OUTPUT] path length to use within the segment. */ virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s); /** * gets an identifier indicating the type of this Path object */ virtual IdentifierType getIdentifier() const { return ID_ROUNDED_COMPOSITE; } virtual ~Path_RoundedComposite(); }; } #endif