Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_Robot.cpp

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                         // Hier scheint etwas zu fehlen, aber ich habe keinen
00094                         // Schimmer mehr, was.
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 }

Generated at Mon Sep 3 01:32:29 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000