00001 /*************************************************************************** 00002 tag: Peter Soetens Mon May 10 19:10:36 CEST 2004 velocityprofile_dirac.cxx 00003 00004 velocityprofile_dirac.cxx - description 00005 ------------------- 00006 begin : Mon May 10 2004 00007 copyright : (C) 2004 Peter Soetens 00008 email : peter.soetens@mech.kuleuven.ac.be 00009 00010 *************************************************************************** 00011 * This library is free software; you can redistribute it and/or * 00012 * modify it under the terms of the GNU Lesser General Public * 00013 * License as published by the Free Software Foundation; either * 00014 * version 2.1 of the License, or (at your option) any later version. * 00015 * * 00016 * This library is distributed in the hope that it will be useful, * 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 00019 * Lesser General Public License for more details. * 00020 * * 00021 * You should have received a copy of the GNU Lesser General Public * 00022 * License along with this library; if not, write to the Free Software * 00023 * Foundation, Inc., 59 Temple Place, * 00024 * Suite 330, Boston, MA 02111-1307 USA * 00025 * * 00026 ***************************************************************************/ 00027 00028 #include "utilities/error.h" 00029 #include "velocityprofile_dirac.hpp" 00030 00031 namespace KDL { 00032 00033 00034 void VelocityProfile_Dirac::SetProfile( 00035 double pos1, 00036 double pos2 00037 ) 00038 { 00039 p1 = pos1; 00040 p2 = pos2; 00041 t = 0; 00042 } 00043 00044 void VelocityProfile_Dirac:: 00045 SetProfileDuration(double pos1,double pos2,double duration) 00046 { 00047 SetProfile(pos1,pos2); 00048 t = duration; 00049 } 00050 00051 double VelocityProfile_Dirac::Duration() const { 00052 return t; 00053 } 00054 00055 double VelocityProfile_Dirac::Pos(double time) const { 00056 if ( t == 0 ) 00057 return time == 0 ? p1 : p2; 00058 else 00059 return p1 + (( p2 - p1)/t)*time; 00060 } 00061 00062 double VelocityProfile_Dirac::Vel(double time) const { 00063 if ( t == 0 ) 00064 { 00065 throw Error_MotionPlanning_Incompatible(); 00066 } 00067 else 00068 if ( 0 < time && time < t ) 00069 return (p2-p1) / t; 00070 return 0; 00071 } 00072 00073 double VelocityProfile_Dirac::Acc(double time) const { 00074 throw Error_MotionPlanning_Incompatible(); 00075 return 0; 00076 } 00077 00078 00079 void VelocityProfile_Dirac::Write(std::ostream& os) const { 00080 os << "DIRACVEL[ ]"; 00081 } 00082 00083 00084 00085 } 00086