00001 #include "SIGEL_Robot/SIG_Robot.h"
00002 #include "SIGEL_Robot/SIG_RobotExceptions.h"
00003 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00004 #include "SIGEL_Robot/SIG_TranslationalJoint.h"
00005 #include "SIGEL_Robot/SIG_CylindricalJoint.h"
00006 #include "SIGEL_Robot/SIG_GlueJoint.h"
00007 #include "SIGEL_Robot/SIG_JointSensor.h"
00008
00009
00010 namespace SIGEL_Robot {
00011
00012 SIG_Robot::SIG_Robot ()
00013 : language (new SIG_LanguageParameters ()),
00014 rootlink (0),
00015 initialLocation(0, 0, 0)
00016 {
00017 initialOrientation.makeone();
00018 }
00019
00020 SIG_Robot::SIG_Robot (QTextStream & tx)
00021 : language (0),
00022 rootlink (0),
00023 initialLocation(0, 0, 0)
00024 {
00025 initialOrientation.makeone();
00026 readFromFileTransfer (tx);
00027 }
00028
00029 SIG_Robot::SIG_Robot (const SIG_Robot & rob)
00030 : language (0),
00031 rootlink (0),
00032 initialLocation(0, 0, 0)
00033 {
00034 initialOrientation.makeone();
00035 QString s;
00036 QTextStream ts (&s, IO_WriteOnly);
00037 rob.writeToFileTransfer (ts);
00038 QTextStream rs (&s, IO_ReadOnly);
00039 readFromFileTransfer (rs);
00040 }
00041
00042 SIG_Robot::~SIG_Robot (void)
00043 {
00044 clear ();
00045 delete language;
00046 }
00047
00048 void SIG_Robot::clear ()
00049 {
00050 QDictIterator<SIG_Body> itbody (bodies);
00051 QDictIterator<SIG_Material> itmaterial (materials);
00052 QDictIterator<SIG_Link> itlink (links);
00053 QDictIterator<SIG_Joint> itjoint (joints);
00054 QDictIterator<SIG_Drive> itdrive (drives);
00055 QDictIterator<SIG_Sensor> itsensor (sensors);
00056
00057 initialLocation = DL_vector (0, 0, 0);
00058 initialOrientation.makeone ();
00059
00060 delete language;
00061 language = new SIG_LanguageParameters ();
00062 rootlink = 0;
00063
00064 while (itsensor.current ()) { delete itsensor.current (); ++itsensor; }
00065 while (itdrive.current ()) { delete itdrive.current (); ++itdrive; }
00066 while (itjoint.current ()) { delete itjoint.current (); ++itjoint; }
00067 while (itlink.current ()) { delete itlink.current (); ++itlink; }
00068 while (itmaterial.current ()) { delete itmaterial.current (); ++itmaterial; }
00069 while (itbody.current ()) { delete itbody.current (); ++itbody; }
00070
00071 sensors.clear ();
00072 drives.clear ();
00073 joints.clear ();
00074 links.clear ();
00075 materials.clear ();
00076 bodies.clear ();
00077 }
00078
00079 void SIG_Robot::addBody (SIG_Body *b)
00080 {
00081 bodies.insert (b->getName (), b);
00082 }
00083
00084 void SIG_Robot::addMaterial (SIG_Material *m)
00085 {
00086 materials.insert (m->getName (), m);
00087 }
00088
00089 void SIG_Robot::addLink (SIG_Link *l, bool isroot = false)
00090 {
00091 links.insert (l->getName (), l);
00092 if (isroot) {
00093
00094
00095 }
00096 }
00097
00098 void SIG_Robot::addJoint (SIG_Joint *j)
00099 {
00100 joints.insert (j->getName (), j);
00101 }
00102
00103 void SIG_Robot::addDrive (SIG_Drive *d)
00104 {
00105 drives.insert (d->getName (), d);
00106 }
00107
00108 void SIG_Robot::addSensor (SIG_Sensor *s)
00109 {
00110 sensors.insert (s->getName (), s);
00111 }
00112
00113 void SIG_Robot::setLangParam (SIG_LanguageParameters *lp, int delprev = 1)
00114 {
00115 if (delprev)
00116 delete language;
00117 language = lp;
00118 }
00119
00120 SIG_LanguageParameters *SIG_Robot::getLangParam (void) const
00121 {
00122 return language;
00123 }
00124
00125 SIG_Body *SIG_Robot::lookupBody (QString n) const
00126 {
00127 return bodies.find (n);
00128 }
00129
00130 SIG_Material *SIG_Robot::lookupMaterial (QString n) const
00131 {
00132 return materials.find (n);
00133 }
00134
00135 SIG_Link *SIG_Robot::lookupLink (QString n) const
00136 {
00137 return links.find (n);
00138 }
00139
00140 SIG_Joint *SIG_Robot::lookupJoint (QString n) const
00141 {
00142 return joints.find (n);
00143 }
00144
00145 SIG_Drive *SIG_Robot::lookupDrive (QString n) const
00146 {
00147 return drives.find (n);
00148 }
00149
00150 SIG_Sensor *SIG_Robot::lookupSensor (QString n) const
00151 {
00152 return sensors.find (n);
00153 }
00154
00155 QDictIterator<SIG_Body> SIG_Robot::getBodyIter (void) const
00156 {
00157 return QDictIterator<SIG_Body> (bodies);
00158 }
00159
00160 QDictIterator<SIG_Material> SIG_Robot::getMaterialIter (void) const
00161 {
00162 return QDictIterator<SIG_Material> (materials);
00163 }
00164
00165 QDictIterator<SIG_Link> SIG_Robot::getLinkIter (void) const
00166 {
00167 return QDictIterator<SIG_Link> (links);
00168 }
00169
00170 QDictIterator<SIG_Joint> SIG_Robot::getJointIter (void) const
00171 {
00172 return QDictIterator<SIG_Joint> (joints);
00173 }
00174
00175 QDictIterator<SIG_Drive> SIG_Robot::getDriveIter (void) const
00176 {
00177 return QDictIterator<SIG_Drive> (drives);
00178 }
00179
00180 QDictIterator<SIG_Sensor> SIG_Robot::getSensorIter (void) const
00181 {
00182 return QDictIterator<SIG_Sensor> (sensors);
00183 }
00184
00185 SIG_Link const *SIG_Robot::getRootLink (void) const
00186 {
00187 return rootlink;
00188 }
00189
00190 int SIG_Robot::getNrOfPoints (void) const
00191 {
00192 int summa = 0;
00193 QDictIterator<SIG_Link> lit (links);
00194 SIG_Link *l;
00195 while (l = lit.current ()) {
00196 summa += l->getNrOfPoints ();
00197 ++lit;
00198 }
00199 return summa;
00200 }
00201
00202 void SIG_Robot::setRootLink (SIG_Link *l)
00203 {
00204 rootlink = l;
00205 }
00206
00207 void SIG_Robot::initiate (void)
00208 {
00209 if (rootlink) {
00210 DL_vector rl (0, 0, 0);
00211 DL_matrix ro;
00212 ro.makeone ();
00213
00214 rootlink->setInitialLocation (rl, ro);
00215 }
00216 }
00217
00218 void SIG_Robot::loadGeometries (void)
00219 {
00220 QDictIterator<SIG_Body> iter = getBodyIter ();
00221 while (iter.current ()) {
00222 iter.current()->load ();
00223 ++iter;
00224 }
00225 }
00226
00227 void SIG_Robot::instantiateGeometries (void)
00228 {
00229 QDictIterator<SIG_Link> iter = getLinkIter ();
00230 while (iter.current ()) {
00231 iter.current ()->instantiateGeometry ();
00232 ++iter;
00233 }
00234 }
00235
00236 void SIG_Robot::prepareDynaMo (void)
00237 {
00238 instantiateGeometries ();
00239
00240 QDictIterator<SIG_Link> iter (links);
00241 while (iter.current ()) {
00242 iter.current ()->transformToDynaMo ();
00243 ++iter;
00244 }
00245
00246 initiate();
00247 }
00248
00249 void SIG_Robot::prepareDynaMechs (void)
00250 {
00251 instantiateGeometries ();
00252
00253 rootlink->transformToDynaMechs( 0 );
00254
00255 initiate();
00256
00257 QList< SIG_Joint > rootJoints = rootlink->getJoints();
00258
00259 SIG_Joint *actJoint = rootJoints.first();
00260
00261 while (actJoint)
00262 {
00263 actJoint->calculateMDH( rootlink );
00264
00265 actJoint = rootJoints.next();
00266 };
00267 }
00268
00269 void SIG_Robot::writeToFileTransfer (QTextStream & tx) const
00270 {
00271 QDictIterator<SIG_Body> itbody (bodies);
00272 QDictIterator<SIG_Material> itmaterial (materials);
00273 QDictIterator<SIG_Link> itlink (links);
00274 QDictIterator<SIG_Joint> itjoint (joints);
00275 QDictIterator<SIG_Drive> itdrive (drives);
00276 QDictIterator<SIG_Sensor> itsensor (sensors);
00277
00278 tx << "StreamedRobot\n";
00279
00280 vectorToStream (tx, initialLocation);
00281 matrixToStream (tx, initialOrientation);
00282
00283 while (itbody.current ()) {
00284 itbody.current ()->writeToFileTransfer (tx);
00285 ++itbody;
00286 }
00287
00288 while (itmaterial.current ()) {
00289 itmaterial.current ()->writeToFileTransfer (tx);
00290 ++itmaterial;
00291 }
00292
00293 while (itlink.current ()) {
00294 itlink.current ()->writeToFileTransfer (tx);
00295 ++itlink;
00296 }
00297
00298 while (itjoint.current ()) {
00299 itjoint.current ()->writeToFileTransfer (tx);
00300 ++itjoint;
00301 }
00302
00303 while (itdrive.current ()) {
00304 itdrive.current ()->writeToFileTransfer (tx);
00305 ++itdrive;
00306 }
00307
00308 while (itsensor.current ()) {
00309 itsensor.current ()->writeToFileTransfer (tx);
00310 ++itsensor;
00311 }
00312
00313 tx << "RobotComplete\n";
00314
00315 if (rootlink)
00316 tx << rootlink->getName () << '\n';
00317 else
00318 tx << "-\n";
00319
00320 if (language)
00321 language->writeToFileTransfer (tx);
00322 else
00323 tx << "-\n";
00324 }
00325
00326 void SIG_Robot::readFromFileTransfer (QTextStream & tx)
00327 {
00328 QString objtype, tmpstr;
00329
00330 clear ();
00331
00332 tx >> tmpstr;
00333 if (tmpstr != "StreamedRobot")
00334 throw SIG_UnstreamingError (__FILE__, __LINE__,
00335 "'StreamedRobot' should be first word");
00336 initialLocation = streamToVector (tx);
00337 initialOrientation = streamToMatrix (tx);
00338 tx >> objtype;
00339 while (objtype != "RobotComplete") {
00340 if (objtype == "Body")
00341 addBody (new SIG_Body (this, tx));
00342 else if (objtype == "Material")
00343 addMaterial (new SIG_Material (this, tx));
00344 else if (objtype == "Link")
00345 addLink (new SIG_Link (this, tx));
00346 else if (objtype == "TranslationalJoint")
00347 addJoint (new SIG_TranslationalJoint (this, tx));
00348 else if (objtype == "RotationalJoint")
00349 addJoint (new SIG_RotationalJoint (this, tx));
00350 else if (objtype == "CylindricalJoint")
00351 addJoint (new SIG_CylindricalJoint (this, tx));
00352 else if (objtype == "GlueJoint")
00353 addJoint (new SIG_GlueJoint (this, tx));
00354 else if (objtype == "Drive")
00355 addDrive (new SIG_Drive (this, tx));
00356 else if (objtype == "JointSensor")
00357 addSensor (new SIG_JointSensor (this, tx));
00358
00359 tx >> objtype;
00360 }
00361
00362 tx >> tmpstr;
00363 if (tmpstr == "-")
00364 rootlink = 0;
00365 else
00366 rootlink = links.find (tmpstr);
00367
00368 tx >> tmpstr;
00369 if (tmpstr == "LanguageParameters") {
00370 delete language;
00371 language = new SIG_LanguageParameters (tx);
00372 } else
00373 language = 0;
00374 }
00375
00376 void SIG_Robot::vectorToStream (QTextStream & tx, DL_vector vec)
00377 {
00378 tx << vec.get (0) << ' '
00379 << vec.get (1) << ' '
00380 << vec.get (2) << ' ';
00381 }
00382
00383 DL_vector SIG_Robot::streamToVector (QTextStream & tx)
00384 {
00385 DL_Scalar x, y, z;
00386 tx >> x >> y >> z;
00387 return DL_vector (x, y, z);
00388 }
00389
00390 void SIG_Robot::matrixToStream (QTextStream & tx, DL_matrix mat)
00391 {
00392 tx << mat.get (0,0) << ' ' << mat.get (0,1) << ' ' << mat.get (0,2) << ' '
00393 << mat.get (1,0) << ' ' << mat.get (1,1) << ' ' << mat.get (1,2) << ' '
00394 << mat.get (2,0) << ' ' << mat.get (2,1) << ' ' << mat.get (2,2) << ' ';
00395 }
00396
00397 DL_matrix SIG_Robot::streamToMatrix (QTextStream & tx)
00398 {
00399 DL_matrix mat;
00400 for (int i = 0; i < 3; i++) {
00401 for (int j = 0; j < 3; j++) {
00402 DL_Scalar v;
00403 tx >> v;
00404 mat.set (i, j, v);
00405 }
00406 }
00407 return mat;
00408 }
00409 }