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 };