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