orocos_kdl
frames_io.cpp
Go to the documentation of this file.
1 
2 /***************************************************************************
3  frames_io.h - description
4  -------------------------
5  begin : June 2006
6  copyright : (C) 2006 Erwin Aertbelien
7  email : firstname.lastname@mech.kuleuven.ac.be
8 
9  History (only major changes)( AUTHOR-Description ) :
10 
11  ***************************************************************************
12  * This library is free software; you can redistribute it and/or *
13  * modify it under the terms of the GNU Lesser General Public *
14  * License as published by the Free Software Foundation; either *
15  * version 2.1 of the License, or (at your option) any later version. *
16  * *
17  * This library is distributed in the hope that it will be useful, *
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
20  * Lesser General Public License for more details. *
21  * *
22  * You should have received a copy of the GNU Lesser General Public *
23  * License along with this library; if not, write to the Free Software *
24  * Foundation, Inc., 59 Temple Place, *
25  * Suite 330, Boston, MA 02111-1307 USA *
26  * *
27  ***************************************************************************/
28 
29 #include "utilities/error.h"
30 #include "utilities/error_stack.h"
31 #include "frames.hpp"
32 #include "frames_io.hpp"
33 
34 #include <stdlib.h>
35 #include <ctype.h>
36 #include <string.h>
37 #include <iostream>
38 
39 namespace KDL {
40 
41 
42 std::ostream& operator << (std::ostream& os,const Vector& v) {
43  os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
44  << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
45  return os;
46 }
47 
48 std::ostream& operator << (std::ostream& os,const Twist& v) {
49  os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
50  << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
51  << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
52  << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
53  << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
54  << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
55  << "]";
56  return os;
57 }
58 
59 std::ostream& operator << (std::ostream& os,const Wrench& v) {
60  os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
61  << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
62  << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
63  << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
64  << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
65  << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
66  << "]";
67  return os;
68 }
69 
70 
71 std::ostream& operator << (std::ostream& os,const Rotation& R) {
72 #ifdef KDL_ROTATION_PROPERTIES_RPY
73  double r,p,y;
74  R.GetRPY(r,p,y);
75  os << "[RPY]"<<endl;
76  os << "[";
77  os << std::setw(KDL_FRAME_WIDTH) << r << ",";
78  os << std::setw(KDL_FRAME_WIDTH) << p << ",";
79  os << std::setw(KDL_FRAME_WIDTH) << y << "]";
80 #else
81 # ifdef KDL_ROTATION_PROPERTIES_EULER
82  double z,y,x;
83  R.GetEulerZYX(z,y,x);
84  os << "[EULERZYX]"<<endl;
85  os << "[";
86  os << std::setw(KDL_FRAME_WIDTH) << z << ",";
87  os << std::setw(KDL_FRAME_WIDTH) << y << ",";
88  os << std::setw(KDL_FRAME_WIDTH) << x << "]";
89 # else
90  os << "[";
91  for (int i=0;i<=2;i++) {
92  os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
93  std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
94  std::setw(KDL_FRAME_WIDTH) << R(i,2);
95  if (i<2)
96  os << ";"<< std::endl << " ";
97  else
98  os << "]";
99  }
100 # endif
101 #endif
102  return os;
103 }
104 
105 std::ostream& operator << (std::ostream& os, const Frame& T)
106 {
107  os << "[" << T.M << std::endl<< T.p << "]";
108  return os;
109 }
110 
111 std::ostream& operator << (std::ostream& os,const Vector2& v) {
112  os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
113  << "]";
114  return os;
115 }
116 
117 // Rotation2 gives back an angle in degrees with the << and >> operators.
118 std::ostream& operator << (std::ostream& os,const Rotation2& R) {
119  os << "[" << R.GetRot()*rad2deg << "]";
120  return os;
121 }
122 
123 std::ostream& operator << (std::ostream& os, const Frame2& T)
124 {
125  os << T.M << T.p;
126  return os;
127 }
128 
129 std::istream& operator >> (std::istream& is,Vector& v)
130 { IOTrace("Stream input Vector (vector or ZERO)");
131  char storage[10];
132  EatWord(is,"[]",storage,10);
133  if (strlen(storage)==0) {
134  Eat(is,'[');
135  is >> v(0);
136  Eat(is,',');
137  is >> v(1);
138  Eat(is,',');
139  is >> v(2);
140  EatEnd(is,']');
141  IOTracePop();
142  return is;
143  }
144  if (strcmp(storage,"ZERO")==0) {
145  v = Vector::Zero();
146  IOTracePop();
147  return is;
148  }
149  throw Error_Frame_Vector_Unexpected_id();
150 }
151 
152 std::istream& operator >> (std::istream& is,Twist& v)
153 { IOTrace("Stream input Twist");
154  Eat(is,'[');
155  is >> v.vel(0);
156  Eat(is,',');
157  is >> v.vel(1);
158  Eat(is,',');
159  is >> v.vel(2);
160  Eat(is,',');
161  is >> v.rot(0);
162  Eat(is,',');
163  is >> v.rot(1);
164  Eat(is,',');
165  is >> v.rot(2);
166  EatEnd(is,']');
167  IOTracePop();
168  return is;
169 }
170 
171 std::istream& operator >> (std::istream& is,Wrench& v)
172 { IOTrace("Stream input Wrench");
173  Eat(is,'[');
174  is >> v.force(0);
175  Eat(is,',');
176  is >> v.force(1);
177  Eat(is,',');
178  is >> v.force(2);
179  Eat(is,',');
180  is >> v.torque(0);
181  Eat(is,',');
182  is >> v.torque(1);
183  Eat(is,',');
184  is >> v.torque(2);
185  EatEnd(is,']');
186  IOTracePop();
187  return is;
188 }
189 
190 std::istream& operator >> (std::istream& is,Rotation& r)
191 { IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)");
192  char storage[10];
193  EatWord(is,"[]",storage,10);
194  if (strlen(storage)==0) {
195  Eat(is,'[');
196  for (int i=0;i<3;i++) {
197  is >> r(i,0);
198  Eat(is,',') ;
199  is >> r(i,1);
200  Eat(is,',');
201  is >> r(i,2);
202  if (i<2)
203  Eat(is,';');
204  else
205  EatEnd(is,']');
206  }
207  IOTracePop();
208  return is;
209  }
210  Vector v;
211  if (strcmp(storage,"EULERZYX")==0) {
212  is >> v;
213  v=v*deg2rad;
214  r = Rotation::EulerZYX(v(0),v(1),v(2));
215  IOTracePop();
216  return is;
217  }
218  if (strcmp(storage,"EULERZYZ")==0) {
219  is >> v;
220  v=v*deg2rad;
221  r = Rotation::EulerZYZ(v(0),v(1),v(2));
222  IOTracePop();
223  return is;
224  }
225  if (strcmp(storage,"RPY")==0) {
226  is >> v;
227  v=v*deg2rad;
228  r = Rotation::RPY(v(0),v(1),v(2));
229  IOTracePop();
230  return is;
231  }
232  if (strcmp(storage,"ROT")==0) {
233  is >> v;
234  double angle;
235  Eat(is,'[');
236  is >> angle;
237  EatEnd(is,']');
238  r = Rotation::Rot(v,angle*deg2rad);
239  IOTracePop();
240  return is;
241  }
242  if (strcmp(storage,"IDENTITY")==0) {
243  r = Rotation::Identity();
244  IOTracePop();
245  return is;
246  }
247  throw Error_Frame_Rotation_Unexpected_id();
248  return is;
249 }
250 
251 std::istream& operator >> (std::istream& is,Frame& T)
252 { IOTrace("Stream input Frame (Rotation,Vector) or DH[...]");
253  char storage[10];
254  EatWord(is,"[",storage,10);
255  if (strlen(storage)==0) {
256  Eat(is,'[');
257  is >> T.M;
258  is >> T.p;
259  EatEnd(is,']');
260  IOTracePop();
261  return is;
262  }
263  if (strcmp(storage,"DH")==0) {
264  double a,alpha,d,theta;
265  Eat(is,'[');
266  is >> a;
267  Eat(is,',');
268  is >> alpha;
269  Eat(is,',');
270  is >> d;
271  Eat(is,',');
272  is >> theta;
273  EatEnd(is,']');
274  T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad);
275  IOTracePop();
276  return is;
277  }
278  throw Error_Frame_Frame_Unexpected_id();
279  return is;
280 }
281 
282 std::istream& operator >> (std::istream& is,Vector2& v)
283 { IOTrace("Stream input Vector2");
284  Eat(is,'[');
285  is >> v(0);
286  Eat(is,',');
287  is >> v(1);
288  EatEnd(is,']');
289  IOTracePop();
290  return is;
291 }
292 std::istream& operator >> (std::istream& is,Rotation2& r)
293 { IOTrace("Stream input Rotation2");
294  Eat(is,'[');
295  double val;
296  is >> val;
297  r.Rot(val*deg2rad);
298  EatEnd(is,']');
299  IOTracePop();
300  return is;
301 }
302 std::istream& operator >> (std::istream& is,Frame2& T)
303 { IOTrace("Stream input Frame2");
304  is >> T.M;
305  is >> T.p;
306  IOTracePop();
307  return is;
308 }
309 
310 } // namespace Frame
std::strcmp
T strcmp(T... args)
frames.hpp
std::strlen
T strlen(T... args)
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
frames_io.hpp
iostream
KDL::deg2rad
const double deg2rad
the value pi/180
Definition: utility.cxx:19
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL_FRAME_WIDTH
#define KDL_FRAME_WIDTH
Definition: kdl-config.h:27
std::ostream
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:552
KDL::IOTrace
void IOTrace(const std::string &description)
Definition: error_stack.cxx:46
error.h
KDL::rad2deg
const double rad2deg
the value 180/pi
Definition: utility.cxx:20
KDL::EatEnd
void EatEnd(std::istream &is, int delim)
Definition: utility_io.cxx:137
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition: frames.cpp:287
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition: frames.cpp:95
KDL::operator>>
std::istream & operator>>(std::istream &is, Vector &v)
Definition: frames_io.cpp:154
std::endl
T endl(T... args)
KDL::IOTracePop
void IOTracePop()
pops a description of the IO-stack
Definition: error_stack.cxx:51
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:471
KDL::EatWord
void EatWord(std::istream &is, const char *delim, char *storage, int maxsize)
Definition: utility_io.cxx:197
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
error_stack.h
KDL::operator<<
std::ostream & operator<<(std::ostream &os, const VectorAcc &r)
Definition: frameacc_io.hpp:49
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:262
std::setw
T setw(T... args)
KDL::Eat
void Eat(std::istream &is, int delim)
Definition: utility_io.cxx:123
std::istream
Generated on Tue Nov 26 2024 04:35:24 for orocos_kdl by   doxygen 1.8.17