path_composite.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  path_composite.cxx
00003 
00004                         path_composite.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Erwin Aertbelien
00008     email                : erwin.aertbelien@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 /*****************************************************************************
00028  *  \author
00029  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00030  *
00031  *  \version
00032  *              ORO_Geometry V0.2
00033  *
00034  *      \par History
00035  *              - $log$
00036  *
00037  *      \par Release
00038  *              $Id: path_composite.cpp,v 1.1.1.1.2.7 2003/07/24 13:49:16 rwaarsin Exp $
00039  *              $Name:  $
00040  ****************************************************************************/
00041 
00042 
00043 #include "path_composite.hpp"
00044 #include "utilities/error.h"
00045 #include <memory>
00046 
00047 namespace KDL {
00048 
00049 // s should be in allowable limits, this is not checked
00050 // simple linear search : TODO : make it binary search
00051 // uses cached_... variables
00052 // returns the relative path length within the segment
00053 // you propably want to use the cached_index variable
00054 double Path_Composite::Lookup(double s) const
00055 {
00056         assert(s>=-1e-12);
00057         assert(s<=pathlength+1e-12);
00058         if ( (cached_starts <=s) && ( s <= cached_ends) ) {
00059                 return s - cached_starts;
00060         }
00061         double previous_s=0;
00062         for (unsigned int i=0;i<dv.size();++i) {
00063                 if ((s <= dv[i])||(i == (dv.size()-1) )) {
00064                         cached_index = i;
00065                         cached_starts = previous_s;
00066                         cached_ends   = dv[i];
00067                         return s - previous_s;
00068                 }
00069                 previous_s = dv[i];
00070         }
00071         return 0;
00072 }
00073 
00074 Path_Composite::Path_Composite() {
00075         pathlength    = 0;
00076         cached_starts = 0;
00077         cached_ends   = 0;
00078         cached_index  = 0;
00079 }
00080 
00081 void Path_Composite::Add(Path* geom, bool aggregate ) {
00082         pathlength += geom->PathLength();
00083         dv.insert(dv.end(),pathlength);
00084         gv.insert( gv.end(),std::make_pair(geom,aggregate) );
00085 }
00086 
00087 double Path_Composite::LengthToS(double length) {
00088         throw Error_MotionPlanning_Not_Applicable();
00089         return 0;
00090 }
00091 
00092 double Path_Composite::PathLength() {
00093         return pathlength;
00094 }
00095 
00096 
00097 Frame Path_Composite::Pos(double s) const {
00098         s = Lookup(s);
00099         return gv[cached_index].first->Pos(s);
00100 }
00101 
00102 Twist Path_Composite::Vel(double s,double sd) const {
00103         s = Lookup(s);
00104         return gv[cached_index].first->Vel(s,sd);
00105 }
00106 
00107 Twist Path_Composite::Acc(double s,double sd,double sdd) const {
00108         s = Lookup(s);
00109         return gv[cached_index].first->Acc(s,sd,sdd);
00110 }
00111 
00112 Path* Path_Composite::Clone()  {
00113         std::auto_ptr<Path_Composite> comp( new Path_Composite() );
00114         for (unsigned int i = 0; i < dv.size(); ++i) {
00115                 comp->Add(gv[i].first->Clone(), gv[i].second);
00116         }
00117         return comp.release();
00118 }
00119 
00120 void Path_Composite::Write(std::ostream& os)  {
00121         os << "COMPOSITE[ " << std::endl;
00122         os << "   " << dv.size() << std::endl;
00123         for (unsigned int i=0;i<dv.size();i++) {
00124                 gv[i].first->Write(os);
00125         }
00126         os << "]" << std::endl;
00127 }
00128 
00129 int Path_Composite::GetNrOfSegments() {
00130         return dv.size();
00131 }
00132 
00133 Path* Path_Composite::GetSegment(int i) {
00134         assert(i>=0);
00135         assert(i<dv.size());
00136         return gv[i].first;
00137 }
00138 
00139 double Path_Composite::GetLengthToEndOfSegment(int i) {
00140         assert(i>=0);
00141         assert(i<dv.size());
00142         return dv[i];
00143 }
00144 
00145 void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
00146                 double& inner_s)
00147 {
00148         inner_s = Lookup(s);
00149         segment_number= cached_index;
00150 }
00151 
00152 Path_Composite::~Path_Composite() {
00153         PathVector::iterator it;
00154         for (it=gv.begin();it!=gv.end();++it) {
00155                 if (it->second)
00156             delete it->first;
00157         }
00158 }
00159 
00160 } // namespace KDL


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14