demo_2dof_pd.cpp
Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 2002-2004  Etienne Lachance
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00023 
00024 -------------------------------------------------------------------------------
00025 Revision_history:
00026 
00027 2004/07/01: Etienne Lachance
00028     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00029 */
00040 
00041 static const char rcsid[] = "$Id: demo_2dof_pd.cpp,v 1.2 2006/05/16 16:27:43 gourdeau Exp $";
00042 
00043 #include "gnugraph.h"
00044 #include "controller.h"
00045 #include "control_select.h"
00046 #include "dynamics_sim.h"
00047 #include "robot.h"
00048 #include "trajectory.h"
00049 
00050 #ifdef use_namespace
00051 using namespace ROBOOP;
00052 #endif
00053 
00063 class New_dynamics: public Dynamics
00064 {
00065 public:
00066   New_dynamics(Robot_basic *robot_);
00067   virtual void plot();
00068 
00069   Robot_basic *robot;    
00070   bool first_pass_plot;  
00071   RowVector tout;        
00072   Matrix xout;           
00073   int i;                 
00074 };
00075 
00080 New_dynamics::New_dynamics(Robot_basic *robot_): Dynamics(robot_)
00081 {
00082   robot = robot_;
00083   first_pass_plot = true;
00084   i = 1;
00085 }
00086 
00094 void New_dynamics::plot()
00095 {
00096   if(first_pass_plot)
00097     {
00098       xout = Matrix(x.Nrows(),(int)(nsteps*(tf_cont-to)+1)*x.Ncols());
00099       xout.SubMatrix(1,x.Nrows(),1,x.Ncols()) = x;
00100       tout = RowVector((int)(nsteps*(tf_cont-to)+1));
00101       tout(1) = to;
00102       i = 0;
00103       first_pass_plot = false;
00104     }
00105 
00106   if(robot)
00107     {
00108       tout(i+1) = time;
00109       xout.SubMatrix(1,x.Nrows(),i*x.Ncols()+1,(i+1)*x.Ncols()) = x;
00110       i++;
00111     }
00112 }
00113 
00114 
00115 int main()
00116 {
00117   
00118   Robot robot("conf/rr_dh.conf", "rr_dh");
00119 
00120   Trajectory_Select path("conf/q_2dof.dat");
00121   Control_Select control("conf/pd_2dof.conf");
00122 
00123   New_dynamics dynamics(&robot);
00124 
00125   dynamics.set_controller(control);
00126   dynamics.set_trajectory(path);
00127   dynamics.set_time_frame(500);
00128 
00129   dynamics.Runge_Kutta4();
00130 
00131   set_plot2d("Robot joints position", "time (sec)", "q(i) (rad)", "q", DATAPOINTS,
00132              dynamics.tout, dynamics.xout, 1, 2);
00133  
00134    return(0);
00135 }
00136 
00137 


kni
Author(s): Martin Günther
autogenerated on Mon Aug 14 2017 02:44:12