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
00029
00030
00031
00032
00033
00034
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
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
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
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 };