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 


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17