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 "utilities/scoped_ptr.hpp"
00046 #include <memory>
00047 
00048 namespace KDL {
00049 
00050 // s should be in allowable limits, this is not checked
00051 // simple linear search : TODO : make it binary search
00052 // uses cached_... variables
00053 // returns the relative path length within the segment
00054 // you probably want to use the cached_index variable
00055 double Path_Composite::Lookup(double s) const
00056 {
00057         assert(s>=-1e-12);
00058         assert(s<=pathlength+1e-12);
00059         if ( (cached_starts <=s) && ( s <= cached_ends) ) {
00060                 return s - cached_starts;
00061         }
00062         double previous_s=0;
00063         for (unsigned int i=0;i<dv.size();++i) {
00064                 if ((s <= dv[i])||(i == (dv.size()-1) )) {
00065                         cached_index = i;
00066                         cached_starts = previous_s;
00067                         cached_ends   = dv[i];
00068                         return s - previous_s;
00069                 }
00070                 previous_s = dv[i];
00071         }
00072         return 0;
00073 }
00074 
00075 Path_Composite::Path_Composite() {
00076         pathlength    = 0;
00077         cached_starts = 0;
00078         cached_ends   = 0;
00079         cached_index  = 0;
00080 }
00081 
00082 void Path_Composite::Add(Path* geom, bool aggregate ) {
00083         pathlength += geom->PathLength();
00084         dv.insert(dv.end(),pathlength);
00085         gv.insert( gv.end(),std::make_pair(geom,aggregate) );
00086 }
00087 
00088 double Path_Composite::LengthToS(double length) {
00089         throw Error_MotionPlanning_Not_Applicable();
00090         return 0;
00091 }
00092 
00093 double Path_Composite::PathLength() {
00094         return pathlength;
00095 }
00096 
00097 
00098 Frame Path_Composite::Pos(double s) const {
00099         s = Lookup(s);
00100         return gv[cached_index].first->Pos(s);
00101 }
00102 
00103 Twist Path_Composite::Vel(double s,double sd) const {
00104         s = Lookup(s);
00105         return gv[cached_index].first->Vel(s,sd);
00106 }
00107 
00108 Twist Path_Composite::Acc(double s,double sd,double sdd) const {
00109         s = Lookup(s);
00110         return gv[cached_index].first->Acc(s,sd,sdd);
00111 }
00112 
00113 Path* Path_Composite::Clone()  {
00114         scoped_ptr<Path_Composite> comp( new Path_Composite() );
00115         for (unsigned int i = 0; i < dv.size(); ++i) {
00116                 comp->Add(gv[i].first->Clone(), gv[i].second);
00117         }
00118         return comp.release();
00119 }
00120 
00121 void Path_Composite::Write(std::ostream& os)  {
00122         os << "COMPOSITE[ " << std::endl;
00123         os << "   " << dv.size() << std::endl;
00124         for (unsigned int i=0;i<dv.size();i++) {
00125                 gv[i].first->Write(os);
00126         }
00127         os << "]" << std::endl;
00128 }
00129 
00130 int Path_Composite::GetNrOfSegments() {
00131         return static_cast<int>(dv.size());
00132 }
00133 
00134 Path* Path_Composite::GetSegment(int i) {
00135         assert(i>=0);
00136         assert(i<static_cast<int>(dv.size()));
00137         return gv[i].first;
00138 }
00139 
00140 double Path_Composite::GetLengthToEndOfSegment(int i) {
00141         assert(i>=0);
00142         assert(i<static_cast<int>(dv.size()));
00143         return dv[i];
00144 }
00145 
00146 void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
00147                 double& inner_s)
00148 {
00149         inner_s = Lookup(s);
00150         segment_number= cached_index;
00151 }
00152 
00153 Path_Composite::~Path_Composite() {
00154         PathVector::iterator it;
00155         for (it=gv.begin();it!=gv.end();++it) {
00156                 if (it->second)
00157             delete it->first;
00158         }
00159 }
00160 
00161 } // namespace KDL


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22