00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include "utilities/error.h"
00030 #include "utilities/error_stack.h"
00031 #include "frames.hpp"
00032 #include "frames_io.hpp"
00033
00034 #include <stdlib.h>
00035 #include <ctype.h>
00036 #include <string.h>
00037 #include <iostream>
00038
00039 namespace KDL {
00040
00041
00042 std::ostream& operator << (std::ostream& os,const Vector& v) {
00043 os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
00044 << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
00045 return os;
00046 }
00047
00048 std::ostream& operator << (std::ostream& os,const Twist& v) {
00049 os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
00050 << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
00051 << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
00052 << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
00053 << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
00054 << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
00055 << "]";
00056 return os;
00057 }
00058
00059 std::ostream& operator << (std::ostream& os,const Wrench& v) {
00060 os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
00061 << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
00062 << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
00063 << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
00064 << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
00065 << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
00066 << "]";
00067 return os;
00068 }
00069
00070
00071 std::ostream& operator << (std::ostream& os,const Rotation& R) {
00072 #ifdef KDL_ROTATION_PROPERTIES_RPY
00073 double r,p,y;
00074 R.GetRPY(r,p,y);
00075 os << "[RPY]"<<endl;
00076 os << "[";
00077 os << std::setw(KDL_FRAME_WIDTH) << r << ",";
00078 os << std::setw(KDL_FRAME_WIDTH) << p << ",";
00079 os << std::setw(KDL_FRAME_WIDTH) << y << "]";
00080 #else
00081 # ifdef KDL_ROTATION_PROPERTIES_EULER
00082 double z,y,x;
00083 R.GetEulerZYX(z,y,x);
00084 os << "[EULERZYX]"<<endl;
00085 os << "[";
00086 os << std::setw(KDL_FRAME_WIDTH) << z << ",";
00087 os << std::setw(KDL_FRAME_WIDTH) << y << ",";
00088 os << std::setw(KDL_FRAME_WIDTH) << x << "]";
00089 # else
00090 os << "[";
00091 for (int i=0;i<=2;i++) {
00092 os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
00093 std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
00094 std::setw(KDL_FRAME_WIDTH) << R(i,2);
00095 if (i<2)
00096 os << ";"<< std::endl << " ";
00097 else
00098 os << "]";
00099 }
00100 # endif
00101 #endif
00102 return os;
00103 }
00104
00105 std::ostream& operator << (std::ostream& os, const Frame& T)
00106 {
00107 os << "[" << T.M << std::endl<< T.p << "]";
00108 return os;
00109 }
00110
00111 std::ostream& operator << (std::ostream& os,const Vector2& v) {
00112 os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
00113 << "]";
00114 return os;
00115 }
00116
00117
00118 std::ostream& operator << (std::ostream& os,const Rotation2& R) {
00119 os << "[" << R.GetRot()*rad2deg << "]";
00120 return os;
00121 }
00122
00123 std::ostream& operator << (std::ostream& os, const Frame2& T)
00124 {
00125 os << T.M << T.p;
00126 return os;
00127 }
00128
00129 std::istream& operator >> (std::istream& is,Vector& v)
00130 { IOTrace("Stream input Vector (vector or ZERO)");
00131 char storage[10];
00132 EatWord(is,"[]",storage,10);
00133 if (strlen(storage)==0) {
00134 Eat(is,'[');
00135 is >> v(0);
00136 Eat(is,',');
00137 is >> v(1);
00138 Eat(is,',');
00139 is >> v(2);
00140 EatEnd(is,']');
00141 IOTracePop();
00142 return is;
00143 }
00144 if (strcmp(storage,"ZERO")==0) {
00145 v = Vector::Zero();
00146 IOTracePop();
00147 return is;
00148 }
00149 throw Error_Frame_Vector_Unexpected_id();
00150 }
00151
00152 std::istream& operator >> (std::istream& is,Twist& v)
00153 { IOTrace("Stream input Twist");
00154 Eat(is,'[');
00155 is >> v.vel(0);
00156 Eat(is,',');
00157 is >> v.vel(1);
00158 Eat(is,',');
00159 is >> v.vel(2);
00160 Eat(is,',');
00161 is >> v.rot(0);
00162 Eat(is,',');
00163 is >> v.rot(1);
00164 Eat(is,',');
00165 is >> v.rot(2);
00166 EatEnd(is,']');
00167 IOTracePop();
00168 return is;
00169 }
00170
00171 std::istream& operator >> (std::istream& is,Wrench& v)
00172 { IOTrace("Stream input Wrench");
00173 Eat(is,'[');
00174 is >> v.force(0);
00175 Eat(is,',');
00176 is >> v.force(1);
00177 Eat(is,',');
00178 is >> v.force(2);
00179 Eat(is,',');
00180 is >> v.torque(0);
00181 Eat(is,',');
00182 is >> v.torque(1);
00183 Eat(is,',');
00184 is >> v.torque(2);
00185 EatEnd(is,']');
00186 IOTracePop();
00187 return is;
00188 }
00189
00190 std::istream& operator >> (std::istream& is,Rotation& r)
00191 { IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)");
00192 char storage[10];
00193 EatWord(is,"[]",storage,10);
00194 if (strlen(storage)==0) {
00195 Eat(is,'[');
00196 for (int i=0;i<3;i++) {
00197 is >> r(i,0);
00198 Eat(is,',') ;
00199 is >> r(i,1);
00200 Eat(is,',');
00201 is >> r(i,2);
00202 if (i<2)
00203 Eat(is,';');
00204 else
00205 EatEnd(is,']');
00206 }
00207 IOTracePop();
00208 return is;
00209 }
00210 Vector v;
00211 if (strcmp(storage,"EULERZYX")==0) {
00212 is >> v;
00213 v=v*deg2rad;
00214 r = Rotation::EulerZYX(v(0),v(1),v(2));
00215 IOTracePop();
00216 return is;
00217 }
00218 if (strcmp(storage,"EULERZYZ")==0) {
00219 is >> v;
00220 v=v*deg2rad;
00221 r = Rotation::EulerZYZ(v(0),v(1),v(2));
00222 IOTracePop();
00223 return is;
00224 }
00225 if (strcmp(storage,"RPY")==0) {
00226 is >> v;
00227 v=v*deg2rad;
00228 r = Rotation::RPY(v(0),v(1),v(2));
00229 IOTracePop();
00230 return is;
00231 }
00232 if (strcmp(storage,"ROT")==0) {
00233 is >> v;
00234 double angle;
00235 Eat(is,'[');
00236 is >> angle;
00237 EatEnd(is,']');
00238 r = Rotation::Rot(v,angle*deg2rad);
00239 IOTracePop();
00240 return is;
00241 }
00242 if (strcmp(storage,"IDENTITY")==0) {
00243 r = Rotation::Identity();
00244 IOTracePop();
00245 return is;
00246 }
00247 throw Error_Frame_Rotation_Unexpected_id();
00248 return is;
00249 }
00250
00251 std::istream& operator >> (std::istream& is,Frame& T)
00252 { IOTrace("Stream input Frame (Rotation,Vector) or DH[...]");
00253 char storage[10];
00254 EatWord(is,"[",storage,10);
00255 if (strlen(storage)==0) {
00256 Eat(is,'[');
00257 is >> T.M;
00258 is >> T.p;
00259 EatEnd(is,']');
00260 IOTracePop();
00261 return is;
00262 }
00263 if (strcmp(storage,"DH")==0) {
00264 double a,alpha,d,theta;
00265 Eat(is,'[');
00266 is >> a;
00267 Eat(is,',');
00268 is >> alpha;
00269 Eat(is,',');
00270 is >> d;
00271 Eat(is,',');
00272 is >> theta;
00273 EatEnd(is,']');
00274 T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad);
00275 IOTracePop();
00276 return is;
00277 }
00278 throw Error_Frame_Frame_Unexpected_id();
00279 return is;
00280 }
00281
00282 std::istream& operator >> (std::istream& is,Vector2& v)
00283 { IOTrace("Stream input Vector2");
00284 Eat(is,'[');
00285 is >> v(0);
00286 Eat(is,',');
00287 is >> v(1);
00288 EatEnd(is,']');
00289 IOTracePop();
00290 return is;
00291 }
00292 std::istream& operator >> (std::istream& is,Rotation2& r)
00293 { IOTrace("Stream input Rotation2");
00294 Eat(is,'[');
00295 double val;
00296 is >> val;
00297 r.Rot(val*deg2rad);
00298 EatEnd(is,']');
00299 IOTracePop();
00300 return is;
00301 }
00302 std::istream& operator >> (std::istream& is,Frame2& T)
00303 { IOTrace("Stream input Frame2");
00304 is >> T.M;
00305 is >> T.p;
00306 IOTracePop();
00307 return is;
00308 }
00309
00310 }