orocos_kdl: orocos_kdl: frames

00001 
00002 /***************************************************************************
00003                         frames_io.h -  description
00004                        -------------------------
00005     begin                : June 2006
00006     copyright            : (C) 2006 Erwin Aertbelien
00007     email                : firstname.lastname@mech.kuleuven.ac.be
00008 
00009  History (only major changes)( AUTHOR-Description ) :
00010 
00011  ***************************************************************************
00012  *   This library is free software; you can redistribute it and/or         *
00013  *   modify it under the terms of the GNU Lesser General Public            *
00014  *   License as published by the Free Software Foundation; either          *
00015  *   version 2.1 of the License, or (at your option) any later version.    *
00016  *                                                                         *
00017  *   This library is distributed in the hope that it will be useful,       *
00018  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00019  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00020  *   Lesser General Public License for more details.                       *
00021  *                                                                         *
00022  *   You should have received a copy of the GNU Lesser General Public      *
00023  *   License along with this library; if not, write to the Free Software   *
00024  *   Foundation, Inc., 59 Temple Place,                                    *
00025  *   Suite 330, Boston, MA  02111-1307  USA                                *
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 // Rotation2 gives back an angle in degrees with the << and >> operators.
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 } // namespace Frame
frames_io.cpp
