00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00016
00017
00018
00019 #include <vpvector3d.h>
00020
00021 #include <vpmovement.h>
00022
00023 #include <string>
00024 using namespace std;
00025
00026
00027 VPMovement::VPMovement(){
00028
00029 bodyName = "";
00030 cycle = false;
00031 deltaT = 0;
00032 time = INIT_TIME;
00033 time_err = 0;
00034 }
00035
00036 VPMovement::VPMovement( string bn, bool cy, float dt, float te ){
00037 bodyName = bn;
00038 cycle = cy;
00039 deltaT = dt;
00040 time = INIT_TIME;
00041 time_err = te;
00042 }
00043
00044
00045 VPMovement::~VPMovement(){
00046 }
00047
00048 float
00049 VPMovement::vpGetDeltaT(){
00050 return deltaT;
00051 }
00052
00053 bool
00054 VPMovement::vpGetCycle(){
00055 return cycle;
00056 }
00057
00058 float
00059 VPMovement::vpGetTime(){
00060 return time;
00061 }
00062
00063 float
00064 VPMovement::vpGetTimeFinal(){
00065 return tFinal;
00066 }
00067
00068 float
00069 VPMovement::vpGetTime_Err(){
00070 return time_err;
00071 }
00072
00073 int
00074 VPMovement::vpGetQtdMotion(){
00075 return qtdMotion;
00076 }
00077
00078 VPJointMotion *
00079 VPMovement::vpGetTimeline(){
00080 return motionTimeline;
00081 }
00082
00083 void
00084 VPMovement::vpSetDeltaT( float dt ){
00085 deltaT = dt;
00086 }
00087
00088 void
00089 VPMovement::vpSetCycle( bool c ){
00090 cycle = c;
00091 }
00092
00093 void
00094 VPMovement::vpSetTime( float t ){
00095 time = t;
00096 }
00097
00098 void
00099 VPMovement::vpSetTimeFinal( float tf ){
00100 tFinal = tf;
00101 }
00102
00103 void
00104 VPMovement::vpSetTime_Err( float te ){
00105 time_err = te;
00106 }
00107
00108 void
00109 VPMovement::vpSetQtdMotion( int qm ){
00110 qtdMotion = qm;
00111 }
00112
00113 void
00114 VPMovement::vpLoadMotionFile( char *fileName ){
00115
00116
00117
00118 try
00119 {
00120 XMLPlatformUtils::Initialize();
00121 }
00122
00123 catch (const XMLException& toCatch)
00124 {
00125 printf( "Error during initialization! : %s\n", toCatch.getMessage() );
00126 return;
00127 }
00128
00129 DOMParser::ValSchemes valScheme = DOMParser::Val_Auto;
00130 bool doNamespaces = false;
00131
00132
00133 DOMParser parser;
00134 parser.setValidationScheme(valScheme);
00135 parser.setDoNamespaces(doNamespaces);
00136
00137
00138 DOMCountErrorHandler errorHandler;
00139 parser.setErrorHandler(&errorHandler);
00140
00141
00142
00143
00144
00145 unsigned long duration;
00146 try
00147 {
00148 const unsigned long startMillis = XMLPlatformUtils::getCurrentMillis();
00149 parser.parse(fileName);
00150 const unsigned long endMillis = XMLPlatformUtils::getCurrentMillis();
00151 duration = endMillis - startMillis;
00152 }
00153
00154 catch (const XMLException& toCatch)
00155 {
00156 cerr << "\nError during parsing: '" << fileName << "'\n"
00157 << "Exception message is: \n"
00158 << StrX(toCatch.getMessage()) << "\n" << endl;
00159 return;
00160 }
00161 catch (const DOM_DOMException& toCatch)
00162 {
00163 cerr << "\nError during parsing: '" << fileName << "'\n"
00164 << "Exception message is: \n"
00165 << toCatch.msg.transcode() << "\n" << endl;
00166 XMLPlatformUtils::Terminate();
00167 return;
00168 }
00169 catch (...)
00170 {
00171 cerr << "\nUnexpected exception during parsing: '" << fileName << "'\n";
00172 XMLPlatformUtils::Terminate();
00173 return;
00174 }
00175
00176
00177
00178
00179
00180 if (errorHandler.getSawErrors())
00181 {
00182 cout << "\nErrors occured, no output available\n" << endl;
00183 }
00184 else
00185 {
00186 DOM_Document doc = parser.getDocument();
00187 unsigned int elementCount = doc.getElementsByTagName("*").getLength();
00188
00189 DOM_Element root = doc.getDocumentElement();
00190
00192 DOM_NodeList move = root.getElementsByTagName("movement");
00193
00194 for( int i = 0; i < 1; i++ ){
00195 DOM_NamedNodeMap attrmove = move.item(i).getAttributes();
00196 bodyName = attrmove.getNamedItem( DOMString("body_name") ).getNodeValue().transcode();
00197 if( attrmove.getNamedItem( DOMString("cycle") ).getNodeValue().equals( DOMString( "on" ) ) ){
00198 cycle = true;
00199 } else {
00200 cycle = false;
00201 }
00202 deltaT = atof( attrmove.getNamedItem( DOMString("delta_t") ).getNodeValue().transcode() );
00203 time_err = atof( attrmove.getNamedItem( DOMString("time_err") ).getNodeValue().transcode() );
00204 tFinal = 0.0;
00205
00206 DOM_NodeList motion = move.item(i).getChildNodes();
00207 int numMotion = 0;
00208 for( int j = 0; j < motion.getLength(); j++ ){
00209 if( motion.item(j).getNodeName().equals( "motion" ) ) numMotion++;
00210 }
00211 motionTimeline = new VPJointMotion [ numMotion ];
00212 qtdMotion = 0;
00213 for( int j = 0; j < motion.getLength(); j++ ){
00214 if( motion.item(j).getNodeName().equals( "motion" ) ){
00215 DOM_NamedNodeMap attrmotion = motion.item(j).getAttributes();
00216 string jointName = attrmotion.getNamedItem( DOMString("joint_name") ).getNodeValue().transcode();
00217 float t0 = atof( attrmotion.getNamedItem( DOMString("t0") ).getNodeValue().transcode() );
00218 float tf = atof( attrmotion.getNamedItem( DOMString("tf") ).getNodeValue().transcode() );
00219 float parameter = atof( attrmotion.getNamedItem( DOMString("parameter") ).getNodeValue().transcode() );
00220 int motionType = FLEX;
00221 if( attrmotion.getNamedItem( DOMString("type") ).getNodeValue().equals( DOMString( "ADDUCT" ) ) ){
00222 motionType = ADDUCT;
00223 } else if( attrmotion.getNamedItem( DOMString("type") ).getNodeValue().equals( DOMString( "TWIST" ) ) ){
00224 motionType = TWIST;
00225 }
00226 motionTimeline[qtdMotion++] = *( new VPJointMotion( jointName, motionType, t0, tf, parameter ) );
00227 if( tf > tFinal ) tFinal = tf;
00228 }
00229 }
00230 }
00231
00233
00234
00235
00236 cout << fileName << ": " << duration << " ms ("
00237 << elementCount << " elems)." << endl;
00238 }
00239
00240
00241
00242
00243 return;
00244 }