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 "path.hpp"
00047 #include "path_line.hpp"
00048 #include "path_point.hpp"
00049 #include "path_circle.hpp"
00050 #include "path_composite.hpp"
00051 #include "path_roundedcomposite.hpp"
00052 #include "path_cyclic_closed.hpp"
00053 #include <memory>
00054 #include <string.h>
00055
00056 namespace KDL {
00057
00058 using namespace std;
00059
00060
00061 Path* Path::Read(istream& is) {
00062
00063 IOTrace("Path::Read");
00064 char storage[64];
00065 EatWord(is,"[",storage,sizeof(storage));
00066 Eat(is,'[');
00067 if (strcmp(storage,"POINT")==0) {
00068 IOTrace("POINT");
00069 Frame startpos;
00070 is >> startpos;
00071 EatEnd(is,']');
00072 IOTracePop();
00073 IOTracePop();
00074 return new Path_Point(startpos);
00075 } else if (strcmp(storage,"LINE")==0) {
00076 IOTrace("LINE");
00077 Frame startpos;
00078 Frame endpos;
00079 is >> startpos;
00080 is >> endpos;
00081 auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00082 double eqradius;
00083 is >> eqradius;
00084 EatEnd(is,']');
00085 IOTracePop();
00086 IOTracePop();
00087 return new Path_Line(startpos,endpos,orient.release(),eqradius);
00088 } else if (strcmp(storage,"CIRCLE")==0) {
00089 IOTrace("CIRCLE");
00090 Frame F_base_start;
00091 Vector V_base_center;
00092 Vector V_base_p;
00093 Rotation R_base_end;
00094 double alpha;
00095 double eqradius;
00096 is >> F_base_start;
00097 is >> V_base_center;
00098 is >> V_base_p;
00099 is >> R_base_end;
00100 is >> alpha;
00101 alpha *= deg2rad;
00102 auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00103 is >> eqradius;
00104 EatEnd(is,']');
00105 IOTracePop();
00106 IOTracePop();
00107 return new Path_Circle(
00108 F_base_start,
00109 V_base_center,
00110 V_base_p,
00111 R_base_end,
00112 alpha,
00113 orient.release() ,
00114 eqradius
00115 );
00116 } else if (strcmp(storage,"ROUNDEDCOMPOSITE")==0) {
00117 IOTrace("ROUNDEDCOMPOSITE");
00118 double radius;
00119 is >> radius;
00120 double eqradius;
00121 is >> eqradius;
00122 auto_ptr<RotationalInterpolation> orient( RotationalInterpolation::Read(is) );
00123 auto_ptr<Path_RoundedComposite> tr(
00124 new Path_RoundedComposite(radius,eqradius,orient.release())
00125 );
00126 int size;
00127 is >> size;
00128 int i;
00129 for (i=0;i<size;i++) {
00130 Frame f;
00131 is >> f;
00132 tr->Add(f);
00133 }
00134 tr->Finish();
00135 EatEnd(is,']');
00136 IOTracePop();
00137 IOTracePop();
00138 return tr.release();
00139 } else if (strcmp(storage,"COMPOSITE")==0) {
00140 IOTrace("COMPOSITE");
00141 int size;
00142 auto_ptr<Path_Composite> tr( new Path_Composite() );
00143 is >> size;
00144 int i;
00145 for (i=0;i<size;i++) {
00146 tr->Add(Path::Read(is));
00147 }
00148 EatEnd(is,']');
00149 IOTracePop();
00150 IOTracePop();
00151 return tr.release();
00152 } else if (strcmp(storage,"CYCLIC_CLOSED")==0) {
00153 IOTrace("CYCLIC_CLOSED");
00154 int times;
00155 auto_ptr<Path> tr( Path::Read(is) );
00156 is >> times;
00157 EatEnd(is,']');
00158 IOTracePop();
00159 IOTracePop();
00160 return new Path_Cyclic_Closed(tr.release(),times);
00161 } else {
00162 throw Error_MotionIO_Unexpected_Traj();
00163 }
00164 return NULL;
00165 }
00166
00167
00168
00169 }
00170