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