00001 #include "SIGEL_Simulation/SIG_DynaSystem.h"
00002 #include "euler.h"
00003 #include "doubleeuler.h"
00004 #include "rungekutta2.h"
00005 #include "rungekutta4.h"
00006 #include "linehinge.h"
00007 #include "connector.h"
00008 #include "bar.h"
00009 #include "pris.h"
00010 #include "actuator_fv.h"
00011 #include "ptp.h"
00012 #include "math.h"
00013 #include "SIGEL_Robot/SIG_GeometryIterator.h"
00014 #include "SIGEL_Robot/SIG_Polygon.h"
00015 #include "SOLID/solid.h"
00016 #include "SIGEL_Tools/SIG_IO.h"
00017 #include "SIGEL_Robot/SIG_JointSensor.h"
00018 #include "SIGEL_Robot/SIG_TranslationalJoint.h"
00019 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00020 #include "SIGEL_Robot/SIG_GlueJoint.h"
00021 #include "SIGEL_Simulation/SIG_Dyna.h"
00022 #include "SIGEL_Simulation/SIG_RotationalController.h"
00023 #include "SIGEL_Simulation/SIG_TranslationalController.h"
00024
00025 bool SIGEL_Simulation::SIG_DynaSystem::foundNaN(false);
00026
00027 void SIGEL_Simulation::SIG_DynaSystem::doNewFrame()
00028 {
00029 newFrame=true;
00030 dynaSystem->dynamics();
00031 checkCollisionPTPs();
00032 };
00033
00034 void SIGEL_Simulation::SIG_DynaSystem::checkCollisionPTPs()
00035 {
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 };
00063
00064
00065 DL_matrix SIGEL_Simulation::SIG_DynaSystem::rotationMatrix(DL_vector axis, DL_Scalar angle)
00066 {
00067 double pi=4*std::atan(1);
00068 DL_Scalar anglerad=angle*pi/180;
00069 DL_Scalar s=sin(angle);
00070 DL_Scalar c=cos(angle);
00071 DL_Scalar u=1-c;
00072 DL_matrix m;
00073 DL_vector v(&axis);
00074
00075 m.c0.x=v.x*v.x*u+c;
00076 m.c0.y=v.x*v.y*u+v.z*s;
00077 m.c0.z=v.x*v.z*u-v.y*s;
00078
00079 m.c1.x=v.y*v.x*u-v.z*s;
00080 m.c1.y=v.y*v.y*u+c;
00081 m.c1.z=v.y*v.z*u+v.x*s;
00082
00083 m.c2.x=v.z*v.x*u+v.y*s;
00084 m.c2.y=v.z*v.y*u-v.x*s;
00085 m.c2.z=v.z*v.z*u+c;
00086
00087 return m;
00088 };
00089
00090
00091 DL_Scalar SIGEL_Simulation::SIG_DynaSystem::unNaN(DL_Scalar val)
00092 {
00093 DL_Scalar v=0;
00094 if (isnan(val))
00095 {
00096 foundNaN=true;
00097 return v;
00098 }
00099 else
00100 {
00101 if (fabs(val)<0.0000001)
00102 return 0;
00103 else
00104 return val;
00105
00106
00107
00108
00109
00110 };
00111 };
00112
00113 DL_vector SIGEL_Simulation::SIG_DynaSystem::unNaN(DL_vector val)
00114 {
00115 DL_vector v(unNaN(val.x),unNaN(val.y),unNaN(val.z));
00116 return v;
00117 };
00118
00119 DL_point SIGEL_Simulation::SIG_DynaSystem::unNaN(DL_point val)
00120 {
00121 DL_point v(unNaN(val.x),unNaN(val.y),unNaN(val.z));
00122 return v;
00123 };
00124
00125 DL_matrix SIGEL_Simulation::SIG_DynaSystem::unNaN(DL_matrix val)
00126 {
00127 DL_vector vc0(unNaN(val.c0)),vc1(unNaN(val.c1)),vc2(unNaN(val.c2));
00128 DL_matrix v(&vc0,&vc1,&vc2);
00129 if(v.c0.norm()==0) v.c0.x=1;
00130 if(v.c1.norm()==0) v.c1.y=1;
00131 if(v.c2.norm()==0) v.c2.z=1;
00132 return v;
00133 };
00134
00135 SIGEL_Simulation::SIG_DynaSystem::SIG_DynaSystem(SIGEL_Environment::SIG_Environment const & theEnvironment,
00136 SIG_SimulationParameters const & theSimulationParameters)
00137 : environment(theEnvironment),
00138 simulationParameters(theSimulationParameters),
00139 QObject()
00140 {
00141 rootLinkNo=-1;
00142
00143
00144 dynaCallbacks=new SIG_DynaCallbacks();
00145 dynaCallbacks->dynaSystem=this;
00146
00147 switch (simulationParameters.getIntegrator())
00148 {
00149 case SIG_SimulationParameters::itEuler : mIntegrator=new DL_euler(); break;
00150 case SIG_SimulationParameters::itDoubleEuler : mIntegrator=new DL_double_euler(); break;
00151 case SIG_SimulationParameters::itRungeKutta2 : mIntegrator=new DL_rungekutta2(); break;
00152 default : mIntegrator=new DL_rungekutta4();
00153 };
00154
00155 #ifdef SIG_DEBUG
00156 SIGEL_Tools::SIG_IO::cerr << "Gleich stuerzt er ab...\n";
00157 #endif
00158 dynaSystem=new DL_dyna_system(dynaCallbacks,mIntegrator);
00159 #ifdef SIG_DEBUG
00160 SIGEL_Tools::SIG_IO::cerr << "...doch nicht!\n";
00161 #endif
00162
00163 constraintManager=new DL_constraint_manager();
00164
00165 DL_vector grav=environment.getGravity();
00166 dynaSystem->set_gravity(&grav);
00167 mIntegrator->set_stepsize(simulationParameters.getStepSize());
00168 constraintManager->max_error=simulationParameters.getStepSize();
00169 constraintManager->analytical=simulationParameters.getAnalytical();
00170 constraintManager->MaxIter=simulationParameters.getMaximalIterations();
00171 constraintManager->NrSkip=simulationParameters.getSkipFrames();
00172 constraintManager->max_collisionloops=simulationParameters.getMaximalCollisionLoops();
00173
00174 switch (simulationParameters.getSolveMode())
00175 {
00176 case SIG_SimulationParameters::smtLUDecomposition :constraintManager->solve_using_lud(); break;
00177 case SIG_SimulationParameters::smtConjugateGradient :constraintManager->solve_using_cg(); break;
00178 default :constraintManager->solve_using_svd(); break;
00179 };
00180
00181
00182 dtEnableCaching();
00183 dtSetDefaultResponse(collisionResponse,DT_SMART_RESPONSE,stdout);
00184
00185 initializeFloor();
00186 };
00187
00188 SIGEL_Simulation::SIG_DynaSystem::~SIG_DynaSystem()
00189 {
00190
00191 clearAllDynamics();
00192 delete constraintManager;
00193 delete dynaSystem;
00194 delete mIntegrator;
00195 delete dynaCallbacks;
00196 };
00197
00198 void SIGEL_Simulation::SIG_DynaSystem::initializeFloor()
00199 {
00200
00201 double y=environment.getYPlaneLevel();
00202
00203 floor=dtNewComplexShape();
00204 dtBegin(DT_POLYGON);
00205 dtVertex(-1000,y,-1000);
00206 dtVertex(-1000,y, 1000);
00207 dtVertex( 1000,y, 1000);
00208 dtVertex( 1000,y,-1000);
00209 dtEnd();
00210 dtEndComplexShape();
00211 dtCreateObject(0,floor);
00212 };
00213
00214 void SIGEL_Simulation::SIG_DynaSystem::clearAllDynamics()
00215 {
00216 rootLinkNo=-1;
00217
00218 dtDeleteObject(0);
00219 int sz=dynaLinks.size();
00220 for (int i=0; i<sz; i++)
00221 if (dynaLinks[i]!=0)
00222 {
00223 dtDeleteObject(dynaLinks[i]);
00224 delete dynaLinks[i];
00225 };
00226 sz=dynaJoints.size();
00227 for (int i=0; i<sz; i++)
00228 if (dynaJoints[i]!=0)
00229 delete dynaJoints[i];
00230 sz=dynaDrives.size();
00231 for (int i=0; i<sz; i++)
00232 if (dynaDrives[i]!=0)
00233 delete dynaJoints[i];
00234 sz=dynaSensors.size();
00235 for (int i=0; i<sz; i++)
00236 if (dynaSensors[i]!=0)
00237 delete dynaSensors[i];
00238 };
00239
00240 void SIGEL_Simulation::SIG_DynaSystem::collisionResponse(void * client_data,
00241 DtObjectRef obj1,
00242 DtObjectRef obj2,
00243 const DtCollData *coll_data)
00244 {
00245
00246
00247
00248 double min_geschwindigkeit=0;
00249
00250 SIG_DynaLink* dlObj1=(SIG_DynaLink*)obj1;
00251 SIG_DynaLink* dlObj2=(SIG_DynaLink*)obj2;
00252
00253 DL_point point1(coll_data->point1[0],
00254 coll_data->point1[1],
00255 coll_data->point1[2]);
00256 DL_point point2(coll_data->point2[0],
00257 coll_data->point2[1],
00258 coll_data->point2[2]);
00259 DL_vector normal(coll_data->normal[0],
00260 coll_data->normal[1],
00261 coll_data->normal[2]);
00262 normal.normalize();
00263
00264 if ((dlObj1!=0)&&(dlObj2!=0))
00265
00266 {
00267
00268
00269
00270 if ((dlObj1->dyna->get_velocity()->norm()>min_geschwindigkeit)
00271 ||(dlObj2->dyna->get_velocity()->norm()>min_geschwindigkeit)
00272 ||(dlObj1->dyna->get_angvelocity()->norm()>min_geschwindigkeit)
00273 ||(dlObj2->dyna->get_angvelocity()->norm()>min_geschwindigkeit))
00274
00275 {
00276
00277 normal=unNaN(normal);
00278 if (normal.norm()==0)
00279 return;
00280
00281
00282
00283 DL_point p1,p2,p1n,p2n,p1w,p2w,p1nw,p2nw;
00284 DL_vector v1,v2,v1n,v2n,d,dn;
00285 p1=(dlObj1->dyna->get_position());
00286 p2=(dlObj2->dyna->get_position());
00287 p1n=(dlObj1->dyna->get_next_position());
00288 p2n=(dlObj2->dyna->get_next_position());
00289 p1.tovector(&v1);
00290 p2.tovector(&v2);
00291 p1n.tovector(&v1n);
00292 p2n.tovector(&v2n);
00293 d=v1;
00294 dn=v1n;
00295 d.minusis(&v2);
00296 dn.minusis(&v2n);
00297
00298 if (d.norm()>dn.norm())
00299
00300 if ((dlObj1->link->getNoCollides().find(dlObj2->link))!=-1)
00301
00302 {
00303
00304 DL_vector F;
00305
00306 SIG_Dyna* sd=dynamic_cast<SIG_Dyna*>(dlObj1->dyna);
00307 F=sd->getPointForce(&point1);
00308
00309 DL_Scalar Flength=F.inprod(&normal);
00310 DL_vector Fnormal(&normal);
00311
00312 Fnormal.timesis(Flength);
00313 DL_vector Fortho(&F);
00314 Fortho.minusis(&Fnormal);
00315 DL_vector newFortho(&Fortho);
00316
00317 newFortho.normalize();
00318 DL_Scalar mue=dlObj1->link->getMaterial()->getFrictionValue(const_cast<SIGEL_Robot::SIG_Material*>(dlObj2->link->getMaterial()));
00319 DL_Scalar FRlength=Fnormal.norm()*mue;
00320 if (FRlength>Fortho.norm())
00321 FRlength=Fortho.norm();
00322 newFortho.timesis(-FRlength);
00323
00324 dlObj1->dyna->applyforce(&point1,dlObj1->dyna,&newFortho);
00325 newFortho.timesis(-1);
00326 dlObj2->dyna->applyforce(&point2,dlObj2->dyna,&newFortho);
00327 DL_collision* collConstr=new DL_collision(dlObj1->dyna,&point1,dlObj2->dyna,&point2,&normal,2);
00328 };
00329
00330 }
00331
00332 else
00333
00334 {
00335 DL_vector nullvektor(0,0,0);
00336 dlObj1->dyna->set_velocity(&nullvektor);
00337 dlObj2->dyna->set_velocity(&nullvektor);
00338 dlObj1->dyna->set_angvelocity(&nullvektor);
00339 dlObj2->dyna->set_angvelocity(&nullvektor);
00340 };
00341
00342 }
00343
00344 else
00345 if ((dlObj1==0)||(dlObj2==0))
00346
00347 {
00348
00349 SIG_DynaLink* theDlObj;
00350 DL_point* thePoint;
00351 if (dlObj1==0)
00352 {
00353 theDlObj=dlObj2;
00354 thePoint=&point2;
00355 }
00356 else
00357 {
00358 theDlObj=dlObj1;
00359 thePoint=&point1;
00360 };
00361
00362 DL_point theWorldPoint,theWorldPointN;
00363 theDlObj->dyna->to_world(thePoint,&theWorldPoint);
00364 theDlObj->dyna->new_toworld(thePoint,&theWorldPointN);
00365
00366
00367
00368
00369
00370
00371
00372 if ((theDlObj->dyna->get_velocity()->norm()>min_geschwindigkeit)
00373 ||(theDlObj->dyna->get_angvelocity()->norm()>min_geschwindigkeit))
00374
00375 {
00376
00377 DL_point pw,pnw;
00378 theDlObj->dyna->to_world(thePoint,&pw);
00379 theDlObj->dyna->new_toworld(thePoint,&pnw);
00380 double y=pw.y;
00381 double yn=pnw.y;
00382 double yw=point2.y;
00383 normal.init(0,1,0);
00384 y=y-yw;
00385 yn=yn-yw;
00386 if ((y>yn))
00387 {
00388 DL_vector F;
00389 SIG_Dyna* sd=dynamic_cast<SIG_Dyna*>(theDlObj->dyna);
00390 F=sd->getPointForce(thePoint);
00391
00392 DL_Scalar Flength=F.inprod(&normal);
00393 DL_vector Fnormal(&normal);
00394
00395 Fnormal.timesis(Flength);
00396 DL_vector Fortho(&F);
00397 Fortho.minusis(&Fnormal);
00398 DL_vector newFortho(&Fortho);
00399
00400 newFortho.normalize();
00401 DL_Scalar mue=theDlObj->link->getMaterial()->getFrictionValue(theDlObj->floorMaterial);
00402 DL_Scalar FRlength=Fnormal.norm()*mue;
00403 if (FRlength>Fortho.norm())
00404 FRlength=Fortho.norm();
00405 newFortho.timesis(-FRlength);
00406
00407 theDlObj->dyna->applyforce(&theWorldPoint,0,&newFortho);
00408 DL_collision* collConstr=new DL_collision(theDlObj->dyna,thePoint,0,&theWorldPoint,&normal,1);
00409 };
00410 }
00411
00412 else
00413
00414 {
00415 DL_vector nullvektor(0,0,0);
00416 theDlObj->dyna->set_velocity(&nullvektor);
00417 theDlObj->dyna->set_angvelocity(&nullvektor);
00418 };
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448 };
00449 //Ende Kollision mit Boden
00450 };
00451
00452 void SIGEL_Simulation::SIG_DynaSystem::msgFunction(QString theMessage)
00453 {
00454 emit signalDynamoMessage(theMessage);
00455 };
00456
00457 void SIGEL_Simulation::SIG_DynaSystem::doCollisionDetection()
00458 {
00459 checkCollisionPTPs();
00460 //Solid aufrufen
00461 if (newFrame)
00462 {
00463 int j=dtTest();
00464 if (j==0)
00465 newFrame=false;
00466 }
00467 else
00468 dtProceed;
00469 };
00470
00471 void SIGEL_Simulation::SIG_DynaSystem::newLink(SIGEL_Robot::SIG_Link& theLink)
00472 {
00473 SIGEL_Tools::SIG_IO::cerr << "Link " << theLink.getNumber() << " : " << theLink.getName() << "\n";
00474 QDictIterator<DL_vector> pts=theLink.getPointIter();
00475 int n=theLink.getNrOfPoints();
00476 while (pts.current()) {
00477 DL_vector p=(pts.current());
00478 SIGEL_Tools::SIG_IO::cerr << " " << pts.currentKey() << " X:" << p.x << " Y:" << p.y << " Z:" << p.z << "\n";
00479 ++pts;
00480 }
00481
00482
00483 //Wenn RootLink, dann eintragen
00484 if (theLink.isRootLink())
00485 rootLinkNo=theLink.getNumber();
00486 //Link in DynaLink umwandeln
00487 int sz=dynaLinks.size();
00488 DL_vector pos,startpos,inertia;
00489 DL_matrix orient2,orient;
00490
00491
00492 // Holgers Vars
00493 DL_Scalar m; // (wie Masse)
00494 DL_vector com; // (wie Centre Of Mass)
00495 DL_matrix it,it2; // (wie inertia tensor)
00496
00497 theLink.getInitialLocation(pos,orient2);
00498 orient=unNaN(orient2);
00499 SIGEL_Tools::SIG_IO::cerr << " Pos X:" << pos.x << " Y:" << pos.y << " Z:" << pos.z << "\n";
00500 SIGEL_Tools::SIG_IO::cerr << " Orient: (" << orient.c0.x << "|" << orient.c1.x << "|" << orient.c2.x << ")\n";
00501 SIGEL_Tools::SIG_IO::cerr << " (" << orient.c0.y << "|" << orient.c1.y << "|" << orient.c2.y << ")\n";
00502 SIGEL_Tools::SIG_IO::cerr << " (" << orient.c0.z << "|" << orient.c1.z << "|" << orient.c2.z << ")\n";
00503
00504 startpos=environment.getStartPosition();
00505 pos.plusis(&startpos);
00506
00507 DL_point pos2;
00508 pos.topoint(&pos2);
00509 dynaLinks.resize(sz+1);
00510 dynaLinks.insert(sz,new SIG_DynaLink(unNaN(pos2),orient));
00511 dynaLinks[sz]->number=theLink.getNumber();
00512 dynaLinks[sz]->shape=dtNewComplexShape();
00513 dynaLinks[sz]->link=&theLink;
00514 dynaLinks[sz]->floorMaterial=floorMaterial;
00515
00516
00517
00518
00519
00520
00521
00522 theLink.getPhysics (m, com, it2);
00523 it=unNaN(it2);
00524 if (it.c0.x==0) it.c0.x=1;
00525 if (it.c1.y==0) it.c1.y=1;
00526 if (it.c2.z==0) it.c2.z=1;
00527 m=unNaN(m);
00528 if (m==0) m=1;
00529
00530
00531
00532 dynaLinks[sz]->dyna->set_inertiatensor(it.get (0, 0),
00533 it.get (1, 1),
00534 it.get (2, 2));
00535
00536
00537
00538 dynaLinks[sz]->dyna->set_mass(m);
00539 dynaLinks[sz]->dyna->set_velodamping(unNaN(environment.getVeloDamping()));
00540 dynaLinks[sz]->dyna->set_elasticity(unNaN(theLink.getMaterial()->getElasticity()));
00541
00542
00543
00544
00545 SIGEL_Robot::SIG_Geometry const *geom=theLink.getGeometry();
00546 SIGEL_Robot::SIG_GeometryIterator iter(geom);
00547 while (iter.valid())
00548 {
00549 SIGEL_Robot::SIG_Polygon const & poly=iter.iterate();
00550
00551 dtBegin(DT_POLYGON);
00552 int n=poly.getNumVertices();
00553
00554 for (int i=0; i<n; i++)
00555 {
00556 DL_vector const & p=poly.getVertex(i);
00557 dtVertex(p.x,p.y,p.z);
00558 };
00559 dtEnd();
00560 };
00561
00562 dtEndComplexShape();
00563 dtCreateObject(dynaLinks[sz],(dynaLinks[sz]->shape));
00564 };
00565
00566 void SIGEL_Simulation::SIG_DynaSystem::newJoint(SIGEL_Robot::SIG_Joint& theJoint)
00567 {
00568 //Joint in dynaJoint eintragen
00569 int sz=dynaJoints.size();
00570 dynaJoints.resize(sz+1);
00571 dynaJoints.insert(sz,new SIG_DynaJoint());
00572
00573 dynaJoints[sz]->number=theJoint.getNumber();
00574
00575 SIGEL_Robot::SIG_Link const * lLink=theJoint.getLeftLink();
00576 SIGEL_Robot::SIG_Link const * rLink=theJoint.getRightLink();
00577 SIG_DynaLink & ldLink = getLink(lLink->getNumber());
00578 SIG_DynaLink & rdLink = getLink(rLink->getNumber());
00579 //passende DL_constraint herausfinden und erzeugen
00580 switch (theJoint.getJointType()) {
00581
00582 //------------- ROTATIONAL JOINT --------------------------------------------------
00583 case SIGEL_Robot::SIG_Joint::tRotationalJoint :
00584 {
00585 //casten
00586 SIGEL_Robot::SIG_RotationalJoint & rotationalJoint = dynamic_cast<SIGEL_Robot::SIG_RotationalJoint&> (theJoint);
00587 //Joint auswerten und die constraint erzeugen
00588 DL_linehinge * dynarot;
00589 dynarot=new DL_linehinge();
00590 //Punkte auslesen
00591 DL_vector vlbase,vldir,vrbase,vrdir,vlhand,vrhand;
00592 vlbase=rotationalJoint.getLeftBase();
00593 vldir=rotationalJoint.getLeftDir();
00594 vlhand=rotationalJoint.getLeftHand();
00595 vrbase=rotationalJoint.getRightBase();
00596 vrdir=rotationalJoint.getRightDir();
00597 vrhand=rotationalJoint.getRightHand();
00598 //Abstand zwischen Dir1 und Dir2 anpassen
00599 DL_vector nvldir(&vldir);
00600 nvldir.minusis(&vlbase);
00601 nvldir.normalize();
00602 nvldir.plusis(&vlbase);
00603 DL_vector nvrdir(&vrdir);
00604 nvrdir.minusis(&vrbase);
00605 nvrdir.normalize();
00606 nvrdir.plusis(&vrbase);
00607 //Constraint erzeugen
00608 DL_point lbase,rbase,ldir,rdir;
00609 vlbase.topoint(&lbase);
00610 nvldir.topoint(&ldir);
00611 vrbase.topoint(&rbase);
00612 nvrdir.topoint(&rdir);
00613 dynarot->init(ldLink.dyna,&lbase,&ldir,rdLink.dyna,&rbase,&rdir);
00614 dynarot->soft();
00615 dynarot->stiffness=0.01;
00616 vlhand.minusis(&vlbase);
00617 vlhand.normalize();
00618 vrhand.minusis(&vrbase);
00619 vrhand.normalize();
00620 dynaJoints[sz]->joint=&theJoint;
00621 dynaJoints[sz]->constraint=dynarot;
00622 dynaJoints[sz]->leftDyna=&ldLink;
00623 dynaJoints[sz]->rightDyna=&rdLink;
00624 dynaJoints[sz]->leftFix.assign(&lbase);
00625 dynaJoints[sz]->leftFixB.assign(&ldir);
00626 dynaJoints[sz]->leftFixC.init(0,0,0);
00627 dynaJoints[sz]->leftDir.init(0,0,0);
00628 dynaJoints[sz]->leftUp.assign(&vlhand);
00629 dynaJoints[sz]->rightFix.assign(&rbase);
00630 dynaJoints[sz]->rightFixB.assign(&rdir);
00631 dynaJoints[sz]->rightFixC.init(0,0,0);
00632 dynaJoints[sz]->rightDir.init(0,0,0);
00633 dynaJoints[sz]->rightUp.assign(&vrhand);
00634 SIG_RotationalController * rc=new SIG_RotationalController();
00635 //rc->init(dynaJoints[sz],rotationalJoint.getMin(),rotationalJoint.getMax());
00636 dynaJoints[sz]->controller=rc;
00637 dynaJoints[sz]->deflection=0;
00638 break;
00639 };
00640
00641 //------------- TRANSLATIONAL JOINT -----------------------------------------------
00642 case SIGEL_Robot::SIG_Joint::tTranslationalJoint :
00643 {
00644 //casten
00645 SIGEL_Robot::SIG_TranslationalJoint & translationalJoint = dynamic_cast<SIGEL_Robot::SIG_TranslationalJoint&> (theJoint);
00646 //Joint auswerten und die constraint erzeugen
00647 DL_pris * dynatrans;
00648 dynatrans=new DL_pris();
00649 //Punkte auslesen
00650 DL_vector vlbase,vldir,vrbase,vrdir,vlfix,vrfix;
00651 vlbase=translationalJoint.getLeftBase();
00652 vldir=translationalJoint.getLeftDir();
00653 vlfix=translationalJoint.getLeftFix();
00654 vrbase=translationalJoint.getRightBase();
00655 vrdir=translationalJoint.getRightDir();
00656 vrfix=translationalJoint.getRightFix();
00657 //Umrechnen der Fix/Dir-Punkte auf Vektoren
00658 vldir.minusis(&vlbase);
00659 vlfix.minusis(&vlbase);
00660 vrdir.minusis(&vrbase);
00661 vrfix.minusis(&vrbase);
00662 //Constraint erzeugen
00663 DL_point lbase,rbase,ldir,rdir,lfix,rfix;
00664 vlbase.topoint(&lbase);
00665 vrbase.topoint(&rbase);
00666 vldir.topoint(&ldir);
00667 vrdir.topoint(&rdir);
00668 vlfix.topoint(&lfix);
00669 vrfix.topoint(&rfix);
00670 dynatrans->init(ldLink.dyna,&lbase,&vldir,&vlfix,rdLink.dyna,&rbase,&vrdir,&vrfix);
00671 dynatrans->soft();
00672 dynatrans->stiffness=0.01;
00673 dynaJoints[sz]->joint=&theJoint;
00674 dynaJoints[sz]->constraint=dynatrans;
00675 dynaJoints[sz]->leftDyna=&ldLink;
00676 dynaJoints[sz]->rightDyna=&rdLink;
00677 dynaJoints[sz]->leftFix.assign(&lbase);
00678 dynaJoints[sz]->leftFixB.init(0,0,0);
00679 dynaJoints[sz]->leftFixC.init(0,0,0);
00680 dynaJoints[sz]->leftDir.assign(&vldir);
00681 dynaJoints[sz]->leftUp.assign(&vlfix);
00682 dynaJoints[sz]->rightFix.assign(&rbase);
00683 dynaJoints[sz]->rightFixB.init(0,0,0);
00684 dynaJoints[sz]->rightFixC.init(0,0,0);
00685 dynaJoints[sz]->rightDir.assign(&vrdir);
00686 dynaJoints[sz]->rightUp.assign(&vrfix);
00687 SIG_TranslationalController * tc=new SIG_TranslationalController();
00688 //tc->init(dynaJoints[sz],translationalJoint.getMin(),translationalJoint.getMax());
00689 dynaJoints[sz]->controller=tc;
00690 dynaJoints[sz]->deflection=0;
00691 break;
00692 };
00693 //------------- GLUE JOINT --------------------------------------------------------
00694 case SIGEL_Robot::SIG_Joint::tGlueJoint :
00695 {
00696 //casten
00697 SIGEL_Robot::SIG_GlueJoint & glueJoint = dynamic_cast<SIGEL_Robot::SIG_GlueJoint&> (theJoint);
00698 //Joint auswerten und die constraint erzeugen
00699 DL_connector * dynaglue;
00700 dynaglue=new DL_connector();
00701 //Punkte auslesen
00702 DL_vector a1,a2,a3,b1,b2,b3;
00703 glueJoint.getPlaneA(a1,a2,a3);
00704 glueJoint.getPlaneB(b1,b2,b3);
00705 //Constraint erzeugen
00706 DL_point pa1,pa2,pa3,pb1,pb2,pb3;
00707 a1.topoint(&pa1);
00708 a2.topoint(&pa2);
00709 a3.topoint(&pa3);
00710 b1.topoint(&pb1);
00711 b2.topoint(&pb2);
00712 b3.topoint(&pb3);
00713 dynaglue->init(ldLink.dyna,&pa1,&pa2,&pa3,rdLink.dyna,&pb1,&pb2,&pb3);
00714 dynaglue->soft();
00715 dynaglue->stiffness=0.01;
00716 dynaJoints[sz]->joint=&theJoint;
00717 dynaJoints[sz]->constraint=dynaglue;
00718 dynaJoints[sz]->leftDyna=&ldLink;
00719 dynaJoints[sz]->rightDyna=&rdLink;
00720 dynaJoints[sz]->leftFix.assign(&pa1);
00721 dynaJoints[sz]->leftFixB.assign(&pa2);
00722 dynaJoints[sz]->leftFixC.assign(&pa3);
00723 dynaJoints[sz]->leftDir.init(0,0,0);
00724 dynaJoints[sz]->leftUp.init(0,0,0);
00725 dynaJoints[sz]->rightFix.assign(&pb1);
00726 dynaJoints[sz]->rightFixB.assign(&pb2);
00727 dynaJoints[sz]->rightFixC.assign(&pb3);
00728 dynaJoints[sz]->rightDir.init(0,0,0);
00729 dynaJoints[sz]->rightUp.init(0,0,0);
00730 dynaJoints[sz]->controller=0;
00731 dynaJoints[sz]->deflection=0;
00732 break;
00733 };
00734 default :
00735 {
00736 dynaJoints[sz]->joint=0;
00737 dynaJoints[sz]->constraint=0;
00738 dynaJoints[sz]->leftDyna=0;
00739 dynaJoints[sz]->rightDyna=0;
00740 dynaJoints[sz]->leftFix.init(0,0,0);
00741 dynaJoints[sz]->leftFixB.init(0,0,0);
00742 dynaJoints[sz]->leftFixC.init(0,0,0);
00743 dynaJoints[sz]->leftDir.init(0,0,0);
00744 dynaJoints[sz]->leftUp.init(0,0,0);
00745 dynaJoints[sz]->rightFix.init(0,0,0);
00746 dynaJoints[sz]->rightFixB.init(0,0,0);
00747 dynaJoints[sz]->rightFixC.init(0,0,0);
00748 dynaJoints[sz]->rightDir.init(0,0,0);
00749 dynaJoints[sz]->rightUp.init(0,0,0);
00750 dynaJoints[sz]->controller=0;
00751 dynaJoints[sz]->deflection=0;
00752 };
00753 };
00754 };
00755
00756 void SIGEL_Simulation::SIG_DynaSystem::newSensor(SIGEL_Robot::SIG_Sensor& theSensor)
00757 {
00758 //Sensor in dynaSensor eintragen
00759 int sz=dynaSensors.size();
00760 dynaSensors.resize(sz+1);
00761 dynaSensors.insert(sz,new SIG_DynaSensor());
00762
00763 dynaSensors[sz]->number=theSensor.getNumber();
00764 dynaSensors[sz]->sensor=&theSensor;
00765 dynaSensors[sz]->joint=0;
00766 };
00767
00768 void SIGEL_Simulation::SIG_DynaSystem::newDrive(SIGEL_Robot::SIG_Drive& theDrive)
00769 {
00770 //Drive in dynaDrives eintragen
00771 int sz=dynaDrives.size();
00772 dynaDrives.resize(sz+1);
00773 dynaDrives.insert(sz,new SIG_DynaDrive());
00774
00775 SIG_DynaJoint * djoint=&getJoint(theDrive.getJoint()->getNumber());
00776 dynaDrives[sz]->drive=&theDrive;
00777 dynaDrives[sz]->number=theDrive.getNumber();
00778 dynaDrives[sz]->joint=djoint;
00779 };
00780
00781 SIGEL_Simulation::SIG_DynaLink& SIGEL_Simulation::SIG_DynaSystem::getLink(int number)
00782 throw(SIG_DynaSystemWrongNumberException)
00783 {
00784 //DynaLink aus QVector holen
00785 int sz=dynaLinks.size();
00786 for (int i=0; i<sz; i++)
00787 if (dynaLinks[i]->number==number)
00788 {
00789 return *(dynaLinks[i]);
00790 };
00791 throw SIG_DynaSystemWrongNumberException(__FILE__,__LINE__,"SIG_DynaLink with this number doesn't exist");
00792 };
00793
00794 SIGEL_Simulation::SIG_DynaJoint& SIGEL_Simulation::SIG_DynaSystem::getJoint(int number)
00795 throw(SIG_DynaSystemWrongNumberException)
00796 {
00797 //DynaJoint aus QVector holen
00798 int sz=dynaJoints.size();
00799 for (int i=0; i<sz; i++)
00800 if (dynaJoints[i]->number==number)
00801 return *(dynaJoints[i]);
00802 throw SIG_DynaSystemWrongNumberException(__FILE__,__LINE__,"SIG_DynaJoint with this number doesn't exist");
00803 };
00804
00805 SIGEL_Simulation::SIG_DynaDrive& SIGEL_Simulation::SIG_DynaSystem::getDrive(int number)
00806 throw(SIG_DynaSystemWrongNumberException)
00807 {
00808 //DynaDrive aus QVector holen
00809 int sz=dynaDrives.size();
00810 for (int i=0; i<sz; i++)
00811 if (dynaDrives[i]->number==number)
00812 return *(dynaDrives[i]);
00813 throw SIG_DynaSystemWrongNumberException(__FILE__,__LINE__,"SIG_DynaDrive with this number doesn't exist");
00814 };
00815
00816 SIGEL_Simulation::SIG_DynaSensor& SIGEL_Simulation::SIG_DynaSystem::getSensor(int number)
00817 throw(SIG_DynaSystemWrongNumberException)
00818 {
00819 //DynaSensor aus QVector holen
00820 int sz=dynaSensors.size();
00821 for (int i=0; i<sz; i++)
00822 if (dynaSensors[i]->number==number)
00823 return *(dynaSensors[i]);
00824 throw SIG_DynaSystemWrongNumberException(__FILE__,__LINE__,"SIG_DynaSensor with this number doesn't exist");
00825 };
00826
00827