ROBOOP, A Robotics Object Oriented Package in C++
dynamics_sim.h
Go to the documentation of this file.
1 /*
2 Copyright (C) 2002-2004 Etienne Lachance
3 
4 This library is free software; you can redistribute it and/or modify
5 it under the terms of the GNU Lesser General Public License as
6 published by the Free Software Foundation; either version 2.1 of the
7 License, or (at your option) any later version.
8 
9 This library is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU Lesser General Public License for more details.
13 
14 You should have received a copy of the GNU Lesser General Public
15 License along with this library; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
17 
18 
19 Report problems and direct all questions to:
20 
21 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
22 
23 -------------------------------------------------------------------------------
24 Revision_history:
25 
26 2005/06/10: Etienne Lachance
27  -The desired joint acceleration was missing in the computed torque method.
28 -------------------------------------------------------------------------------
29 */
30 
31 
32 #ifndef DYNAMICS_SIM_H
33 #define DYNAMICS_SIM_H
34 
40 #include "control_select.h"
41 #include "quaternion.h"
42 #include "trajectory.h"
43 #include "utils.h"
44 
45 #ifdef use_namespace
46 namespace ROBOOP {
47  using namespace NEWMAT;
48 #endif
49 
50 class Robot_basic;
51 
52 
57 class Dynamics
58 {
59 public:
60  Dynamics(Robot_basic *robot_);
61  static Dynamics *Instance();
62  virtual ~Dynamics(){}
63  void set_dof(Robot_basic *robot_);
64  short set_controller(const Control_Select & x);
65  short set_trajectory(const Trajectory_Select & x);
66  ReturnMatrix set_robot_on_first_point_of_splines();
67  void set_time_frame(const int nsteps);
68  void set_final_time(const double tf);
69  void reset_time();
70  void Runge_Kutta4_Real_time();
71  void Runge_Kutta4();
72 
73  virtual void plot(){}
74 
75 // private:
76  ReturnMatrix xdot(const Matrix & xin);
77 
78  bool first_pass_Kutta;
79  int ndof,
80  dof_fix,
81  nsteps;
82  double h,
83  h2,
84  time,
85  to,
86  tf,
87  tf_cont,
88  dt;
89  Matrix k1,
90  k2,
91  k3,
92  k4,
93  x,
94  xd;
95  ColumnVector q,
96  qp,
97  qpp,
98  qd,
99  qpd,
100  qppd,
101  tau,
102  pd,
103  ppd,
104  pppd,
105  wd,
106  wpd;
107  Quaternion quatd;
108  Control_Select control;
109  Trajectory_Select path_select;
110  Robot_basic *robot;
111 
112  static Dynamics *instance;
113 };
114 
115 #ifdef use_namespace
116 }
117 #endif
118 
119 #endif
120