path.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Erwin Aertbelien  Mon May 10 19:10:36 CEST 2004  path.cxx
00003 
00004                         path.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Erwin Aertbelien
00008     email                : erwin.aertbelien@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 
00028 /*****************************************************************************
00029  *  \author
00030  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
00031  *
00032  *  \version
00033  *              ORO_Geometry V0.2
00034  *
00035  *      \par History
00036  *              - $log$
00037  *
00038  *      \par Release
00039  *              $Id: path.cpp,v 1.1.1.1.2.4 2003/07/18 14:49:50 psoetens Exp $
00040  *              $Name:  $
00041  ****************************************************************************/
00042 
00043 
00044 #include "utilities/error.h"
00045 #include "utilities/error_stack.h"
00046 #include "utilities/scoped_ptr.hpp"
00047 #include "path.hpp"
00048 #include "path_line.hpp"
00049 #include "path_point.hpp"
00050 #include "path_circle.hpp"
00051 #include "path_composite.hpp"
00052 #include "path_roundedcomposite.hpp"
00053 #include "path_cyclic_closed.hpp"
00054 #include <memory>
00055 #include <string.h>
00056 
00057 namespace KDL {
00058 
00059 using namespace std;
00060 
00061 
00062 Path* Path::Read(istream& is) {
00063         // auto_ptr because exception can be thrown !
00064         IOTrace("Path::Read");
00065         char storage[64];
00066         EatWord(is,"[",storage,sizeof(storage));
00067         Eat(is,'[');
00068         if (strcmp(storage,"POINT")==0) {
00069                 IOTrace("POINT");
00070                 Frame startpos;
00071                 is >> startpos;
00072                 EatEnd(is,']');
00073                 IOTracePop();
00074                 IOTracePop();
00075                 return new Path_Point(startpos);
00076         } else  if (strcmp(storage,"LINE")==0) {
00077                 IOTrace("LINE");
00078                 Frame startpos;
00079                 Frame endpos;
00080                 is >> startpos;
00081                 is >> endpos;
00082                 scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00083                 double eqradius;
00084                 is >> eqradius;
00085                 EatEnd(is,']');
00086                 IOTracePop();
00087                 IOTracePop();
00088                 return new Path_Line(startpos,endpos,orient.release(),eqradius);
00089         } else if (strcmp(storage,"CIRCLE")==0) {
00090                 IOTrace("CIRCLE");
00091                 Frame F_base_start;
00092                 Vector V_base_center;
00093                 Vector V_base_p;
00094                 Rotation R_base_end;
00095                 double alpha;
00096                 double eqradius;
00097                 is >> F_base_start;
00098                 is >> V_base_center;
00099                 is >> V_base_p;
00100                 is >> R_base_end;
00101                 is >> alpha;
00102                 alpha *= deg2rad;
00103                 scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00104                 is >> eqradius;
00105                 EatEnd(is,']');
00106                 IOTracePop();
00107                 IOTracePop();
00108                 return new Path_Circle(
00109                                                 F_base_start,
00110                                                 V_base_center,
00111                                                 V_base_p,
00112                                                 R_base_end,
00113                                                 alpha,
00114                                                 orient.release() ,
00115                                                 eqradius
00116                                         );
00117         } else if (strcmp(storage,"ROUNDEDCOMPOSITE")==0) {
00118                 IOTrace("ROUNDEDCOMPOSITE");
00119                 double radius;
00120                 is >> radius;
00121                 double eqradius;
00122                 is >> eqradius;
00123                 scoped_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00124                 scoped_ptr<Path_RoundedComposite> tr(
00125                         new Path_RoundedComposite(radius,eqradius,orient.release())
00126                 );
00127                 int size;
00128                 is >> size;
00129                 int i;
00130                 for (i=0;i<size;i++) {
00131                         Frame f;
00132                         is >> f;
00133                         tr->Add(f);
00134                 }
00135                 tr->Finish();
00136                 EatEnd(is,']');
00137                 IOTracePop();
00138                 IOTracePop();
00139                 return tr.release();
00140         } else if (strcmp(storage,"COMPOSITE")==0) {
00141                 IOTrace("COMPOSITE");
00142                 int size;
00143                 scoped_ptr<Path_Composite> tr( new Path_Composite() );
00144                 is >> size;
00145                 int i;
00146                 for (i=0;i<size;i++) {
00147                         tr->Add(Path::Read(is));
00148                 }
00149                 EatEnd(is,']');
00150                 IOTracePop();
00151                 IOTracePop();
00152                 return tr.release();
00153         } else if (strcmp(storage,"CYCLIC_CLOSED")==0) {
00154                 IOTrace("CYCLIC_CLOSED");
00155                 int times;
00156                 scoped_ptr<Path> tr( Path::Read(is) );
00157                 is >> times;
00158                 EatEnd(is,']');
00159                 IOTracePop();
00160                 IOTracePop();
00161                 return new Path_Cyclic_Closed(tr.release(),times);
00162         } else {
00163                 throw Error_MotionIO_Unexpected_Traj();
00164         }
00165         return NULL; // just to avoid the warning;
00166 }
00167 
00168 
00169 
00170 }
00171 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22