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

SIG_DynaSystem.cpp

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 /* int sz=dynaLinks.size();
00037  for (int i=0; i<sz; i++)
00038   if (dynaLinks[i]!=0)
00039   {
00040    if (dynaLinks[i]->collptp!=0)
00041    {
00042     DL_vector rF,nV;
00043     DL_point dP;
00044     dynaLinks[i]->collptp->reactionforce(&rF);
00045     dynaLinks[i]->collptp->get_dyna_point(&dP);
00046     dynaLinks[i]->dyna->get_newvelocity(&dP,&nV); 
00047     if (nV.y>0)
00048     {
00049      rF.timesis(-1);
00050      dynaLinks[i]->dyna->applyforce(&dP,dynaLinks[i]->dyna,&rF);
00051      rF.assign(&environment.getGravity());
00052      rF.timesis(-2);
00053      dynaLinks[i]->dyna->applyforce(&dP,dynaLinks[i]->dyna,&rF);
00054 #ifdef SIG_DEBUG
00055      SIGEL_Tools::SIG_IO::cerr << "----------------ptp deleted " << dynaLinks[i]->number << "\n";
00056 #endif
00057      delete dynaLinks[i]->collptp;
00058      dynaLinks[i]->collptp=0;
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 /*  if (fabs(val)>1000000)
00106    v=0;
00107   else
00108    v=val;
00109   return v; */
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  // Dynamo initialisieren
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  //SOLID initialisieren
00182  dtEnableCaching();
00183  dtSetDefaultResponse(collisionResponse,DT_SMART_RESPONSE,stdout);
00184 
00185  initializeFloor();
00186 };
00187 
00188 SIGEL_Simulation::SIG_DynaSystem::~SIG_DynaSystem()
00189 {
00190   //Dynamo beenden und zerstoeren
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  //Als "Grund"-Boden wird ein riesiges Polygon erzeugt
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  //alle DynaObjekte zerstoeren
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  //if (isnan(coll_data->normal[0]))
00246  // return;
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  // Kollision zweier Objekte
00266  {
00267 //  SIGEL_Tools::SIG_IO::cerr << "Kollision zweier Objekte\n";
00268   //Ist die Geschwindigkeit gross genug, wird die Kollision erzeugt,
00269   // ansonsten wird die Geschwindigkeit geloescht.
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   //Geschwindigkeit gross genug  
00275   {
00276 //   SIGEL_Tools::SIG_IO::cerr << "-Geschwindigkeit gross genug\n";
00277    normal=unNaN(normal);
00278    if (normal.norm()==0)
00279     return;
00280 //   SIGEL_Tools::SIG_IO::cerr << "N(w) x:" << normal.x << " y:" << normal.y << " z:" << normal.z << "\n";
00281        
00282     //Bewegen sich die Objekte aufeinander zu?
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     //Duerfen sie ueberhaupt kollidieren?
00300     if (/*true*/(dlObj1->link->getNoCollides().find(dlObj2->link))!=-1)
00301     //Kollision erlaubt
00302     {
00303 //     SIGEL_Tools::SIG_IO::cerr << "--Bewegen sich aufeinander zu und Kollision erlaubt\n";
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);  //Normalenkraftvektor
00313      DL_vector Fortho(&F);
00314      Fortho.minusis(&Fnormal);  //Restkraftvektor
00315      DL_vector newFortho(&Fortho);
00316      // Kraft mit mue
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     //Ende Kollision erlaubt
00330   }
00331   //Ende Geschwindigkeit gross genug 
00332   else
00333   //Geschwindigkeit zu klein
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   //Ende Geschwindigkeit zu klein
00342  } 
00343  //Ende Kollision zweier Objekte
00344  else
00345   if ((dlObj1==0)||(dlObj2==0))
00346  //Kollision mit Boden 
00347  {   
00348   //ist allerdings einer der pointer 0, so war dies eine Kollision mit dem Boden.
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   //und zwar im (lokalen) Punkt thePoint des DynaLinks theDlObj
00366   //nun muss die noch wirkende Kraft auf das Objekt geloescht werden und
00367   //aus dem Normalenvektor der Kollision sowie der Gravitation die neue
00368   //Gegenkraefte abzueglich der Reibung errechnet werden.
00369   //Fuer das Zwischenziel reicht allerdings eine simple Gegenkraft...
00370   //Ist die Geschwindigkeit gross genug, wird die Kollision erzeugt,
00371   // ansonsten wird die Geschwindigkeit geloescht.
00372   if ((theDlObj->dyna->get_velocity()->norm()>min_geschwindigkeit)
00373     ||(theDlObj->dyna->get_angvelocity()->norm()>min_geschwindigkeit))
00374   //Geschwindigkeit gross genug
00375   {
00376    //Bewegt sich das Objekt auf den Boden zu?
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);  //Normalenkraftvektor
00396     DL_vector Fortho(&F);
00397     Fortho.minusis(&Fnormal);  //Restkraftvektor
00398     DL_vector newFortho(&Fortho);
00399     // Gegenkraft mit mue
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   //Ende Geschwindigkeit gross genug
00412   else
00413   //Geschwindigkeit zu klein
00414   {
00415    DL_vector nullvektor(0,0,0);
00416    theDlObj->dyna->set_velocity(&nullvektor);
00417    theDlObj->dyna->set_angvelocity(&nullvektor);
00418   };
00419   //Ende Geschwindigkeit zu klein
00420   /*
00421   if (theWorldPointN.y<0)
00422   {
00423    DL_vector F,M,Fp,V;
00424    theDlObj->dyna->get_velocity(thePoint,&V);
00425    F=dynamic_cast<SIG_Dyna*>(theDlObj->dyna)->getForce();
00426    Fp=dynamic_cast<SIG_Dyna*>(theDlObj->dyna)->getPointForce(thePoint);
00427    M=dynamic_cast<SIG_Dyna*>(theDlObj->dyna)->getMomentum();
00428 /*#ifdef SIG_DEBUG
00429 SIGEL_Tools::SIG_IO::cerr << "--------------coll data\n";
00430 SIGEL_Tools::SIG_IO::cerr << "Point local X:" << thePoint->x << " Y:" << thePoint->y << " Z:" << thePoint->z << "\n";
00431 SIGEL_Tools::SIG_IO::cerr << "Point world X:" << theWorldPoint.x << " Y:" << theWorldPoint.y << " Z:" << theWorldPoint.z << "\n";
00432 SIGEL_Tools::SIG_IO::cerr << "Velocity X:" << V.x << " Y:" << V.y << " Z:" << V.z << "\n";
00433 SIGEL_Tools::SIG_IO::cerr << "Momentum X:" << M.x << " Y:" << M.y << " Z:" << M.z << "\n";
00434 SIGEL_Tools::SIG_IO::cerr << "Force X:" << F.x << " Y:" << F.y << " Z:" << F.z << "\n";
00435 SIGEL_Tools::SIG_IO::cerr << "Force Point X:" << Fp.x << " Y:" << Fp.y << " Z:" << Fp.z << "\n";
00436 #endif
00437    if ((theDlObj->collptp==0)&&(V.y<0))
00438    {
00439 #ifdef SIG_DEBUG
00440     SIGEL_Tools::SIG_IO::cerr << "----------------ptp created " << theDlObj->number << "\n";
00441 #endif
00442     theDlObj->collptp=new DL_ptp();
00443     theDlObj->collptp->init(theDlObj->dyna,thePoint,0,&theWorldPoint);
00444     theDlObj->collptp->soft();
00445     theDlObj->collptp->stiffness=0.01;
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  /* mal kurz weggemacht
00516    inertia=theLink.getInertiaVector();
00517    inertia=unNaN(inertia);
00518    if (inertia.x==0) inertia.x=1;
00519    if (inertia.y==0) inertia.y=1;
00520    if (inertia.z==0) inertia.z=1;
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  dynaLinks[sz]->dyna->set_inertiatensor(inertia.x,inertia.y,inertia.z);
00531  */
00532  dynaLinks[sz]->dyna->set_inertiatensor(it.get (0, 0),
00533                                         it.get (1, 1),
00534                                         it.get (2, 2));
00535  /*
00536  dynaLinks[sz]->dyna->set_mass(unNaN(theLink.getMass()));
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  SIGEL_Robot::SIG_Body const* body=theLink.getBody(); 
00543  SIGEL_Robot::SIG_Geometry const* geom=body->getGeometry();
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 

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