path.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path.cxx
3 
4  path.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 /*****************************************************************************
29  * \author
30  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
31  *
32  * \version
33  * ORO_Geometry V0.2
34  *
35  * \par History
36  * - $log$
37  *
38  * \par Release
39  * $Id: path.cpp,v 1.1.1.1.2.4 2003/07/18 14:49:50 psoetens Exp $
40  * $Name: $
41  ****************************************************************************/
42 
43 
44 #include "utilities/error.h"
45 #include "utilities/error_stack.h"
46 #include "utilities/scoped_ptr.hpp"
47 #include "path.hpp"
48 #include "path_line.hpp"
49 #include "path_point.hpp"
50 #include "path_circle.hpp"
51 #include "path_composite.hpp"
52 #include "path_roundedcomposite.hpp"
53 #include "path_cyclic_closed.hpp"
54 #include <memory>
55 #include <string.h>
56 
57 namespace KDL {
58 
59 using namespace std;
60 
61 
62 Path* Path::Read(istream& is) {
63  // auto_ptr because exception can be thrown !
64  IOTrace("Path::Read");
65  char storage[64];
66  EatWord(is,"[",storage,sizeof(storage));
67  Eat(is,'[');
68  if (strcmp(storage,"POINT")==0) {
69  IOTrace("POINT");
70  Frame startpos;
71  is >> startpos;
72  EatEnd(is,']');
73  IOTracePop();
74  IOTracePop();
75  return new Path_Point(startpos);
76  } else if (strcmp(storage,"LINE")==0) {
77  IOTrace("LINE");
78  Frame startpos;
79  Frame endpos;
80  is >> startpos;
81  is >> endpos;
82  scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
83  double eqradius;
84  is >> eqradius;
85  EatEnd(is,']');
86  IOTracePop();
87  IOTracePop();
88  return new Path_Line(startpos,endpos,orient.release(),eqradius);
89  } else if (strcmp(storage,"CIRCLE")==0) {
90  IOTrace("CIRCLE");
91  Frame F_base_start;
92  Vector V_base_center;
93  Vector V_base_p;
94  Rotation R_base_end;
95  double alpha;
96  double eqradius;
97  is >> F_base_start;
98  is >> V_base_center;
99  is >> V_base_p;
100  is >> R_base_end;
101  is >> alpha;
102  alpha *= deg2rad;
103  scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
104  is >> eqradius;
105  EatEnd(is,']');
106  IOTracePop();
107  IOTracePop();
108  return new Path_Circle(
109  F_base_start,
110  V_base_center,
111  V_base_p,
112  R_base_end,
113  alpha,
114  orient.release() ,
115  eqradius
116  );
117  } else if (strcmp(storage,"ROUNDEDCOMPOSITE")==0) {
118  IOTrace("ROUNDEDCOMPOSITE");
119  double radius;
120  is >> radius;
121  double eqradius;
122  is >> eqradius;
123  scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
124  scoped_ptr<Path_RoundedComposite> tr(
125  new Path_RoundedComposite(radius,eqradius,orient.release())
126  );
127  int size;
128  is >> size;
129  int i;
130  for (i=0;i<size;i++) {
131  Frame f;
132  is >> f;
133  tr->Add(f);
134  }
135  tr->Finish();
136  EatEnd(is,']');
137  IOTracePop();
138  IOTracePop();
139  return tr.release();
140  } else if (strcmp(storage,"COMPOSITE")==0) {
141  IOTrace("COMPOSITE");
142  int size;
143  scoped_ptr<Path_Composite> tr( new Path_Composite() );
144  is >> size;
145  int i;
146  for (i=0;i<size;i++) {
147  tr->Add(Path::Read(is));
148  }
149  EatEnd(is,']');
150  IOTracePop();
151  IOTracePop();
152  return tr.release();
153  } else if (strcmp(storage,"CYCLIC_CLOSED")==0) {
154  IOTrace("CYCLIC_CLOSED");
155  int times;
156  scoped_ptr<Path> tr( Path::Read(is) );
157  is >> times;
158  EatEnd(is,']');
159  IOTracePop();
160  IOTracePop();
161  return new Path_Cyclic_Closed(tr.release(),times);
162  } else {
163  throw Error_MotionIO_Unexpected_Traj();
164  }
165  return NULL; // just to avoid the warning;
166 }
167 
168 
169 
170 }
171 
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
path_line.hpp
KDL::IOTracePop
void IOTracePop()
pops a description of the IO-stack
KDL::EatEnd
void EatEnd(std::istream &is, int delim)
KDL::EatWord
void EatWord(std::istream &is, const char *delim, char *storage, int maxsize)
KDL::Path::Read
static Path * Read(std::istream &is)
Definition: path.cpp:62
KDL::RotationalInterpolation::Read
static RotationalInterpolation * Read(std::istream &is)
Definition: rotational_interpolation.cpp:53
KDL::Path_Circle
Definition: path_circle.hpp:60
KDL::Eat
void Eat(std::istream &is, int delim)
path_composite.hpp
KDL::Error_MotionIO_Unexpected_Traj
Definition: error.h:156
KDL::Path_RoundedComposite
Definition: path_roundedcomposite.hpp:58
path_circle.hpp
path_cyclic_closed.hpp
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::scoped_ptr
Definition: scoped_ptr.hpp:33
path_roundedcomposite.hpp
KDL::Path_Cyclic_Closed
Definition: path_cyclic_closed.hpp:60
KDL::Path_Line
Definition: path_line.hpp:59
KDL::Path_Composite
Definition: path_composite.hpp:69
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
path_point.hpp
KDL::scoped_ptr::release
T * release()
Definition: scoped_ptr.hpp:56
error_stack.h
KDL::Path_Point
Definition: path_point.hpp:61
KDL::IOTrace
void IOTrace(const std::string &description)
scoped_ptr.hpp
KDL::deg2rad
const double deg2rad
the value pi/180

orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44