path_composite.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_composite.cxx
3 
4  path_composite.cxx - description
5  -------------------
6  begin : Mon May 10 2004
7  copyright : (C) 2004 Erwin Aertbelien
8  email : erwin.aertbelien@mech.kuleuven.ac.be
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU Lesser General Public *
13  * License as published by the Free Software Foundation; either *
14  * version 2.1 of the License, or (at your option) any later version. *
15  * *
16  * This library is distributed in the hope that it will be useful, *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
19  * Lesser General Public License for more details. *
20  * *
21  * You should have received a copy of the GNU Lesser General Public *
22  * License along with this library; if not, write to the Free Software *
23  * Foundation, Inc., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 /*****************************************************************************
28  * \author
29  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
30  *
31  * \version
32  * ORO_Geometry V0.2
33  *
34  * \par History
35  * - $log$
36  *
37  * \par Release
38  * $Id: path_composite.cpp,v 1.1.1.1.2.7 2003/07/24 13:49:16 rwaarsin Exp $
39  * $Name: $
40  ****************************************************************************/
41 
42 
43 #include "path_composite.hpp"
44 #include "utilities/error.h"
45 #include "utilities/scoped_ptr.hpp"
46 #include <memory>
47 
48 namespace KDL {
49 
50 // s should be in allowable limits, this is not checked
51 // simple linear search : TODO : make it binary search
52 // uses cached_... variables
53 // returns the relative path length within the segment
54 // you probably want to use the cached_index variable
55 double Path_Composite::Lookup(double s) const
56 {
57  assert(s>=-1e-12);
58  assert(s<=pathlength+1e-12);
59  if ( (cached_starts <=s) && ( s <= cached_ends) ) {
60  return s - cached_starts;
61  }
62  double previous_s=0;
63  for (unsigned int i=0;i<dv.size();++i) {
64  if ((s <= dv[i])||(i == (dv.size()-1) )) {
65  cached_index = i;
66  cached_starts = previous_s;
67  cached_ends = dv[i];
68  return s - previous_s;
69  }
70  previous_s = dv[i];
71  }
72  return 0;
73 }
74 
76  pathlength = 0;
77  cached_starts = 0;
78  cached_ends = 0;
79  cached_index = 0;
80 }
81 
82 void Path_Composite::Add(Path* geom, bool aggregate ) {
83  pathlength += geom->PathLength();
84  dv.insert(dv.end(),pathlength);
85  gv.insert( gv.end(),std::make_pair(geom,aggregate) );
86 }
87 
88 double Path_Composite::LengthToS(double length) {
90  return 0;
91 }
92 
94  return pathlength;
95 }
96 
97 
98 Frame Path_Composite::Pos(double s) const {
99  s = Lookup(s);
100  return gv[cached_index].first->Pos(s);
101 }
102 
103 Twist Path_Composite::Vel(double s,double sd) const {
104  s = Lookup(s);
105  return gv[cached_index].first->Vel(s,sd);
106 }
107 
108 Twist Path_Composite::Acc(double s,double sd,double sdd) const {
109  s = Lookup(s);
110  return gv[cached_index].first->Acc(s,sd,sdd);
111 }
112 
115  for (unsigned int i = 0; i < dv.size(); ++i) {
116  comp->Add(gv[i].first->Clone(), gv[i].second);
117  }
118  return comp.release();
119 }
120 
121 void Path_Composite::Write(std::ostream& os) {
122  os << "COMPOSITE[ " << std::endl;
123  os << " " << dv.size() << std::endl;
124  for (unsigned int i=0;i<dv.size();i++) {
125  gv[i].first->Write(os);
126  }
127  os << "]" << std::endl;
128 }
129 
131  return static_cast<int>(dv.size());
132 }
133 
135  assert(i>=0);
136  assert(i<static_cast<int>(dv.size()));
137  return gv[i].first;
138 }
139 
141  assert(i>=0);
142  assert(i<static_cast<int>(dv.size()));
143  return dv[i];
144 }
145 
146 void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
147  double& inner_s)
148 {
149  inner_s = Lookup(s);
150  segment_number= cached_index;
151 }
152 
154  PathVector::iterator it;
155  for (it=gv.begin();it!=gv.end();++it) {
156  if (it->second)
157  delete it->first;
158  }
159 }
160 
161 } // namespace KDL
void Add(Path *geom, bool aggregate=true)
virtual double PathLength()
virtual Path * GetSegment(int i)
represents both translational and rotational velocities.
Definition: frames.hpp:723
virtual void Write(std::ostream &os)
virtual void GetCurrentSegmentLocation(double s, int &segment_number, double &inner_s)
virtual Frame Pos(double s) const
virtual Twist Vel(double s, double sd) const
virtual int GetNrOfSegments()
virtual Path * Clone()
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
virtual double GetLengthToEndOfSegment(int i)
virtual double LengthToS(double length)
virtual Twist Acc(double s, double sd, double sdd) const
virtual double PathLength()=0
double Lookup(double s) const


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14