Go to the documentation of this file.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
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
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
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;
00166 }
00167
00168
00169
00170 }
00171