demo_2dof_pd.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 2002-2004 Etienne Lachance
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
23 
24 -------------------------------------------------------------------------------
25 Revision_history:
26 
27 2004/07/01: Etienne Lachance
28  -Added support for newmat's use_namespace #define, using ROBOOP namespace
29 */
40 static const char rcsid[] = "$Id: demo_2dof_pd.cpp,v 1.2 2006/05/16 16:27:43 gourdeau Exp $";
42 
43 #include "gnugraph.h"
44 #include "controller.h"
45 #include "control_select.h"
46 #include "dynamics_sim.h"
47 #include "robot.h"
48 #include "trajectory.h"
49 
50 #ifdef use_namespace
51 using namespace ROBOOP;
52 #endif
53 
63 class New_dynamics: public Dynamics
64 {
65 public:
66  New_dynamics(Robot_basic *robot_);
67  virtual void plot();
68 
73  int i;
74 };
75 
81 {
82  robot = robot_;
83  first_pass_plot = true;
84  i = 1;
85 }
86 
95 {
96  if(first_pass_plot)
97  {
98  xout = Matrix(x.Nrows(),(int)(nsteps*(tf_cont-to)+1)*x.Ncols());
99  xout.SubMatrix(1,x.Nrows(),1,x.Ncols()) = x;
100  tout = RowVector((int)(nsteps*(tf_cont-to)+1));
101  tout(1) = to;
102  i = 0;
103  first_pass_plot = false;
104  }
105 
106  if(robot)
107  {
108  tout(i+1) = time;
109  xout.SubMatrix(1,x.Nrows(),i*x.Ncols()+1,(i+1)*x.Ncols()) = x;
110  i++;
111  }
112 }
113 
114 
115 int main()
116 {
117 
118  Robot robot("conf/rr_dh.conf", "rr_dh");
119 
120  Trajectory_Select path("conf/q_2dof.dat");
121  Control_Select control("conf/pd_2dof.conf");
122 
123  New_dynamics dynamics(&robot);
124 
125  dynamics.set_controller(control);
126  dynamics.set_trajectory(path);
127  dynamics.set_time_frame(500);
128 
129  dynamics.Runge_Kutta4();
130 
131  set_plot2d("Robot joints position", "time (sec)", "q(i) (rad)", "q", DATAPOINTS,
132  dynamics.tout, dynamics.xout, 1, 2);
133 
134  return(0);
135 }
136 
137 
double to
Initial simulation time.
Definition: dynamics_sim.h:85
short set_plot2d(const char *title_graph, const char *x_axis_title, const char *y_axis_title, const char *label, LineType_en enLineType, const Matrix &xdata, const Matrix &ydata, int start_y, int end_y)
Definition: gnugraph.cpp:793
Matrix x
Stated vector obtain in Runge Kutta functions.
Definition: dynamics_sim.h:92
DH notation robot class.
Definition: robot.h:340
void Runge_Kutta4()
Runge Kutta solver.
Header file for trajectory generation class.
Header file for Control_Select class definitions.
int nsteps
Numbers of iterations between.
Definition: dynamics_sim.h:82
int main()
Robots class definitions.
double tf_cont
Final time used in Runge_Kutta4.
Definition: dynamics_sim.h:85
bool first_pass_plot
RowVector tout
Header file for Dynamics definitions.
Trajectory class selection.
Definition: trajectory.h:164
int Nrows() const
Definition: newmat.h:494
double time
Time during simulation.
Definition: dynamics_sim.h:85
New_dynamics(Robot_basic *robot_)
Constructor.
void set_time_frame(const int nsteps)
Set the number of iterations.
Virtual base robot class.
Definition: robot.h:216
The usual rectangular matrix.
Definition: newmat.h:625
Header file for controller class definitions.
short set_trajectory(const Trajectory_Select &x)
Set the path_select variable from the Trajectory_Select reference.
virtual void plot()
Customize plot function.
Robot_basic * robot
int Ncols() const
Definition: newmat.h:495
Header file for graphics definitions.
Select controller class.
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
static const char rcsid[]
RCS/CVS version.
Row vector.
Definition: newmat.h:953
This is an example of customize Dynamics class.
Control_Select control
Instance of Control_Select class.
Definition: dynamics_sim.h:111
short set_controller(const Control_Select &x)
Set the control variable from the Control_Select reference.
Dynamics simulation handling class.
Definition: dynamics_sim.h:60


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16