velocityprofile_rect.cpp
Go to the documentation of this file.00001 /*************************************************************************** 00002 tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 velocityprofile_rect.cxx 00003 00004 velocityprofile_rect.cxx - description 00005 ------------------- 00006 begin : Mon May 10 2004 00007 copyright : (C) 2004 Erwin Aertbelien 00008 email : erwin.aertbelien@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 * \author 00029 * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 00030 * 00031 * \version 00032 * ORO_Geometry V0.2 00033 * 00034 * \par History 00035 * - $log$ 00036 * 00037 * \par Release 00038 * $Id: velocityprofile_rect.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ 00039 * $Name: $ 00040 ****************************************************************************/ 00041 00042 00043 #include "utilities/error.h" 00044 #include "velocityprofile_rect.hpp" 00045 00046 namespace KDL { 00047 00048 00049 void VelocityProfile_Rectangular::SetProfile( 00050 double pos1, 00051 double pos2 00052 ) 00053 { 00054 double diff; 00055 diff = pos2-pos1; // increment per sec. 00056 if ( diff != 0 ) 00057 { 00058 v = (diff>0)?maxvel:-maxvel; 00059 p = pos1; // start pos 00060 d = diff/v; 00061 } 00062 else 00063 { 00064 v = 0; 00065 p = pos1; 00066 d = 0; 00067 } 00068 } 00069 00070 void VelocityProfile_Rectangular::SetMax( double vMax ) 00071 { 00072 maxvel = vMax; 00073 } 00074 00075 00076 void VelocityProfile_Rectangular:: 00077 SetProfileDuration(double pos1,double pos2,double duration) 00078 { 00079 double diff; 00080 diff = pos2-pos1; // increment per sec. 00081 if ( diff != 0 ) 00082 { 00083 v = diff/duration; 00084 if (v > maxvel || duration==0 ) // safety. 00085 v=maxvel; 00086 p = pos1; // start pos 00087 d = diff/v; 00088 } 00089 else 00090 { 00091 v = 0; 00092 p = pos1; 00093 d = duration; 00094 } 00095 } 00096 00097 double VelocityProfile_Rectangular::Duration() const { 00098 return d; 00099 } 00100 00101 double VelocityProfile_Rectangular::Pos(double time) const { 00102 if (time < 0) { 00103 return p; 00104 } else if (time>d) { 00105 return v*d+p; 00106 } else { 00107 return v*time+p; 00108 } 00109 } 00110 00111 double VelocityProfile_Rectangular::Vel(double time) const { 00112 if (time < 0) { 00113 return 0; 00114 } else if (time>d) { 00115 return 0; 00116 } else { 00117 return v; 00118 } 00119 } 00120 00121 double VelocityProfile_Rectangular::Acc(double time) const { 00122 throw Error_MotionPlanning_Incompatible(); 00123 return 0; 00124 } 00125 00126 00127 void VelocityProfile_Rectangular::Write(std::ostream& os) const { 00128 os << "CONSTVEL[" << maxvel << "]"; 00129 } 00130 00131 00132 } 00133