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

SIG_DynaMechsSimulationData.cpp

00001 #include "SIGEL_Simulation/SIG_DynaMechsSimulationData.h"
00002 
00003 #include "SIGEL_Robot/SIG_Drive.h"
00004 #include "SIGEL_Robot/SIG_TranslationalJoint.h"
00005 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00006 #include "SIGEL_Robot/SIG_JointSensor.h"
00007 #include "SIGEL_Tools/SIG_TypeConverter.h"
00008 #include "SIGEL_Tools/SIG_IO.h"
00009 
00010 #include <dmIntegEuler.hpp>
00011 #include <dmIntegRK4.hpp>
00012 #include <dmIntegRK45.hpp>
00013 #include <dmMobileBaseLink.hpp>
00014 #include <dmMDHLink.hpp>
00015 #include <dmPrismaticLink.hpp>
00016 #include <dmRevoluteLink.hpp>
00017 
00018 #include <cstdlib>
00019 #include <cmath>
00020 #include <cfloat>
00021 
00022 using namespace SIGEL_Tools;
00023 
00024 SIGEL_Simulation::SIG_DynaMechsSimulationData::SIG_DynaMechsSimulationData( SIGEL_Robot::SIG_Robot const& robot,
00025                                                           SIGEL_Environment::SIG_Environment const& environment,
00026                                                           SIGEL_Simulation::SIG_SimulationParameters const& simulationParameter)
00027   : SIG_SimulationData( robot, environment, simulationParameter ),
00028     dynaMechsLinks( robot.getLinkIter().count() ),
00029     jointIndices( robot.getJointIter().count() ),
00030     drives( robot.getDriveIter().count() ),
00031     driveForcesTimeAccounts( robot.getDriveIter().count() ),
00032     sensors( robot.getSensorIter().count() ),
00033     pi( std::atan( 1 ) * 4 )
00034 {
00035   for (int i=0; i<dynaMechsLinks.size(); i++)
00036     dynaMechsLinks.insert( i, 0 );
00037 
00038   dynaMechsLinks.setAutoDelete( true );
00039 
00040   jointIndices.fill( 0 );
00041 
00042   driveForcesTimeAccounts.fill( 0 );
00043 
00044   for (int i=0; i<drives.size(); i++)
00045     drives.insert( i, 0 );
00046 
00047   for (int i=0; i<sensors.size(); i++)
00048     sensors.insert( i, 0 );
00049 
00050   initializeEnvironment();
00051 
00052   switch (simulationParameter.getDynaMechsIntegrator())
00053     {
00054     case SIG_SimulationParameters::Euler:
00055       dynaMechsIntegrator = new dmIntegEuler();
00056       break;
00057     case SIG_SimulationParameters::RungeKutta4:
00058       dynaMechsIntegrator = new dmIntegRK4();
00059       break;
00060     case SIG_SimulationParameters::RungeKutta45:
00061       dynaMechsIntegrator = new dmIntegRK45();
00062       break;
00063     };
00064 
00065   initializeArticulation();
00066 
00067   QDictIterator< SIGEL_Robot::SIG_Drive > driveIt = robot.getDriveIter();
00068 
00069   driveIt.toFirst();
00070 
00071   while (driveIt.current())
00072     {
00073       SIGEL_Robot::SIG_Drive *actDrive = driveIt.current();
00074 
00075       switch (actDrive->getMode())
00076         {
00077         case SIGEL_Robot::SIG_Drive::tForceMode:
00078           {
00079             int jointNumber = actDrive->getJoint()->getNumber();
00080 
00081             int linkNumber = jointIndices[ jointNumber ];
00082 
00083             if (dynaMechsLinks[ linkNumber ])
00084               {
00085 
00086 #ifdef SIG_DEBUG
00087                 SIGEL_Tools::SIG_IO::cerr << "Inserting drive "
00088                                           << actDrive->getName()
00089                                           << " at joint "
00090                                           << jointNumber
00091                                           << " corresponding to link "
00092                                           << linkNumber
00093                                           << ": "
00094                                           << dynaMechsLinks[ linkNumber ]->link->getName()
00095                                           << ".\n";
00096 #endif
00097 
00098                 drives.insert( actDrive->getNumber(), actDrive );
00099               };
00100           };
00101           break;
00102         };
00103       ++driveIt;
00104     };
00105 
00106   QDictIterator< SIGEL_Robot::SIG_Sensor > sensorIt = robot.getSensorIter();
00107 
00108   sensorIt.toFirst();
00109 
00110   while (sensorIt.current())
00111     {
00112       switch (sensorIt.current()->getSensorType())
00113         {
00114         case SIGEL_Robot::SIG_Sensor::tJointSensor:
00115           {
00116             SIGEL_Robot::SIG_JointSensor *actSensor = static_cast< SIGEL_Robot::SIG_JointSensor* >(sensorIt.current());
00117 
00118             SIGEL_Robot::SIG_Joint const *joint = actSensor->getJoint();
00119 
00120             int linkNumber = jointIndices[ joint->getNumber() ];
00121 
00122             if (dynaMechsLinks[ linkNumber ])
00123               {
00124                 sensors.insert( actSensor->getNumber(), actSensor );
00125 
00126 #ifdef SIG_DEBUG
00127                 SIGEL_Tools::SIG_IO::cerr << "Inserting sensor "
00128                                           << actSensor->getName()
00129                                           << " at joint "
00130                                           << joint->getNumber()
00131                                           << " corresponding to link "
00132                                           << linkNumber
00133                                           << ": "
00134                                           << dynaMechsLinks[ linkNumber ]->link->getName()
00135                                           << ".\n";
00136 #endif
00137               };
00138           };
00139           break;
00140         };
00141 
00142       ++sensorIt;
00143     };
00144 
00145   dynaMechsIntegrator->setSystem( &dynaMechsSystem );
00146 };
00147 
00148 void SIGEL_Simulation::SIG_DynaMechsSimulationData::setNewFrame( bool newValue )
00149 { };
00150 
00151 void SIGEL_Simulation::SIG_DynaMechsSimulationData::simulationProgress()
00152 {
00153   Float stepSize = static_cast< Float >(simulationParameter.getStepSize());
00154 
00155   dynaMechsIntegrator->simulate( stepSize );
00156 
00157   for (int i = 0; i < drives.size(); i++)
00158     {
00159       SIGEL_Robot::SIG_Drive *actDrive = drives[ i ];
00160 
00161       if (actDrive)
00162         {
00163           driveForcesTimeAccounts[ i ] -= simulationParameter.getStepSize();
00164 
00165 #ifdef SIG_DEBUG
00166           SIGEL_Tools::SIG_IO::cerr << "driveForcesTimeAccount[ "
00167                                     << i
00168                                     << " ], drive "
00169                                     << actDrive->getName()
00170                                     << ": "
00171                                     << driveForcesTimeAccounts[ i ]
00172                                     << "\n";
00173 #endif
00174 
00175           if (driveForcesTimeAccounts[ i ] <= 0)
00176             {
00177 #ifdef SIG_DEBUG
00178               SIGEL_Tools::SIG_IO::cerr << "Resetting drive "
00179                                         << actDrive->getName()
00180                                         << ".\n";
00181 #endif
00182               driveForcesTimeAccounts[ i ] = 0;
00183 
00184               int jointNumber = actDrive->getJoint()->getNumber();
00185 
00186               int linkNumber = jointIndices[ jointNumber ];
00187 
00188               SIG_DynaMechsLink *dynaMechsLink = dynaMechsLinks[ linkNumber ];
00189 
00190               double resetForce = 0;
00191 
00192               dynaMechsLink->dynaMechsLink->setJointInput( &resetForce );
00193             };
00194         };
00195     };
00196 
00197   dynaMechsLinks[ robot.getRootLink()->getNumber() ]->forwardKinematics( 0 );
00198 };
00199 
00200 void SIGEL_Simulation::SIG_DynaMechsSimulationData::initializeEnvironment()
00201 {
00202   NEWMAT::ColumnVector gravity( 3 );
00203   gravity = SIG_TypeConverter::toColumnVector( environment.getGravity() );
00204 
00205   gravity = SIG_TypeConverter::sigelToDynaMechs() * gravity;
00206 
00207   CartesianVector dynaMechsGravity;
00208   SIG_TypeConverter::toCartesianVector( gravity, dynaMechsGravity );
00209   dynaMechsEnvironment.setGravity( dynaMechsGravity );
00210 
00211   dynaMechsEnvironment.setGroundPlanarSpringConstant( environment.getGroundPlanarSpringConstant() );
00212   dynaMechsEnvironment.setGroundNormalSpringConstant( environment.getGroundNormalSpringConstant() );
00213   dynaMechsEnvironment.setGroundPlanarDamperConstant( environment.getGroundPlanarDamperConstant() );
00214   dynaMechsEnvironment.setGroundNormalDamperConstant( environment.getGroundNormalDamperConstant() );
00215   dynaMechsEnvironment.setFrictionCoeffs( environment.getFrictionCoeff_u_s(),
00216                                           environment.getFrictionCoeff_u_k() );
00217 
00218   char *sigelRootCString = std::getenv( "SIGEL_ROOT" );
00219 
00220   QString sigelRootString( sigelRootCString );
00221 
00222   QString terrainDataFileName = sigelRootString + "/flatTerrain.ter";
00223 
00224   QCString terrainDataFileNameQCString = terrainDataFileName.utf8();
00225   char const *terrainDataFileNameCString = terrainDataFileNameQCString;
00226 
00227   dynaMechsEnvironment.loadTerrainData( terrainDataFileNameCString );
00228 
00229   dmEnvironment::setEnvironment( &dynaMechsEnvironment );
00230 };
00231 
00232 void SIGEL_Simulation::SIG_DynaMechsSimulationData::initializeArticulation()
00233 {
00234   SIGEL_Robot::SIG_Link const *rootLink = robot.getRootLink();
00235 
00236   dmMobileBaseLink *internalRootLink = new dmMobileBaseLink();
00237 
00238 
00239   SpatialVector velocity = { 0, 0, 0, 0, 0, 0 };
00240 
00241   Float initialState[7];
00242 
00243   NEWMAT::Matrix startRotation =   SIG_TypeConverter::sigelToDynaMechs()
00244                                  * SIG_TypeConverter::toMatrix( robot.initialOrientation );
00245 
00246   rotationMatrixToQuaternion( startRotation,
00247                               initialState[0],
00248                               initialState[1],
00249                               initialState[2],
00250                               initialState[3] );
00251 
00252   normalizeQuat( initialState );
00253 
00254   NEWMAT::ColumnVector startPosition =   SIG_TypeConverter::sigelToDynaMechs()
00255                                        * (   SIG_TypeConverter::toColumnVector( environment.getStartPosition() )
00256                                            + SIG_TypeConverter::toColumnVector( robot.initialLocation ) );
00257 
00258 #ifdef SIG_DEBUG
00259   SIGEL_Tools::SIG_IO::cerr << "Setting initial robot location: ";
00260   for (int i=1; i<=3; i++)
00261     SIGEL_Tools::SIG_IO::cerr << " " << startPosition( i );
00262   SIGEL_Tools::SIG_IO::cerr << "\n";
00263   SIGEL_Tools::SIG_IO::cerr << "Setting initial robot orientation:\n";
00264   for (int i=1; i<=3; i++)
00265     {
00266       for (int j=1; j<=3; j++)
00267         SIGEL_Tools::SIG_IO::cerr << startRotation( i, j ) << " ";
00268       SIGEL_Tools::SIG_IO::cerr << "\n";
00269     };
00270 #endif
00271 
00272   SIG_TypeConverter::toCartesianVector( startPosition, initialState + 4 );
00273 
00274   internalRootLink->setState( initialState, velocity );
00275 
00276   SIG_DynaMechsLink *dynaMechsRootLink = new SIG_DynaMechsLink( 0,
00277                                                                 rootLink,
00278                                                                 internalRootLink,
00279                                                                 0,
00280                                                                 0 );
00281 
00282   dynaMechsLinks.insert( rootLink->getNumber(), dynaMechsRootLink );
00283 
00284   dynaMechsSystem.addLink( internalRootLink, 0 );
00285 
00286   QList< SIGEL_Robot::SIG_Joint > rootJoints = rootLink->getJoints();
00287 
00288   SIGEL_Robot::SIG_Joint *actJoint = rootJoints.first();
00289 
00290   while (actJoint)
00291     {
00292       SIG_DynaMechsLink *newDynaMechsLink = initializeJoint( actJoint, rootLink );
00293 
00294       if (newDynaMechsLink)
00295         dynaMechsRootLink->successors.append( newDynaMechsLink );
00296 
00297       actJoint = rootJoints.next();
00298     };
00299 
00300   dynaMechsRootLink->forwardKinematics( 0 );
00301 };
00302 
00303 SIGEL_Simulation::SIG_DynaMechsLink *SIGEL_Simulation::SIG_DynaMechsSimulationData::initializeJoint( SIGEL_Robot::SIG_Joint *joint,
00304                                                                                                      SIGEL_Robot::SIG_Link const *caller )
00305 {
00306   double a;
00307   double alpha;
00308   double d;
00309   double theta;
00310 
00311   double screwD;
00312   double screwTheta;
00313 
00314   SIGEL_Robot::SIG_Link *predecessor;
00315   joint->getMDH( predecessor,
00316                  a,
00317                  alpha,
00318                  d,
00319                  theta,
00320                  screwD,
00321                  screwTheta );
00322 
00323   if (predecessor!=caller)
00324     return 0;
00325 
00326   SIGEL_Robot::SIG_Link const *link = ( joint->getLeftLink() == caller ) ? joint->getRightLink() : joint->getLeftLink();
00327 
00328   dmMDHLink *internalDynaMechsLink;
00329 
00330   switch (joint->getJointType())
00331     {
00332     case SIGEL_Robot::SIG_Joint::tTranslationalJoint:
00333       internalDynaMechsLink = new dmPrismaticLink();
00334       break;
00335     case SIGEL_Robot::SIG_Joint::tRotationalJoint:
00336       internalDynaMechsLink = new dmRevoluteLink();
00337       break;
00338     default:
00339       SIGEL_Tools::SIG_IO::cerr << "Cannot simulate robot with DynaMechs: Joint type not allowed!\n";
00340       exit( 1 );
00341     };
00342 
00343   double k_spring = simulationParameter.getJointLimitsK_spring();
00344   double b_damper = simulationParameter.getJointLimitsB_damper();
00345 
00346   double minLimit = joint->getMechsMinPos();
00347   double maxLimit = joint->getMechsMaxPos();
00348 
00349   if (minLimit==maxLimit)
00350     {
00351       k_spring = b_damper = 0;
00352       minLimit = - DBL_MAX;
00353       maxLimit = DBL_MAX;
00354     };
00355 
00356   double jointFriction = simulationParameter.getJointFrictionU_c();
00357 
00358   internalDynaMechsLink->setJointFriction( jointFriction );
00359 
00360   internalDynaMechsLink->setMDHParameters( a,
00361                                            alpha,
00362                                            d,
00363                                            theta);
00364 
00365 #ifdef SIG_DEBUG
00366   SIGEL_Tools::SIG_IO::cerr << "MDH Parameters of joint "
00367                             << joint->getName()
00368                             << ": a: "
00369                             << a
00370                             << " alpha: "
00371                             << alpha
00372                             << " d: "
00373                             << d
00374                             << " theta: "
00375                             << theta
00376                             << " screwD: "
00377                             << screwD
00378                             << " screwTheta: "
00379                             << screwTheta
00380                             << "\n"
00381                             << "Joint limits: Min: "
00382                             << minLimit
00383                             << " Max: "
00384                             << maxLimit
00385                             << "\n";
00386 #endif
00387 
00388   internalDynaMechsLink->setJointLimits( minLimit,
00389                                          maxLimit,
00390                                          k_spring,
00391                                          b_damper );
00392 
00393   int dynaMechsLinkIndex = dynaMechsSystem.getNumLinks();
00394 
00395   if ((screwD!=0) || (screwTheta!=0))
00396     dynaMechsLinkIndex++;
00397 
00398   SIG_DynaMechsLink *dynaMechsLink = new SIG_DynaMechsLink( dynaMechsLinkIndex,
00399                                                             link,
00400                                                             internalDynaMechsLink,
00401                                                             screwD,
00402                                                             screwTheta );
00403 
00404   dynaMechsLinks.insert( link->getNumber(), dynaMechsLink );
00405 
00406   jointIndices[ joint->getNumber() ] = link->getNumber();
00407 
00408   int predIndex = predecessor->getNumber();
00409 
00410   dmLink *internalPredecessor = dynaMechsLinks[ predIndex ]->dynaMechsLink;
00411 
00412   dmZScrewTxLink *screwPredecessor = dynaMechsLink->screwLink;
00413 
00414   if (screwPredecessor)
00415     {
00416       dynaMechsSystem.addLink( screwPredecessor, internalPredecessor );
00417       dynaMechsSystem.addLink( internalDynaMechsLink, screwPredecessor );
00418     }
00419   else
00420     dynaMechsSystem.addLink( internalDynaMechsLink, internalPredecessor );
00421 
00422   QList< SIGEL_Robot::SIG_Joint > joints = link->getJoints();
00423 
00424   SIGEL_Robot::SIG_Joint *actJoint = joints.first();
00425 
00426   while (actJoint)
00427     {
00428       SIG_DynaMechsLink *newSuccessor = initializeJoint( actJoint, link );
00429 
00430       if (newSuccessor)
00431         dynaMechsLink->successors.append( newSuccessor );
00432 
00433       actJoint = joints.next();
00434     };
00435 
00436   return dynaMechsLink;
00437 };
00438 
00439 void SIGEL_Simulation::SIG_DynaMechsSimulationData::rotationMatrixToQuaternion( NEWMAT::Matrix rotationMatrix,
00440                                                                                 double &x,
00441                                                                                 double &y,
00442                                                                                 double &z,
00443                                                                                 double &w )
00444 {
00445   rotationMatrix = rotationMatrix.t();
00446 
00447   double tr, s;
00448 
00449   tr = rotationMatrix(1,1) + rotationMatrix(2,2) + rotationMatrix(3,3) + 1;
00450   if (tr > 0.0625)
00451     {
00452       s = std::sqrt(tr);
00453       w = s*0.5;
00454       s = 0.5/s;
00455 
00456       x = (rotationMatrix(2,3) - rotationMatrix(3,2))*s;
00457       y = (rotationMatrix(3,1) - rotationMatrix(1,3))*s;
00458       z = (rotationMatrix(1,2) - rotationMatrix(2,1))*s;
00459       return;
00460     };
00461 
00462   tr = -rotationMatrix(1,1) - rotationMatrix(2,2) + rotationMatrix(3,3) + 1;
00463   if (tr > 0.0625)
00464     {
00465       s = std::sqrt(tr);
00466       z = s*0.5;
00467       s = 0.5/s;
00468 
00469       x = (rotationMatrix(3,1) - rotationMatrix(1,3))*s;
00470       y = (rotationMatrix(3,2) + rotationMatrix(2,3))*s;
00471       w = (rotationMatrix(1,2) - rotationMatrix(2,1))*s;
00472       return;
00473     };
00474  
00475   tr = -rotationMatrix(1,1) + rotationMatrix(2,2) - rotationMatrix(3,3) + 1;
00476   if (tr > 0.0625)
00477     {
00478       s = std::sqrt(tr);
00479       y = s*0.5;
00480       s = 0.5/s;
00481 
00482       x = (rotationMatrix(2,1) + rotationMatrix(1,2))*s;
00483       z = (rotationMatrix(3,2) + rotationMatrix(2,3))*s;
00484       w = (rotationMatrix(3,1) - rotationMatrix(1,3))*s;
00485       return;
00486     };
00487 
00488   tr = rotationMatrix(1,1) - rotationMatrix(2,2) - rotationMatrix(3,3) + 1;
00489   if (tr > 0.0625)
00490     {
00491       s = std::sqrt(tr);
00492       x = s*0.5;
00493       s = 0.5/s;
00494 
00495       y = (rotationMatrix(2,1) + rotationMatrix(1,2))*s;
00496       z = (rotationMatrix(3,1) - rotationMatrix(1,3))*s;
00497       w = (rotationMatrix(2,3) - rotationMatrix(3,2))*s;
00498       return;
00499     };
00500 };

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