00001 #include "SIGEL_RobotIO/SIG_RobotCompiler.h"
00002 #include "SIGEL_RobotIO/SIG_RobotIOExceptions.h"
00003
00004 namespace SIGEL_RobotIO {
00005 SIG_RobotCompiler::SIG_RobotCompiler (SIG_RobotScanner &sc,
00006 SIG_Robot *tg,
00007 QString homepath)
00008 : myScanner (sc),
00009 target (tg),
00010 dirprefix (homepath)
00011 { }
00012
00013 SIG_RobotCompiler::~SIG_RobotCompiler (void)
00014 { }
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 void SIG_RobotCompiler::expect (int symboltype)
00030 {
00031 int st;
00032 QString sym;
00033
00034 myScanner.readSymbol (st, sym);
00035 if (st != symboltype)
00036 throw SIG_SyntaxError (__FILE__, __LINE__,
00037 "Wrong symbol: " +
00038 QString::number (st) + " instead of " +
00039 QString::number (symboltype) + ".",
00040 "(unknown)",
00041 myScanner.currentLine ());
00042 }
00043
00044 QString SIG_RobotCompiler::expectWord (void)
00045 {
00046 int st;
00047 QString sym;
00048
00049 myScanner.readSymbol (st, sym);
00050 if (st != RobotSymbol::word)
00051 throw SIG_SyntaxError (__FILE__, __LINE__,
00052 "Word expected.",
00053 "(unknown)",
00054 myScanner.currentLine ());
00055 return sym;
00056 }
00057
00058 DL_Scalar SIG_RobotCompiler::expectNumber (void)
00059 {
00060 int st;
00061 QString sym;
00062
00063 myScanner.readSymbol (st, sym);
00064 if (st != RobotSymbol::number)
00065 throw SIG_SyntaxError (__FILE__, __LINE__,
00066 "Number expected.",
00067 "(unknown)",
00068 myScanner.currentLine ());
00069 return sym.toDouble ();
00070 }
00071
00072 QString SIG_RobotCompiler::expectString (void)
00073 {
00074 int st;
00075 QString sym;
00076
00077 myScanner.readSymbol (st, sym);
00078 if (st != RobotSymbol::string)
00079 throw SIG_SyntaxError (__FILE__, __LINE__,
00080 "String expected.",
00081 "(unknown)",
00082 myScanner.currentLine ());
00083 return sym;
00084 }
00085
00086 bool SIG_RobotCompiler::compileNextEntity (void)
00087 {
00088 int symType;
00089 QString symbol;
00090
00091 do {
00092 myScanner.readSymbol (symType, symbol);
00093 } while (symType == Symbol::None);
00094
00095 if (symType == Symbol::EOS) {
00096 return false;
00097 } else if (symType != RobotSymbol::word) {
00098 throw SIG_SyntaxError (__FILE__, __LINE__,
00099 "'material','link','joint','drive','sensor','scaleall' expected.",
00100 "(unknown)", myScanner.currentLine ());
00101 } else {
00102 if (symbol == "material")
00103 nextIsMaterial ();
00104 else if (symbol == "link")
00105 nextIsLink ();
00106 else if (symbol == "joint")
00107 nextIsJoint ();
00108 else if (symbol == "glue")
00109 nextIsGlue ();
00110 else if (symbol == "drive")
00111 nextIsDrive ();
00112 else if (symbol == "sensor")
00113 nextIsSensor ();
00114 else if (symbol == "surface")
00115 nextIsSurface ();
00116 else if (symbol == "scaleall")
00117 nextModScaleall ();
00118 else
00119 throw SIG_SyntaxError (__FILE__, __LINE__,
00120 "'material','link','joint','drive','sensor','scaleall' expected.",
00121 "(unknown)", myScanner.currentLine ());
00122 }
00123 return true;
00124 }
00125
00126 void SIG_RobotCompiler::nextIsMaterial (void)
00127 {
00128 int symtype;
00129 QString symstr;
00130 SIG_Material *mater;
00131
00132 QString objectName = expectWord ();
00133 expect (RobotSymbol::openingBrace);
00134
00135
00136 mater = materialFind (objectName);
00137
00138 symstr = expectWord ();
00139 if (symstr != "density")
00140 throw SIG_SyntaxError (__FILE__, __LINE__,
00141 "'density' expected",
00142 "(unknown)",
00143 myScanner.currentLine ());
00144 DL_Scalar dichte = expectNumber ();
00145 expect (RobotSymbol::semicolon);
00146
00147 materialDensity (mater, dichte);
00148
00149 myScanner.peekSymbol (symtype, symstr);
00150 while ((symtype == RobotSymbol::word) &&
00151 (symstr == "friction")) {
00152 myScanner.nextSymbol ();
00153
00154 DL_Scalar konstante = expectNumber ();
00155
00156 symstr = expectWord ();
00157 if (symstr != "on")
00158 throw SIG_SyntaxError
00159 (__FILE__, __LINE__,
00160 "'on' expected", "(unknown)",
00161 myScanner.currentLine ());
00162
00163 QString anderes_material = expectWord ();
00164
00165 expect (RobotSymbol::semicolon);
00166
00167
00168 materialFriction (mater, anderes_material, konstante);
00169 myScanner.peekSymbol (symtype, symstr);
00170 }
00171
00172 if ((symtype == RobotSymbol::word) &&
00173 (symstr == "elasticity")) {
00174 myScanner.nextSymbol ();
00175
00176 DL_Scalar elas = expectNumber ();
00177 expect (RobotSymbol::semicolon);
00178
00179
00180 materialElasticity (mater, elas);
00181 myScanner.peekSymbol (symtype, symstr);
00182 }
00183
00184 if ((symtype == RobotSymbol::word) &&
00185 (symstr == "colour")) {
00186 myScanner.nextSymbol ();
00187
00188 DL_Scalar rot, gruen, blau;
00189
00190 if (expectWord () != "red")
00191 throw SIG_SyntaxError
00192 (__FILE__, __LINE__,
00193 "'red' expected", "(unknown)",
00194 myScanner.currentLine ());
00195 rot = expectNumber ();
00196
00197 if (expectWord () != "green")
00198 throw SIG_SyntaxError
00199 (__FILE__, __LINE__,
00200 "'green' expected", "(unknown)",
00201 myScanner.currentLine ());
00202 gruen = expectNumber ();
00203
00204 if (expectWord () != "blue")
00205 throw SIG_SyntaxError
00206 (__FILE__, __LINE__,
00207 "'blue' expected", "(unknown)",
00208 myScanner.currentLine ());
00209 blau = expectNumber ();
00210
00211 expect (RobotSymbol::semicolon);
00212
00213 materialColour (mater, rot, gruen, blau);
00214
00215 myScanner.peekSymbol (symtype, symstr);
00216 }
00217
00218 expect (RobotSymbol::closingBrace);
00219
00220 materialFinish (mater);
00221 }
00222
00223 void SIG_RobotCompiler::nextIsLink (void)
00224 {
00225 int symtype;
00226 QString symstr;
00227 SIG_Link *thema;
00228
00229 QString objectName = expectWord ();
00230 expect (RobotSymbol::openingBrace);
00231
00232
00233 thema = linkFind (objectName);
00234
00235 symstr = expectWord ();
00236 if (symstr == "torso") {
00237
00238 linkIsTorso (thema);
00239 expect (RobotSymbol::semicolon);
00240 symstr = expectWord ();
00241 }
00242
00243 if (symstr != "geometry")
00244 throw SIG_SyntaxError (__FILE__, __LINE__,
00245 "'geometry' expected",
00246 "(unknown)",
00247 myScanner.currentLine ());
00248 symstr = expectString ();
00249
00250 linkGeometryFile (thema, symstr);
00251 expect (RobotSymbol::semicolon);
00252
00253 symstr = expectWord ();
00254 if (symstr != "material")
00255 throw SIG_SyntaxError (__FILE__, __LINE__,
00256 "'material' expected.",
00257 "(unknown)",
00258 myScanner.currentLine ());
00259 symstr = expectWord ();
00260
00261 linkMaterial (thema, symstr);
00262 expect (RobotSymbol::semicolon);
00263
00264 myScanner.peekSymbol (symtype, symstr);
00265 while ((symtype == RobotSymbol::word) &&
00266 (symstr == "point")) {
00267 myScanner.nextSymbol ();
00268
00269 QString pointname;
00270 DL_Scalar xval, yval, zval;
00271
00272 pointname = expectWord ();
00273 expect (RobotSymbol::equals);
00274 expect (RobotSymbol::openingParen);
00275 xval = expectNumber ();
00276 expect (RobotSymbol::comma);
00277 yval = expectNumber ();
00278 expect (RobotSymbol::comma);
00279 zval = expectNumber ();
00280 expect (RobotSymbol::closingParen);
00281 expect (RobotSymbol::semicolon);
00282
00283
00284 linkPoint (thema, pointname, xval, yval, zval);
00285 myScanner.peekSymbol (symtype, symstr);
00286 }
00287
00288 if ((symtype == RobotSymbol::word) &&
00289 (symstr == "no_collision")) {
00290 myScanner.nextSymbol ();
00291
00292 QString ncl;
00293
00294 do {
00295 ncl = expectWord ();
00296
00297 linkNoCollide (thema, ncl);
00298 myScanner.readSymbol (symtype, symstr);
00299 } while (symtype == RobotSymbol::comma);
00300
00301 if (symtype != RobotSymbol::semicolon)
00302 throw SIG_SyntaxError (__FILE__, __LINE__,
00303 "';' expected.",
00304 "(unknown)",
00305 myScanner.currentLine ());
00306 }
00307
00308 expect (RobotSymbol::closingBrace);
00309
00310 linkFinish (thema);
00311 }
00312
00313 void SIG_RobotCompiler::nextIsJoint (void)
00314 {
00315 QString symstr;
00316 int symtype;
00317
00318 QString jointtype = expectWord ();
00319 if (jointtype == "rotational") {
00320 QString jointname = expectWord ();
00321 expect (RobotSymbol::openingBrace);
00322 SIG_RotationalJoint *rj = rjointFind (jointname);
00323
00324 QString Alink, A1, A2, A3, Blink, B1, B2, B3;
00325 if (expectWord () != "between")
00326 throw SIG_SyntaxError (__FILE__, __LINE__,
00327 "'between' expected.",
00328 "(unknown)", myScanner.currentLine ());
00329 Alink = expectWord ();
00330 expect (RobotSymbol::openingParen);
00331 A1 = expectWord ();
00332 expect (RobotSymbol::comma);
00333 A2 = expectWord ();
00334 expect (RobotSymbol::comma);
00335 A3 = expectWord ();
00336 expect (RobotSymbol::closingParen);
00337 if (expectWord () != "and")
00338 throw SIG_SyntaxError (__FILE__, __LINE__,
00339 "'and' expected.",
00340 "(unknown)", myScanner.currentLine ());
00341 Blink = expectWord ();
00342 expect (RobotSymbol::openingParen);
00343 B1 = expectWord ();
00344 expect (RobotSymbol::comma);
00345 B2 = expectWord ();
00346 expect (RobotSymbol::comma);
00347 B3 = expectWord ();
00348 expect (RobotSymbol::closingParen);
00349 expect (RobotSymbol::semicolon);
00350 rjointLinking (rj, Alink, A1, A2, A3, Blink, B1, B2, B3);
00351
00352 if (expectWord () != "minimal")
00353 throw SIG_SyntaxError (__FILE__, __LINE__,
00354 "'minimal' expected",
00355 "(unknown)", myScanner.currentLine ());
00356 DL_Scalar mn = expectNumber ();
00357 expect (RobotSymbol::semicolon);
00358 if (expectWord () != "maximal")
00359 throw SIG_SyntaxError (__FILE__, __LINE__,
00360 "'maximal' expected",
00361 "(unknown)", myScanner.currentLine ());
00362 DL_Scalar mx = expectNumber ();
00363 expect (RobotSymbol::semicolon);
00364 if (expectWord () != "init")
00365 throw SIG_SyntaxError (__FILE__, __LINE__,
00366 "'init' expected",
00367 "(unknown)", myScanner.currentLine ());
00368 DL_Scalar ii = expectNumber ();
00369 expect (RobotSymbol::semicolon);
00370 rjointExtents (rj, mn, mx, ii);
00371
00372 expect (RobotSymbol::closingBrace);
00373 rjointFinish (rj);
00374 } else if (jointtype == "translational") {
00375 QString jointname = expectWord ();
00376 expect (RobotSymbol::openingBrace);
00377 SIG_TranslationalJoint *tj = tjointFind (jointname);
00378
00379 QString Alink, A1, A2, A3, Blink, B1, B2, B3;
00380 if (expectWord () != "between")
00381 throw SIG_SyntaxError (__FILE__, __LINE__,
00382 "'between' expected.",
00383 "(unknown)", myScanner.currentLine ());
00384 Alink = expectWord ();
00385 expect (RobotSymbol::openingParen);
00386 A1 = expectWord ();
00387 expect (RobotSymbol::comma);
00388 A2 = expectWord ();
00389 expect (RobotSymbol::comma);
00390 A3 = expectWord ();
00391 expect (RobotSymbol::closingParen);
00392 if (expectWord () != "and")
00393 throw SIG_SyntaxError (__FILE__, __LINE__,
00394 "'and' expected.",
00395 "(unknown)", myScanner.currentLine ());
00396 Blink = expectWord ();
00397 expect (RobotSymbol::openingParen);
00398 B1 = expectWord ();
00399 expect (RobotSymbol::comma);
00400 B2 = expectWord ();
00401 expect (RobotSymbol::comma);
00402 B3 = expectWord ();
00403 expect (RobotSymbol::closingParen);
00404 expect (RobotSymbol::semicolon);
00405 tjointLinking (tj, Alink, A1, A2, A3, Blink, B1, B2, B3);
00406
00407 if (expectWord () != "minimal")
00408 throw SIG_SyntaxError (__FILE__, __LINE__,
00409 "'minimal' expected",
00410 "(unknown)", myScanner.currentLine ());
00411 DL_Scalar mn = expectNumber ();
00412 expect (RobotSymbol::semicolon);
00413 if (expectWord () != "maximal")
00414 throw SIG_SyntaxError (__FILE__, __LINE__,
00415 "'maximal' expected",
00416 "(unknown)", myScanner.currentLine ());
00417 DL_Scalar mx = expectNumber ();
00418 expect (RobotSymbol::semicolon);
00419 if (expectWord () != "init")
00420 throw SIG_SyntaxError (__FILE__, __LINE__,
00421 "'init' expected",
00422 "(unknown)", myScanner.currentLine ());
00423 DL_Scalar ii = expectNumber ();
00424 expect (RobotSymbol::semicolon);
00425 tjointExtents (tj, mn, mx, ii);
00426
00427 expect (RobotSymbol::closingBrace);
00428 tjointFinish (tj);
00429 } else if (jointtype == "cylindrical") {
00430 QString jointname = expectWord ();
00431 expect (RobotSymbol::openingBrace);
00432 SIG_CylindricalJoint *cj = cjointFind (jointname);
00433
00434 QString Alink, A1, A2, A3, Blink, B1, B2, B3;
00435 if (expectWord () != "between")
00436 throw SIG_SyntaxError (__FILE__, __LINE__,
00437 "'between' expected.",
00438 "(unknown)", myScanner.currentLine ());
00439 Alink = expectWord ();
00440 expect (RobotSymbol::openingParen);
00441 A1 = expectWord ();
00442 expect (RobotSymbol::comma);
00443 A2 = expectWord ();
00444 expect (RobotSymbol::comma);
00445 A3 = expectWord ();
00446 expect (RobotSymbol::closingParen);
00447 if (expectWord () != "and")
00448 throw SIG_SyntaxError (__FILE__, __LINE__,
00449 "'and' expected.",
00450 "(unknown)", myScanner.currentLine ());
00451 Blink = expectWord ();
00452 expect (RobotSymbol::openingParen);
00453 B1 = expectWord ();
00454 expect (RobotSymbol::comma);
00455 B2 = expectWord ();
00456 expect (RobotSymbol::comma);
00457 B3 = expectWord ();
00458 expect (RobotSymbol::closingParen);
00459 expect (RobotSymbol::semicolon);
00460 cjointLinking (cj, Alink, A1, A2, A3, Blink, B1, B2, B3);
00461
00462 if (expectWord () != "minimal_rot")
00463 throw SIG_SyntaxError (__FILE__, __LINE__,
00464 "'minimal_rot' expected.",
00465 "(unknown)", myScanner.currentLine ());
00466 DL_Scalar mn = expectNumber ();
00467 expect (RobotSymbol::semicolon);
00468 if (expectWord () != "maximal_rot")
00469 throw SIG_SyntaxError (__FILE__, __LINE__,
00470 "'maximal_rot' expected.",
00471 "(unknown)", myScanner.currentLine ());
00472 DL_Scalar mx = expectNumber ();
00473 expect (RobotSymbol::semicolon);
00474 if (expectWord () != "init_rot")
00475 throw SIG_SyntaxError (__FILE__, __LINE__,
00476 "'init_rot' expected.",
00477 "(unknown)", myScanner.currentLine ());
00478 DL_Scalar ii = expectNumber ();
00479 expect (RobotSymbol::semicolon);
00480 cjointRotExtents (cj, mn, mx, ii);
00481
00482 if (expectWord () != "minimal_trans")
00483 throw SIG_SyntaxError (__FILE__, __LINE__,
00484 "'minimal_trans' expected.",
00485 "(unknown)", myScanner.currentLine ());
00486 mn = expectNumber ();
00487 expect (RobotSymbol::semicolon);
00488 if (expectWord () != "maximal_trans")
00489 throw SIG_SyntaxError (__FILE__, __LINE__,
00490 "'maximal_trans' expected.",
00491 "(unknown)", myScanner.currentLine ());
00492 mx = expectNumber ();
00493 expect (RobotSymbol::semicolon);
00494 if (expectWord () != "init_trans")
00495 throw SIG_SyntaxError (__FILE__, __LINE__,
00496 "'init_trans' expected.",
00497 "(unknown)", myScanner.currentLine ());
00498 ii = expectNumber ();
00499 expect (RobotSymbol::semicolon);
00500 cjointTraExtents (cj, mn, mx, ii);
00501
00502 expect (RobotSymbol::closingBrace);
00503 cjointFinish (cj);
00504 } else
00505 throw SIG_SyntaxError (__FILE__, __LINE__,
00506 "Unknown joint type '" + jointtype + "'",
00507 "(unknown)", myScanner.currentLine ());
00508 }
00509
00510 void SIG_RobotCompiler::nextIsGlue (void)
00511 {
00512 QString name;
00513 SIG_GlueJoint *glue;
00514 QString Alink, A1, A2, A3, Blink, B1, B2, B3;
00515
00516 name = expectWord ();
00517
00518 expect (RobotSymbol::openingBrace);
00519 glue = glueFind (name);
00520 if (expectWord () != "between")
00521 throw SIG_SyntaxError (__FILE__, __LINE__,
00522 "'between' expected", "(unknown)",
00523 myScanner.currentLine ());
00524 Alink = expectWord ();
00525 expect (RobotSymbol::openingParen);
00526 A1 = expectWord ();
00527 expect (RobotSymbol::comma);
00528 A2 = expectWord ();
00529 expect (RobotSymbol::comma);
00530 A3 = expectWord ();
00531 expect (RobotSymbol::closingParen);
00532 if (expectWord () != "and")
00533 throw SIG_SyntaxError (__FILE__, __LINE__,
00534 "'and' expected.",
00535 "(unknown)", myScanner.currentLine ());
00536 Blink = expectWord ();
00537 expect (RobotSymbol::openingParen);
00538 B1 = expectWord ();
00539 expect (RobotSymbol::comma);
00540 B2 = expectWord ();
00541 expect (RobotSymbol::comma);
00542 B3 = expectWord ();
00543 expect (RobotSymbol::closingParen);
00544 expect (RobotSymbol::semicolon);
00545 glueLinking (glue, Alink, A1, A2, A3, Blink, B1, B2, B3);
00546
00547 expect (RobotSymbol::closingBrace);
00548 glueFinish (glue);
00549 }
00550
00551 void SIG_RobotCompiler::nextIsDrive (void)
00552 {
00553 QString name, m;
00554 SIG_Drive *thedrive;
00555
00556 name = expectWord ();
00557 thedrive = driveFind (name);
00558
00559 expect (RobotSymbol::openingBrace);
00560 m = expectWord ();
00561 if ((m != "force") &&
00562 (m != "relative") &&
00563 (m != "absolute"))
00564 throw SIG_SyntaxError (__FILE__, __LINE__,
00565 "Drive mode ('force', 'relative', 'absolute') expected.",
00566 "(unknown)", myScanner.currentLine ());
00567 driveMode (thedrive, m);
00568 driveJoint (thedrive, expectWord ());
00569 expect (RobotSymbol::semicolon);
00570 if (expectWord () != "minimalforce")
00571 throw SIG_SyntaxError (__FILE__, __LINE__,
00572 "'minimalforce' expected",
00573 "(unknown)", myScanner.currentLine ());
00574 DL_Scalar mini = expectNumber ();
00575 expect (RobotSymbol::semicolon);
00576 if (expectWord () != "maximalforce")
00577 throw SIG_SyntaxError (__FILE__, __LINE__,
00578 "'maximalforce' expected",
00579 "(unknown)", myScanner.currentLine ());
00580 driveMinMaxForce (thedrive, mini, expectNumber ());
00581 expect (RobotSymbol::semicolon);
00582 expect (RobotSymbol::closingBrace);
00583 }
00584
00585 void SIG_RobotCompiler::nextIsSensor (void)
00586 {
00587 QString name, styp;
00588
00589 name = expectWord ();
00590 expect (RobotSymbol::openingBrace);
00591 styp = expectWord ();
00592 if (styp == "joint") {
00593 SIG_JointSensor *js = jointSensorFind (name);
00594 jointSensorJoint (js, expectWord ());
00595 expect (RobotSymbol::semicolon);
00596 jointSensorFinish (js);
00597 } else
00598 throw SIG_SyntaxError (__FILE__, __LINE__,
00599 "Invalid sensor type '"+styp+"'",
00600 "(unknown)", myScanner.currentLine ());
00601 expect (RobotSymbol::closingBrace);
00602 }
00603
00604 void SIG_RobotCompiler::nextIsSurface (void)
00605 {
00606
00607 QString symstr;
00608 int symtype;
00609
00610 SIG_Geometry *geom;
00611 SIG_Polygon *poly;
00612
00613 if (expectWord () != "for")
00614 throw SIG_SyntaxError (__FILE__, __LINE__,
00615 "'for' expected",
00616 "(unknown)",
00617 myScanner.currentLine ());
00618 QString filename = expectString ();
00619
00620 geom = surfaceFind (filename);
00621
00622 myScanner.readSymbol (symtype, symstr);
00623 while (symtype == RobotSymbol::openingParen) {
00624 poly = surfaceNewPoly (geom);
00625
00626 do {
00627 DL_Scalar x = expectNumber ();
00628 expect (RobotSymbol::comma);
00629 DL_Scalar y = expectNumber ();
00630 expect (RobotSymbol::comma);
00631 DL_Scalar z = expectNumber ();
00632
00633 surfaceNewPoint (poly, x, y, z);
00634
00635 myScanner.readSymbol (symtype, symstr);
00636 } while (symtype == RobotSymbol::slash);
00637 if (symtype != RobotSymbol::closingParen)
00638 throw SIG_SyntaxError (__FILE__, __LINE__,
00639 "')' expected",
00640 "(unknown)",
00641 myScanner.currentLine ());
00642 myScanner.readSymbol (symtype, symstr);
00643 }
00644 if (symtype != RobotSymbol::semicolon)
00645 throw SIG_SyntaxError (__FILE__, __LINE__,
00646 "';' expected",
00647 "(unknown)",
00648 myScanner.currentLine ());
00649
00650 surfaceFinish (geom, filename);
00651 }
00652
00653 void SIG_RobotCompiler::nextModScaleall (void)
00654 {
00655 DL_Scalar sf = expectNumber ();
00656 expect (RobotSymbol::semicolon);
00657
00658 modifierScaleall (sf);
00659 }
00660
00661 void SIG_RobotCompiler::runPass (void)
00662 {
00663 while (compileNextEntity ());
00664 }
00665 }