CtrlPolicy.C

Go to the documentation of this file.
00001 /*! @file ArmControl/CtrlPolicy.C  control policy for robot arm */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2005   //
00005 // by the University of Southern California (USC) and the iLab at USC.  //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Chin-Kai Chang <chinkaic@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/ArmControl/CtrlPolicy.C $
00035 // $Id: CtrlPolicy.C 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include "Util/MathFunctions.H"
00039 #include "ArmControl/CtrlPolicy.H"
00040 
00041 CtrlPolicy::CtrlPolicy() :
00042   dt(.01),
00043   alpha_z(4),
00044   alpha_py(1),
00045   beta_z(2),
00046   y(0),
00047   y_vel(0),
00048   z(0),
00049   z_vel(0),
00050   goal(0)
00051 {
00052 }
00053 
00054 
00055 void CtrlPolicy::setCurrentPos(double pos)
00056 {
00057   y = pos;
00058   z = 0;
00059 
00060 }
00061 void CtrlPolicy::setGoalPos(double pos)
00062 {
00063   goal = pos;
00064 }
00065 double CtrlPolicy::getPos(const double y_actual)
00066 {
00067 
00068     //x_acc = alpha_x*(beta_x*(goal-x)-x_vel);
00069     //x_vel += dt*x_acc;
00070     //x += dt*x_vel;
00071 
00072     z_vel = alpha_z*(beta_z*(goal-y)-z);
00073     z+= dt*z_vel;
00074 
00075     //float x_tilde=(x-x_0)/(goal-x_0);
00076     float f = 0;
00077     y_vel= (z+f) + alpha_py*(y_actual-y);
00078     y+= dt*y_vel;
00079     return y;
00080 
00081 
00082  // z+=dt;
00083 
00084  // double minJerk = y_actual + (goal - y_actual)*
00085  //   (10*pow(z,3) - 15*pow(z,4) + 6*pow(z,5));
00086 
00087   //return minJerk;
00088 }
00089 bool CtrlPolicy::moveDone()
00090 {
00091   return (fabs(goal-y)<0.001);
00092 
00093 }
Generated on Sun May 8 08:40:11 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3