Blender V4.3
frames_io.cpp
Go to the documentation of this file.
1
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
42namespace KDL {
43
44
45std::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
51std::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
62std::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
74std::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
108std::ostream& operator << (std::ostream& os, const Frame& T)
109{
110 os << "[" << T.M << std::endl<< T.p << "]";
111 return os;
112}
113
114std::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.
121std::ostream& operator << (std::ostream& os,const Rotation2& R) {
122 os << "[" << R.GetRot()*rad2deg << "]";
123 return os;
124}
125
126std::ostream& operator << (std::ostream& os, const Frame2& T)
127{
128 os << T.M << T.p;
129 return os;
130}
131
132std::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
155std::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
174std::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
193std::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
254std::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
285std::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}
295std::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}
305std::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
x
x
Definition BLI_expr_pylike_eval_test.cc:345
angle
static double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2)
Definition IK_Math.h:125
v
ATTR_WARN_UNUSED_RESULT const BMVert * v
Definition bmesh_query_inline.hh:17
z
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition btQuadWord.h:117
KDL::Error_Frame_Frame_Unexpected_id
Definition error.h:114
KDL::Error_Frame_Rotation_Unexpected_id
Definition error.h:119
KDL::Error_Frame_Vector_Unexpected_id
Definition error.h:109
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition frames.hpp:526
KDL::Frame::DH
static Frame DH(double a, double alpha, double d, double theta)
Definition frames.cpp:70
KDL::Rotation2::Rot
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition frames.inl:936
KDL::Rotation
represents rotations in 3 dimensional space.
Definition frames.hpp:299
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition frames.hpp:435
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition frames.inl:556
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotaxis, double angle)
Definition frames.cpp:250
KDL::Rotation::EulerZYZ
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
Definition frames.cpp:220
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition frames.cpp:195
KDL::Twist
represents both translational and rotational velocities.
Definition frames.hpp:679
KDL::Vector2
2D version of Vector
Definition frames.hpp:916
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:143
KDL::Vector::Zero
static Vector Zero()
Definition frames.inl:154
KDL::Wrench
represents both translational and rotational acceleration.
Definition frames.hpp:835
y
y
Definition compositor_morphological_blur_info.hh:15
error_stack.h
frames_io.hpp
KDL_FRAME_WIDTH
#define KDL_FRAME_WIDTH
Definition kdl-config.h:30
R
#define R
Definition mball_tessellate.cc:271
KDL
Definition chain.cpp:27
KDL::EatEnd
void EatEnd(std::istream &is, int delim)
Definition utility_io.cpp:123
KDL::deg2rad
const double deg2rad
the value pi/180
Definition utility.cpp:20
KDL::Eat
void Eat(std::istream &is, int delim)
Definition utility_io.cpp:109
KDL::EatWord
void EatWord(std::istream &is, const char *delim, char *storage, int maxsize)
Definition utility_io.cpp:183
KDL::operator<<
std::ostream & operator<<(std::ostream &os, const Vector &v)
Definition frames_io.cpp:45
KDL::rad2deg
const double rad2deg
the value 180/pi
Definition utility.cpp:21
KDL::IOTracePop
void IOTracePop()
pops a description of the IO-stack
Definition error_stack.cpp:39
KDL::IOTrace
void IOTrace(const std::string &description)
Definition error_stack.cpp:34
KDL::operator>>
std::istream & operator>>(std::istream &is, Vector &v)
Definition frames_io.cpp:132
Generated on Thu Feb 6 2025 07:36:39 for Blender by  doxygen 1.11.0