$search
00001 /* 00002 Copyright (C) 2002-2004 Etienne Lachance 00003 00004 This library is free software; you can redistribute it and/or modify 00005 it under the terms of the GNU Lesser General Public License as 00006 published by the Free Software Foundation; either version 2.1 of the 00007 License, or (at your option) any later version. 00008 00009 This library is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 GNU Lesser General Public License for more details. 00013 00014 You should have received a copy of the GNU Lesser General Public 00015 License along with this library; if not, write to the Free Software 00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00017 00018 00019 Report problems and direct all questions to: 00020 00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca 00022 00023 ------------------------------------------------------------------------------- 00024 Revision_history: 00025 00026 2005/06/10: Etienne Lachance 00027 -The desired joint acceleration was missing in the computed torque method. 00028 ------------------------------------------------------------------------------- 00029 */ 00030 00031 00032 #ifndef DYNAMICS_SIM_H 00033 #define DYNAMICS_SIM_H 00034 00040 00041 static const char header_dynamics_sim_rcsid[] = "$Id: dynamics_sim.h,v 1.4 2006/05/16 16:11:15 gourdeau Exp $"; 00042 00043 #include "control_select.h" 00044 #include "quaternion.h" 00045 #include "trajectory.h" 00046 #include "utils.h" 00047 00048 #ifdef use_namespace 00049 namespace ROBOOP { 00050 using namespace NEWMAT; 00051 #endif 00052 00053 class Robot_basic; 00054 00055 00060 class Dynamics 00061 { 00062 public: 00063 Dynamics(Robot_basic *robot_); 00064 static Dynamics *Instance(); 00065 virtual ~Dynamics(){} 00066 void set_dof(Robot_basic *robot_); 00067 short set_controller(const Control_Select & x); 00068 short set_trajectory(const Trajectory_Select & x); 00069 ReturnMatrix set_robot_on_first_point_of_splines(); 00070 void set_time_frame(const int nsteps); 00071 void set_final_time(const double tf); 00072 void reset_time(); 00073 void Runge_Kutta4_Real_time(); 00074 void Runge_Kutta4(); 00075 00076 virtual void plot(){} 00077 00078 // private: 00079 ReturnMatrix xdot(const Matrix & xin); 00080 00081 bool first_pass_Kutta; 00082 int ndof, 00083 dof_fix, 00084 nsteps; 00085 double h, 00086 h2, 00087 time, 00088 to, 00089 tf, 00090 tf_cont, 00091 dt; 00092 Matrix k1, 00093 k2, 00094 k3, 00095 k4, 00096 x, 00097 xd; 00098 ColumnVector q, 00099 qp, 00100 qpp, 00101 qd, 00102 qpd, 00103 qppd, 00104 tau, 00105 pd, 00106 ppd, 00107 pppd, 00108 wd, 00109 wpd; 00110 Quaternion quatd; 00111 Control_Select control; 00112 Trajectory_Select path_select; 00113 Robot_basic *robot; 00114 00115 static Dynamics *instance; 00116 }; 00117 00118 #ifdef use_namespace 00119 } 00120 #endif 00121 00122 #endif 00123