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

SIG_RotationalController.cpp

00001 #include "SIGEL_Simulation/SIG_RotationalController.h"
00002 #include "SIGEL_Simulation/SIG_Dyna.h"
00003 #include "pointvector.h"
00004 #include <cmath>
00005 #include "SIGEL_Tools/SIG_IO.h"
00006 
00007 SIGEL_Simulation::SIG_RotationalController::SIG_RotationalController()
00008 {
00009  minimum=0;
00010  maximum=0;
00011  joint=0;
00012 };
00013 
00014 SIGEL_Simulation::SIG_RotationalController::~SIG_RotationalController()
00015 {
00016 };
00017 
00018 void SIGEL_Simulation::SIG_RotationalController::init(SIG_DynaJoint * theJoint, DL_Scalar theMinimum, DL_Scalar theMaximum)
00019 {
00020  minimum=theMinimum;
00021  maximum=theMaximum;
00022  joint=theJoint;
00023  activate();
00024 };
00025 
00026 void SIGEL_Simulation::SIG_RotationalController::calculate_and_apply()
00027 {
00028   /* Hier muss nun der Winkel zwischen den beiden up-Vektoren in Weltkoordinaten gemessen werden,
00029    * danach sollte ueberprueft werden, ob eine der Grenzen ueberschritten ist.
00030    * Zum Schluss sollte auf die beiden Dynas eine Kraft einwirken 
00031    * Diese Kraft errechnet sich irgendwie aus der in den beiden Punkten wirkenden Geschwindigkeit
00032    */
00033 
00034  /* Daten auslesen */
00035  DL_vector lUp,rUp,dir,nextlUp,nextrUp,nextdir;
00036  DL_point fix,fixB,nextfix,nextfixB;
00037  joint->leftDyna->dyna->to_world(&joint->leftUp,&lUp);
00038  joint->rightDyna->dyna->to_world(&joint->rightUp,&rUp);
00039  joint->rightDyna->dyna->to_world(&joint->rightFix,&fix);
00040  joint->rightDyna->dyna->to_world(&joint->rightFixB,&fixB);
00041  fixB.minus(&fix,&dir);
00042  lUp.normalize();
00043  rUp.normalize();
00044  dir.normalize();
00045  joint->leftDyna->dyna->new_toworld(&joint->leftUp,&nextlUp);
00046  joint->rightDyna->dyna->new_toworld(&joint->rightUp,&nextrUp);
00047  joint->rightDyna->dyna->new_toworld(&joint->rightFix,&nextfix);
00048  joint->rightDyna->dyna->new_toworld(&joint->rightFixB,&nextfixB);
00049  nextfixB.minus(&nextfix,&nextdir);
00050  nextlUp.normalize();
00051  nextrUp.normalize();
00052  nextdir.normalize();
00053  
00054  /* Winkel berechnen */
00055  DL_Scalar prod=lUp.inprod(&rUp);
00056  DL_Scalar nextprod=nextlUp.inprod(&nextrUp);
00057  double pi=4*std::atan(1);
00058  double angle=acos(prod)*180/pi;
00059  double nextangle=acos(nextprod)*180/pi;
00060  
00061  /* Herausfinden, ob der Winkel groesser als 180 Grad ist */
00062  DL_vector x,nextx;
00063  dir.crossprod(&lUp,&x);
00064  DL_Scalar sp=x.inprod(&rUp);
00065  if (sp>0)
00066   angle=360-angle;
00067  nextdir.crossprod(&nextlUp,&nextx);
00068  DL_Scalar nextsp=nextx.inprod(&nextrUp);
00069  if (nextsp>0)
00070   nextangle=360-nextangle;
00071  
00072  /* Angriffspunkt und Richtung der Gegenkraft errechnen */
00073  DL_point lpoint(&joint->leftFix);
00074  DL_vector lup(&joint->leftUp);
00075  lup.normalize();
00076  lpoint.plusis(&lup);
00077  
00078  DL_point lFix(&joint->leftFix);
00079  DL_point lFixB(&joint->leftFixB);
00080  DL_vector ldir;
00081  lFixB.minus(&lFix,&ldir);
00082  ldir.normalize();
00083  
00084  DL_vector lfdir;
00085  ldir.crossprod(&lup,&lfdir);
00086  lfdir.normalize();
00087  
00088  DL_point rpoint(&joint->rightFix);
00089  DL_vector rup(&joint->rightUp);
00090  rup.normalize();
00091  rpoint.plusis(&rup);
00092  
00093  DL_point rFix(&joint->rightFix);
00094  DL_point rFixB(&joint->rightFixB);
00095  DL_vector rdir;
00096  rFixB.minus(&rFix,&rdir);
00097  rdir.normalize();
00098  
00099  DL_vector rfdir;
00100  rdir.crossprod(&rup,&rfdir);
00101  rfdir.normalize();
00102 
00103  DL_Scalar newMax=maximum;
00104  if (minimum>maximum)
00105  {
00106   newMax=newMax+360;
00107   if (angle<180)
00108    angle=angle+360;
00109   if (nextangle<180)
00110    nextangle=nextangle+360;
00111  };
00112  
00113 #ifdef SIG_DEBUG 
00114  SIGEL_Tools::SIG_IO::cerr << "Auslenkung Min:"<< minimum <<" Akt:" << angle << " Max:" << newMax <<"\n";
00115 #endif 
00116 
00117  DL_point llpoint,lrpoint;
00118  joint->leftDyna->dyna->to_local(&lpoint,0,&llpoint);
00119  joint->rightDyna->dyna->to_local(&rpoint,0,&lrpoint);
00120  DL_vector llfdir,lrfdir;
00121  joint->leftDyna->dyna->to_local(&lfdir,0,&llfdir);
00122  joint->rightDyna->dyna->to_local(&rfdir,0,&lrfdir);
00123  
00124  DL_vector forcel=(dynamic_cast<SIG_Dyna*>(joint->leftDyna->dyna))->getPointForce(&llpoint);
00125  DL_vector forcer=(dynamic_cast<SIG_Dyna*>(joint->rightDyna->dyna))->getPointForce(&lrpoint);
00126  forcel.inprod(&llfdir);
00127  forcer.inprod(&lrfdir);
00128  DL_Scalar nforcel=forcel.norm();
00129  DL_Scalar nforcer=forcer.norm();
00130  DL_Scalar reagent=(nforcel+nforcer);
00131  DL_Scalar border=(newMax-minimum)*0.0;
00132  
00133  if (angle<(minimum+border))
00134  {
00135   DL_Scalar faktor=((minimum+border)-angle)*reagent;
00136   if (nextangle>angle)
00137    faktor=faktor/2;
00138   lfdir.timesis(faktor);
00139   rfdir.timesis(-faktor);
00140 #ifdef SIG_DEBUG 
00141  SIGEL_Tools::SIG_IO::cerr << "#### ZU KLEIN ###################################################\n";
00142  SIGEL_Tools::SIG_IO::cerr << "Gegenkraft mit:" << lfdir.norm() <<"\n";
00143 #endif 
00144   joint->leftDyna->dyna->applyforce(&llpoint,joint->leftDyna->dyna,&lfdir); 
00145   joint->rightDyna->dyna->applyforce(&lrpoint,joint->rightDyna->dyna,&rfdir);
00146  };
00147  if (angle>(newMax-border))
00148  {
00149   DL_Scalar faktor=(angle-(newMax-border))*reagent;
00150   if (nextangle<angle)
00151    faktor=faktor/2;
00152   lfdir.timesis(-faktor);
00153   rfdir.timesis(faktor);
00154 #ifdef SIG_DEBUG 
00155  SIGEL_Tools::SIG_IO::cerr << "#### ZU GROSS ###################################################\n";
00156  SIGEL_Tools::SIG_IO::cerr << "Gegenkraft mit:" << lfdir.norm() <<"\n";
00157 #endif 
00158   joint->leftDyna->dyna->applyforce(&llpoint,joint->leftDyna->dyna,&lfdir);
00159   joint->rightDyna->dyna->applyforce(&lrpoint,joint->rightDyna->dyna,&rfdir);
00160  };
00161 };

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