00001 #include "SIGEL_RobotIO/SIG_RobotCompilerStructure.h"
00002
00003 namespace SIGEL_RobotIO {
00004 bool checkLinkHasPoint (SIG_Link *l, QString pn, SIG_Joint *j)
00005 {
00006 if (!l->hasPoint (pn))
00007 throw SIG_SemanticError (__FILE__, __LINE__,
00008 QString ("There is no point ") +
00009 pn + " in link " + l->getName () +
00010 ", as requested in joint " +
00011 j->getName ());
00012 }
00013 SIG_RobotCompilerStructure::SIG_RobotCompilerStructure
00014 (SIG_RobotScanner &sc, SIG_Robot *tg, QString homepath)
00015 : SIG_RobotCompiler (sc, tg, homepath)
00016 { }
00017
00018 SIG_RobotCompilerStructure::~SIG_RobotCompilerStructure (void)
00019 { }
00020
00021 SIG_Material *SIG_RobotCompilerStructure::materialFind (QString name)
00022 {
00023 return target->lookupMaterial (name);
00024 }
00025
00026 void SIG_RobotCompilerStructure::materialDensity (SIG_Material *material,
00027 DL_Scalar density)
00028 {
00029
00030 }
00031
00032 void SIG_RobotCompilerStructure::materialFriction (SIG_Material *material,
00033 QString otherSide,
00034 DL_Scalar frictionconst)
00035 {
00036 SIG_Material *oppo = target->lookupMaterial (otherSide);
00037 if (!oppo)
00038 throw SIG_SemanticError
00039 (__FILE__, __LINE__,
00040 "There is no material \"" + otherSide +
00041 "\" to which a friction constant is to be set in \"" +
00042 material->getName () + "\"");
00043 material->setFrictionValue (oppo, frictionconst);
00044 }
00045
00046 void SIG_RobotCompilerStructure::materialElasticity (SIG_Material *material,
00047 DL_Scalar elasconst)
00048 {
00049
00050 }
00051
00052 void SIG_RobotCompilerStructure::materialColour (SIG_Material *material,
00053 DL_Scalar red,
00054 DL_Scalar green,
00055 DL_Scalar blue)
00056 {
00057
00058 }
00059
00060 void SIG_RobotCompilerStructure::materialFinish (SIG_Material *material)
00061 {
00062
00063 }
00064
00065 SIG_Link *SIG_RobotCompilerStructure::linkFind (QString name)
00066 {
00067 return target->lookupLink (name);
00068 }
00069
00070 void SIG_RobotCompilerStructure::linkIsTorso (SIG_Link *link)
00071 {
00072
00073 }
00074
00075 void SIG_RobotCompilerStructure::linkGeometryFile (SIG_Link *link,
00076 QString geometryfile)
00077 {
00078
00079 }
00080
00081 void SIG_RobotCompilerStructure::linkMaterial (SIG_Link *link,
00082 QString material)
00083 {
00084 SIG_Material *m = target->lookupMaterial (material);
00085 if (!m)
00086 throw SIG_SemanticError
00087 (__FILE__, __LINE__,
00088 "There is no material \"" + material +
00089 "\" for link \"" + link->getName () + "\"");
00090 link->setMaterial (m);
00091 }
00092
00093 void SIG_RobotCompilerStructure::linkPoint (SIG_Link *link,
00094 QString pointname,
00095 DL_Scalar x,
00096 DL_Scalar y,
00097 DL_Scalar z)
00098 {
00099
00100 }
00101
00102 void SIG_RobotCompilerStructure::linkNoCollide (SIG_Link *link,
00103 QString nocollide)
00104 {
00105 SIG_Link *nc = target->lookupLink (nocollide);
00106 if (!nc)
00107 throw SIG_SemanticError
00108 (__FILE__, __LINE__,
00109 "There is no link \"" + nocollide +
00110 "\" to be marked as not colliding with \"" +
00111 link->getName () + "\"");
00112 link->addNoCollide (nc);
00113 }
00114
00115 void SIG_RobotCompilerStructure::linkFinish (SIG_Link *link)
00116 {
00117
00118 }
00119
00120 SIG_RotationalJoint *SIG_RobotCompilerStructure::rjointFind (QString name)
00121 {
00122 SIG_Joint *gj = target->lookupJoint (name);
00123 if (gj->getJointType () != SIG_Joint::tRotationalJoint)
00124 throw SIG_SemanticError (__FILE__, __LINE__,
00125 "internal: " + name + " is not tRotationalJoint");
00126 SIG_RotationalJoint *j = static_cast<SIG_RotationalJoint *> (gj);
00127 return j;
00128 }
00129
00130 void SIG_RobotCompilerStructure::rjointLinking (SIG_RotationalJoint *rj,
00131 QString Alink, QString AB, QString AD, QString AH,
00132 QString Blink, QString BB, QString BD, QString BH)
00133 {
00134 SIG_Link *leftlink, *rightlink;
00135 DL_vector lb, ld, lh, rb, rd, rh;
00136
00137 leftlink = target->lookupLink (Alink);
00138 if (!leftlink)
00139 throw SIG_SemanticError (__FILE__, __LINE__,
00140 "No link \"" + Alink + "\" available.");
00141 rightlink = target->lookupLink (Blink);
00142 if (!rightlink)
00143 throw SIG_SemanticError (__FILE__, __LINE__,
00144 "No link \"" + Blink + "\" available.");
00145 rj->setLeftLink (leftlink);
00146 rj->setRightLink (rightlink);
00147 checkLinkHasPoint (leftlink, AB, rj);
00148 checkLinkHasPoint (leftlink, AD, rj);
00149 checkLinkHasPoint (leftlink, AH, rj);
00150 checkLinkHasPoint (rightlink, BB, rj);
00151 checkLinkHasPoint (rightlink, BD, rj);
00152 checkLinkHasPoint (rightlink, BH, rj);
00153 rj->setLeftPoints (leftlink->getPoint (AB),
00154 leftlink->getPoint (AD),
00155 leftlink->getPoint (AH));
00156 rj->setRightPoints (rightlink->getPoint (BB),
00157 rightlink->getPoint (BD),
00158 rightlink->getPoint (BH));
00159 }
00160
00161 void SIG_RobotCompilerStructure::rjointExtents (SIG_RotationalJoint *rj,
00162 DL_Scalar mn, DL_Scalar mx, DL_Scalar ii)
00163 {
00164
00165 }
00166
00167 void SIG_RobotCompilerStructure::rjointFinish (SIG_RotationalJoint *rj)
00168 {
00169
00170 }
00171
00172 SIG_TranslationalJoint *SIG_RobotCompilerStructure::tjointFind (QString name)
00173 {
00174 SIG_Joint *gj = target->lookupJoint (name);
00175 if (gj->getJointType () != SIG_Joint::tTranslationalJoint)
00176 throw SIG_SemanticError (__FILE__, __LINE__,
00177 "internal: " + name + " is not tTranslationalJoint");
00178 SIG_TranslationalJoint *j = static_cast<SIG_TranslationalJoint *> (gj);
00179 return j;
00180 }
00181
00182 void SIG_RobotCompilerStructure::tjointLinking (SIG_TranslationalJoint *tj,
00183 QString Alink, QString AB, QString AD, QString AF,
00184 QString Blink, QString BB, QString BD, QString BF)
00185 {
00186 SIG_Link *leftlink, *rightlink;
00187 DL_vector lb, ld, lh, rb, rd, rh;
00188
00189 leftlink = target->lookupLink (Alink);
00190 if (!leftlink)
00191 throw SIG_SemanticError (__FILE__, __LINE__,
00192 "No link \"" + Alink + "\" available.");
00193 rightlink = target->lookupLink (Blink);
00194 if (!rightlink)
00195 throw SIG_SemanticError (__FILE__, __LINE__,
00196 "No link \"" + Blink + "\" available.");
00197 tj->setLeftLink (leftlink);
00198 tj->setRightLink (rightlink);
00199 checkLinkHasPoint (leftlink, AB, tj);
00200 checkLinkHasPoint (leftlink, AD, tj);
00201 checkLinkHasPoint (leftlink, AF, tj);
00202 checkLinkHasPoint (rightlink, BB, tj);
00203 checkLinkHasPoint (rightlink, BD, tj);
00204 checkLinkHasPoint (rightlink, BF, tj);
00205 tj->setLeftPoints (leftlink->getPoint (AB),
00206 leftlink->getPoint (AD),
00207 leftlink->getPoint (AF));
00208 tj->setRightPoints (rightlink->getPoint (BB),
00209 rightlink->getPoint (BD),
00210 rightlink->getPoint (BF));
00211 }
00212
00213 void SIG_RobotCompilerStructure::tjointExtents (SIG_TranslationalJoint *tj,
00214 DL_Scalar mn, DL_Scalar mx, DL_Scalar ii)
00215 {
00216
00217 }
00218
00219 void SIG_RobotCompilerStructure::tjointFinish (SIG_TranslationalJoint *tj)
00220 {
00221
00222
00223 }
00224
00225 SIG_CylindricalJoint *SIG_RobotCompilerStructure::cjointFind (QString name)
00226 {
00227 SIG_Joint *cj = target->lookupJoint (name);
00228 if (cj->getJointType () != SIG_Joint::tCylindricalJoint)
00229 throw SIG_SemanticError (__FILE__, __LINE__,
00230 "internal: " + name + " is not tCylindricalJoint");
00231 SIG_CylindricalJoint *j = static_cast<SIG_CylindricalJoint *> (cj);
00232 return j;
00233 }
00234
00235 void SIG_RobotCompilerStructure::cjointLinking (SIG_CylindricalJoint *cj,
00236 QString Alink, QString AB, QString AD, QString AH,
00237 QString Blink, QString BB, QString BD, QString BH)
00238 {
00239 SIG_Link *leftlink, *rightlink;
00240 leftlink = target->lookupLink (Alink);
00241 if (!leftlink)
00242 throw SIG_SemanticError (__FILE__, __LINE__,
00243 "No link \"" + Alink + "\" available.");
00244 rightlink = target->lookupLink (Blink);
00245 if (!rightlink)
00246 throw SIG_SemanticError (__FILE__, __LINE__,
00247 "No link \"" + Blink + "\" available.");
00248 cj->setLeftLink (leftlink);
00249 cj->setRightLink (rightlink);
00250 checkLinkHasPoint (leftlink, AB, cj);
00251 checkLinkHasPoint (leftlink, AD, cj);
00252 checkLinkHasPoint (leftlink, AH, cj);
00253 checkLinkHasPoint (rightlink, BB, cj);
00254 checkLinkHasPoint (rightlink, BD, cj);
00255 checkLinkHasPoint (rightlink, BH, cj);
00256 cj->setLeftPoints (leftlink->getPoint (AB),
00257 leftlink->getPoint (AD),
00258 leftlink->getPoint (AH));
00259 cj->setRightPoints (rightlink->getPoint (BB),
00260 rightlink->getPoint (BD),
00261 rightlink->getPoint (BH));
00262 }
00263
00264 void SIG_RobotCompilerStructure::cjointRotExtents (SIG_CylindricalJoint *cj,
00265 DL_Scalar mn, DL_Scalar mx, DL_Scalar ii)
00266 {
00267
00268 }
00269
00270 void SIG_RobotCompilerStructure::cjointTraExtents (SIG_CylindricalJoint *cj,
00271 DL_Scalar mn, DL_Scalar mx, DL_Scalar ii)
00272 {
00273
00274 }
00275
00276 void SIG_RobotCompilerStructure::cjointFinish (SIG_CylindricalJoint *cj)
00277 {
00278
00279 }
00280
00281 SIG_GlueJoint *SIG_RobotCompilerStructure::glueFind (QString name)
00282 {
00283 SIG_Joint *gj = target->lookupJoint (name);
00284 if (gj->getJointType () != SIG_Joint::tGlueJoint)
00285 throw SIG_SemanticError (__FILE__, __LINE__,
00286 "internal: " + name + " is not tGlueJoint");
00287 SIG_GlueJoint *j = static_cast<SIG_GlueJoint *> (gj);
00288 return j;
00289 }
00290
00291 void SIG_RobotCompilerStructure::glueLinking (SIG_GlueJoint *glue,
00292 QString Alink, QString AA, QString AB, QString AC,
00293 QString Blink, QString BP, QString BQ, QString BR)
00294 {
00295 SIG_Link *leftlink, *rightlink;
00296 leftlink = target->lookupLink (Alink);
00297 if (!leftlink)
00298 throw SIG_SemanticError (__FILE__, __LINE__,
00299 "No link \"" + Alink + "\" available");
00300 rightlink = target->lookupLink (Blink);
00301 if (!rightlink)
00302 throw SIG_SemanticError (__FILE__, __LINE__,
00303 "No link \"" + Blink + "\" available");
00304 glue->setLeftLink (leftlink);
00305 glue->setRightLink (rightlink);
00306 checkLinkHasPoint (leftlink, AA, glue);
00307 checkLinkHasPoint (leftlink, AB, glue);
00308 checkLinkHasPoint (leftlink, AC, glue);
00309 checkLinkHasPoint (rightlink, BP, glue);
00310 checkLinkHasPoint (rightlink, BQ, glue);
00311 checkLinkHasPoint (rightlink, BR, glue);
00312 glue->setPlaneA (leftlink->getPoint (AA),
00313 leftlink->getPoint (AB),
00314 leftlink->getPoint (AC));
00315 glue->setPlaneB (rightlink->getPoint (BP),
00316 rightlink->getPoint (BQ),
00317 rightlink->getPoint (BR));
00318 }
00319
00320 void SIG_RobotCompilerStructure::glueFinish (SIG_GlueJoint *glue)
00321 {
00322
00323 }
00324
00325 SIG_Drive *SIG_RobotCompilerStructure::driveFind (QString name)
00326 {
00327 return target->lookupDrive (name);
00328
00329 }
00330
00331 void SIG_RobotCompilerStructure::driveMode (SIG_Drive *d, QString mode)
00332 {
00333
00334 }
00335
00336 void SIG_RobotCompilerStructure::driveJoint (SIG_Drive *d, QString jointname)
00337 {
00338 SIG_Joint *j = target->lookupJoint (jointname);
00339 if (!j)
00340 throw SIG_SemanticError (__FILE__, __LINE__,
00341 "No joint '" + jointname + "' available");
00342 d->setJoint (j);
00343 }
00344
00345 void SIG_RobotCompilerStructure::driveMinMaxForce (SIG_Drive *d, DL_Scalar minf, DL_Scalar maxf)
00346 {
00347
00348 }
00349
00350 void SIG_RobotCompilerStructure::driveFinish (SIG_Drive *d)
00351 {
00352
00353
00354 }
00355
00356 SIG_JointSensor *SIG_RobotCompilerStructure::jointSensorFind (QString name)
00357 {
00358 SIG_Sensor *s = target->lookupSensor (name);
00359 if (s->getSensorType () != SIG_Sensor::tJointSensor)
00360 throw SIG_SemanticError (__FILE__, __LINE__,
00361 "internal: " + name + " is not tJointSensor");
00362 return static_cast<SIG_JointSensor *> (s);
00363 }
00364
00365 void SIG_RobotCompilerStructure::jointSensorJoint (SIG_JointSensor *js, QString jointname)
00366 {
00367 SIG_Joint *j = target->lookupJoint (jointname);
00368 if (!j)
00369 throw SIG_SemanticError (__FILE__, __LINE__,
00370 "No joint called '"+jointname+"'");
00371 js->setJoint (j);
00372 }
00373
00374 void SIG_RobotCompilerStructure::jointSensorFinish (SIG_JointSensor *js)
00375 {
00376
00377 }
00378 }