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

SIG_DynaDrive.cpp

00001 #include "SIGEL_Simulation/SIG_DynaDrive.h"
00002 #include "SIGEL_Simulation/SIG_Dyna.h"
00003 #include "SIGEL_Tools/SIG_IO.h"
00004 
00005 void SIGEL_Simulation::SIG_DynaDrive::applyForce(DL_Scalar f1, DL_Scalar f2)
00006 {
00007  SIGEL_Tools::SIG_IO::cerr << "Move " << number << " (" << f1 << "," << f2 << ")\n";
00008  
00009  if (joint==0) return;
00010  DL_dyna * ldyna=joint->leftDyna->dyna;
00011  DL_dyna * rdyna=joint->rightDyna->dyna;
00012  switch(drive->getJoint()->getJointType())
00013  {
00014   case SIGEL_Robot::SIG_Joint::tTranslationalJoint :
00015   {
00016    if (f1==0) return;
00017    DL_point lpoint(&joint->leftFix);
00018    DL_vector ldir(&joint->leftDir);
00019    ldir.normalize();
00020    ldir.timesis(f1);
00021 
00022    DL_point rpoint(&joint->rightFix);
00023    DL_vector rdir(&joint->rightDir);
00024    rdir.normalize();
00025    rdir.timesis(-f1);
00026    
00027    ldyna->applyforce(&lpoint,ldyna,&ldir);
00028    rdyna->applyforce(&rpoint,rdyna,&rdir);
00029    break;
00030   };
00031   case SIGEL_Robot::SIG_Joint::tRotationalJoint :
00032   {
00033    if (f1==0) return;
00034    
00035    DL_point lpoint(&joint->leftFix);
00036    DL_vector lup(&joint->leftUp);
00037    lup.normalize();
00038    lpoint.plusis(&lup);
00039    DL_point ldirp(&joint->leftFixB);
00040    DL_vector ldir;
00041    ldirp.minus(&lpoint,&ldir);
00042    ldir.normalize();
00043    DL_vector lfdir;
00044    ldir.crossprod(&lup,&lfdir);
00045    lfdir.timesis(f1);
00046    
00047    DL_point rpoint(&joint->rightFix);
00048    DL_vector rup(&joint->rightUp);
00049    rup.normalize();
00050    rpoint.plusis(&rup);
00051    DL_point rdirp(&joint->rightFixB);
00052    DL_vector rdir;
00053    rdirp.minus(&rpoint,&rdir);
00054    rdir.normalize();
00055    DL_vector rfdir;
00056    rdir.crossprod(&rup,&rfdir);
00057    rfdir.timesis(-f1);
00058    
00059    ldyna->applyforce(&lpoint,ldyna,&lfdir);
00060    rdyna->applyforce(&rpoint,rdyna,&rfdir);
00061    break;
00062   };
00063   case SIGEL_Robot::SIG_Joint::tCylindricalJoint :
00064   {
00065    if ((f1==0)&&(f2==0)) return;
00066    {
00067     DL_point lpoint(&joint->leftFix);
00068     DL_vector ldir(&joint->leftDir);
00069     ldir.normalize();
00070     ldir.timesis(f1); 
00071  
00072     DL_point rpoint(&joint->rightFix);
00073     DL_vector rdir(&joint->rightDir);
00074     rdir.normalize();
00075     rdir.timesis(-f1);
00076     
00077     ldyna->applyforce(&lpoint,ldyna,&ldir);
00078     rdyna->applyforce(&rpoint,rdyna,&rdir);
00079    };
00080    {
00081     DL_point lpoint(&joint->leftFix);
00082     DL_vector lup(&joint->leftUp);
00083     lup.normalize();
00084     lpoint.plusis(&lup);
00085     DL_point ldirp(&joint->leftFixB);
00086     DL_vector ldir;
00087     ldirp.minus(&lpoint,&ldir);
00088     ldir.normalize();
00089     DL_vector lfdir;
00090     ldir.crossprod(&lup,&lfdir);
00091     lfdir.timesis(f2);
00092    
00093     DL_point rpoint(&joint->rightFix);
00094     DL_vector rup(&joint->rightUp);
00095     rup.normalize();
00096     rpoint.plusis(&rup);
00097     DL_point rdirp(&joint->rightFixB);
00098     DL_vector rdir;
00099     rdirp.minus(&rpoint,&rdir);
00100     rdir.normalize();
00101     DL_vector rfdir;
00102     rdir.crossprod(&rup,&rfdir);
00103     rfdir.timesis(-f2);
00104    
00105     ldyna->applyforce(&lpoint,ldyna,&lfdir);
00106     rdyna->applyforce(&rpoint,rdyna,&rfdir);
00107    };
00108    break;
00109   };
00110  }; 
00111 };
00112 

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