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

SIG_DynaSensor.cpp

00001 #include "SIGEL_Simulation/SIG_DynaSensor.h"
00002 #include <cmath>
00003 
00004 DL_Scalar SIGEL_Simulation::SIG_DynaSensor::senseJoint1()
00005 {
00006  switch (joint->joint->getJointType())
00007  {
00008   case SIGEL_Robot::SIG_Joint::tTranslationalJoint :
00009   {
00010    DL_point lFix,rFix,lFixB;
00011    joint->leftDyna->dyna->to_world(&joint->leftFix,&lFix);  
00012    joint->rightDyna->dyna->to_world(&joint->rightFix,&rFix);  
00013    DL_vector diff;
00014    lFix.minus(&rFix,&diff);
00015    return diff.norm();
00016    break;
00017   }; 
00018   case SIGEL_Robot::SIG_Joint::tRotationalJoint :
00019   {
00020    DL_point lFix,lFixB;
00021    joint->leftDyna->dyna->to_world(&joint->leftFix,&lFix);  
00022    joint->leftDyna->dyna->to_world(&joint->leftFixB,&lFixB);  
00023    DL_vector lUp,rUp,dir;
00024    joint->leftDyna->dyna->to_world(&joint->leftUp,&lUp);
00025    joint->rightDyna->dyna->to_world(&joint->rightUp,&rUp);
00026    lFix.minus(&lFixB,&dir);
00027    dir.normalize();
00028    lUp.normalize();
00029    rUp.normalize();
00030    DL_Scalar prod=lUp.inprod(&rUp);
00031    double pi=4*std::atan(1);
00032    DL_Scalar angle=acos(prod)*180/pi;
00033    DL_vector x;
00034    dir.crossprod(&lUp,&x);
00035    DL_Scalar sp=x.inprod(&rUp);
00036    if (sp>0)
00037     angle=360-angle;
00038    return angle;
00039    break;
00040   }; 
00041   case SIGEL_Robot::SIG_Joint::tCylindricalJoint :
00042   {
00043    DL_point lFix,rFix;
00044    joint->leftDyna->dyna->to_world(&joint->leftFix,&lFix);  
00045    joint->rightDyna->dyna->to_world(&joint->rightFix,&rFix);  
00046    DL_vector diff;
00047    lFix.minus(&rFix,&diff);
00048    return diff.norm();
00049    break;
00050   }; 
00051  };
00052  return 0;
00053 };
00054 
00055 DL_Scalar SIGEL_Simulation::SIG_DynaSensor::senseJoint2()
00056 {
00057  switch (joint->joint->getJointType())
00058  {
00059   case SIGEL_Robot::SIG_Joint::tTranslationalJoint :
00060   {
00061    break;
00062   }; 
00063   case SIGEL_Robot::SIG_Joint::tRotationalJoint :
00064   {
00065    break;
00066   }; 
00067   case SIGEL_Robot::SIG_Joint::tCylindricalJoint :
00068   {
00069    DL_point lFix,lFixB;
00070    joint->leftDyna->dyna->to_world(&joint->leftFix,&lFix);  
00071    joint->leftDyna->dyna->to_world(&joint->leftFixB,&lFixB);  
00072    DL_vector lUp,rUp,dir;
00073    joint->leftDyna->dyna->to_world(&joint->leftUp,&lUp);
00074    joint->rightDyna->dyna->to_world(&joint->rightUp,&rUp);
00075    lFix.minus(&lFixB,&dir);
00076    dir.normalize();
00077    lUp.normalize();
00078    rUp.normalize();
00079    DL_Scalar prod=lUp.inprod(&rUp);
00080    double pi=4*std::atan(1);
00081    DL_Scalar angle=acos(prod)*180/pi;
00082    DL_vector x;
00083    dir.crossprod(&lUp,&x);
00084    DL_Scalar sp=x.inprod(&rUp);
00085    if (sp>0)
00086     angle=360-angle;
00087    return angle;
00088    break;
00089   }; 
00090  };
00091  return 0;
00092 };
00093 

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