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