Homepage Demos Overview Downloads Tutorials Reference
Credits

dynamics_sim.cpp

Go to the documentation of this file.
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 /*!
00025   @file dynamics_sim.cpp
00026   @brief Basic dynamics simulation class.
00027 */
00028 
00029 #include "dynamics_sim.h"
00030 
00031 #ifdef use_namespace
00032 namespace ROBOOP {
00033   using namespace NEWMAT;
00034 #endif
00035   //! @brief RCS/CVS version.
00036   static const char rcsid[] __UNUSED__ = "$Id: dynamics_sim.cpp,v 1.3 2005/07/26 03:22:09 ejt Exp $";
00037 
00038 Dynamics *Dynamics::instance = 0;
00039 
00040 
00041 /*!
00042   @fn Dynamics::Dynamics(Robot_basic *robot)
00043   @brief Constructor
00044 */
00045 Dynamics::Dynamics(Robot_basic *robot_): robot(robot_)
00046 {
00047   ndof = 1;
00048   dof_fix = 1;
00049   if(robot)
00050     {
00051       ndof = robot->get_dof();
00052       dof_fix = ndof + robot->get_fix();
00053     }
00054 
00055   q = ColumnVector(ndof); 
00056   q = 0;
00057   qp = ColumnVector(ndof);
00058   qp = 0;
00059   qpp = ColumnVector(ndof);
00060   qpp = 0;
00061   qd = ColumnVector(ndof);
00062   qd = 0.0;
00063   qpd = ColumnVector(ndof);
00064   qpd = 0;
00065   tau = ColumnVector(ndof);
00066   tau = 0.0;
00067   pd = ColumnVector(3); pd = 0;
00068   ppd = ColumnVector(3);
00069   ppd = 0;
00070   pppd = ColumnVector(3);
00071   pppd = 0;
00072   wd = ColumnVector(3);
00073   wd = 0;
00074   wpd = ColumnVector(3);
00075   wpd = 0;
00076   nsteps = 50;
00077   to = 0;
00078   tf = 0.1;
00079   dt = ((tf-to)/nsteps)/4.0;
00080   tf_cont = 1;
00081 
00082   first_pass_Kutta = true;
00083   instance = this;
00084 }
00085 
00086 Dynamics *Dynamics::Instance()
00087 /*!
00088   @brief A pointer to Dynamics instance. Pointer is 0 if there 
00089          is no instance (logic done in Constructor).
00090 */
00091 {
00092     return instance;
00093 }
00094 
00095 void Dynamics::set_dof(Robot_basic *robot_)
00096 /*!
00097   @brief Set the degree of freedom.
00098 
00099   Obtain the degree of freedom from Robot_basic pointer. Some vectors
00100   will be resize with new current dof value.
00101 */
00102 {
00103   ndof = 1;
00104   dof_fix = 1;
00105     robot = robot_;
00106   if(robot)
00107     {
00108       ndof = robot->get_dof();
00109       dof_fix = ndof + robot->get_fix();
00110     }
00111   q = ColumnVector(ndof); 
00112   q = 0;
00113   qp = ColumnVector(ndof);
00114   qp = 0;
00115   qpp = ColumnVector(ndof);
00116   qpp = 0;
00117   qd = ColumnVector(ndof);
00118   qd = 0.0;
00119   qpd = ColumnVector(ndof);
00120   qpd = 0;
00121   tau = ColumnVector(ndof);
00122   tau = 0.0;
00123   
00124   first_pass_Kutta = true;
00125 }
00126 
00127 void Dynamics::set_time_frame(const int nsteps_)
00128 //! @brief Set the number of iterations.
00129 { 
00130     nsteps = nsteps_; 
00131 }
00132 
00133 void Dynamics::set_final_time(const double tf)
00134 //! @brief Set the file time.
00135 { 
00136     tf_cont = tf; 
00137 }
00138 
00139 void Dynamics::reset_time()
00140 //! @brief Set the simulation time to the inital time.
00141 {
00142     first_pass_Kutta = true;
00143 }
00144 
00145 short Dynamics::set_controller(const Control_Select & control_)
00146 //! @brief Set the control variable from the Control_Select reference.
00147 {
00148     control = control_;
00149     if(ndof != control.get_dof())
00150       {
00151   control.type = NONE;
00152   cerr << "Dynamics::set_controller: mismatch degree of freedom between robot and controller." << endl;
00153   return -1;
00154       }
00155     return 0;
00156 }
00157 
00158 short Dynamics::set_trajectory(const Trajectory_Select & path_select_)
00159 //! @brief Set the path_select variable from the Trajectory_Select reference.
00160 {
00161     path_select = path_select_;
00162 
00163     if (control.space_type != path_select.type)
00164       {
00165   control.type = NONE;
00166   path_select.type = NONE;
00167   cerr << "Dynamics::set_trajectory: controller space and trajectory space are not compatible.\n" 
00168        << "                          Both should be in joint space or in cartesian space." << endl;
00169   return -1;
00170       }
00171     return 0;
00172 }
00173 
00174 ReturnMatrix Dynamics::set_robot_on_first_point_of_splines()
00175 /*!
00176   @brief Set the robot on first point of trajectory.
00177 
00178   Assigned the robot joints position to the first point of the trajectory 
00179   if the latter is expressed in joint space, or assigned the robot joints
00180   position via inverse kinematics if the trajectory is expressed in 
00181   cartesian space.
00182   The function return a message on the console if the format of the 
00183   trajectory file is incorrect.
00184 */
00185 {
00186   ColumnVector qs(2*ndof);
00187   
00188   if(path_select.type == JOINT_SPACE)
00189     {
00190       if(path_select.path.p_pdot(0.0, qd, qpd))
00191   cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid joint space path." << endl;
00192       else
00193   {
00194     tf_cont = path_select.path.get_final_time();
00195     robot->set_q(qd);
00196     qs.SubMatrix(1,ndof,1,1) = qd;
00197     qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00198     qs.Release();
00199     return qs;
00200   }
00201     }
00202   else if(path_select.type == CARTESIAN_SPACE) // && quaternion active
00203     {
00204       if( (path_select.path.p_pdot_pddot(0.0, pd, ppd, pppd) == 0) && 
00205     (path_select.path_quat.quat_w(0.0, quatd, wd) == 0) )
00206   {
00207     Matrix Tobj(4,4); Tobj = 0;
00208     Tobj.SubMatrix(1,3,1,3) = quatd.R();
00209     Tobj.SubMatrix(1,3,4,4) = pd;
00210     Tobj(4,4) = 1;
00211     bool converge;
00212     robot->inv_kin(Tobj, 0, converge);
00213     
00214     if(converge)
00215       {
00216         tf_cont = path_select.path.get_final_time();
00217         q = robot->get_q();
00218         qs.SubMatrix(1,ndof,1,1) = q;
00219         qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00220         
00221         qs.Release();
00222         return qs;
00223       }
00224   }
00225       else
00226   cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid cartesian path." << endl;
00227     }
00228   
00229   q = robot->get_q();
00230   qs.SubMatrix(1,ndof,1,1) = q;
00231   qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00232   qs.Release();
00233   return qs;
00234 }
00235 
00236 ReturnMatrix Dynamics::xdot(const Matrix & x)
00237 /*!
00238   @brief Obtain state derivative.
00239   @param x: state vector (joint speed and joint velocity).
00240 
00241   The controller torque is applied if any controller has been
00242   selected, then the joint acceleration is obtained.
00243 */
00244 {
00245    q = x.SubMatrix(1,ndof,1,1);        // joint position from state vecteur
00246    qp = x.SubMatrix(ndof+1,2*ndof,1,1);// " "   velocity    "           "
00247 
00248    if(robot)
00249    {
00250        switch (control.type)
00251        {
00252      case PD:
00253          if(path_select.type == JOINT_SPACE)
00254          {
00255        if(path_select.path.p_pdot(time, qd, qpd))
00256        {
00257            xd = qp & qpp;          
00258            xd.Release(); 
00259            return xd;
00260        }
00261          }
00262          else if(path_select.type == CARTESIAN_SPACE)
00263        cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00264 
00265          tau = control.pd.torque_cmd(*robot, qd, qpd);
00266 
00267          break;
00268 
00269      case CTM:
00270          if(path_select.type == JOINT_SPACE)
00271          {
00272        if(path_select.path.p_pdot(time, qd, qpd))
00273        {
00274            xd = qp & qpp;          
00275            xd.Release(); 
00276            return xd;
00277        }
00278          }
00279          else if(path_select.type == CARTESIAN_SPACE)
00280        cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00281 
00282          tau = control.ctm.torque_cmd(*robot, qd, qpd);
00283 
00284          break;
00285 
00286      case RRA:
00287          if(path_select.type == CARTESIAN_SPACE)
00288          {
00289        if (path_select.path.p_pdot_pddot(time, pd, ppd, pppd) ||
00290            path_select.path_quat.quat_w(time, quatd, wd)) 
00291        {
00292            xd = qp & qpp;
00293            xd.Release();
00294            return xd;
00295        }
00296          }
00297          else if(path_select.type == JOINT_SPACE)
00298        cerr << "Dynamics::xdot: Joint Space path can not be used with RRA controller." << endl;
00299 
00300          tau = control.rra.torque_cmd(*robot, pppd, ppd, pd, wpd, wd, quatd, dof_fix, dt);
00301          break;
00302      default:
00303          tau = 0;
00304        }
00305        qpp = robot->acceleration(q, qp, tau);
00306    }
00307 
00308    plot();
00309 
00310    xd = qp & qpp;  // state derivative
00311 
00312    xd.Release(); 
00313    return xd;
00314 }
00315 
00316 void Dynamics::Runge_Kutta4_Real_time()
00317 //! @brief Runge Kutta solver for real time.
00318 {
00319     if(first_pass_Kutta)
00320     {
00321   h = (tf-to)/nsteps;
00322   h2 = h/2.0;
00323   time = to;
00324   x = set_robot_on_first_point_of_splines();
00325   first_pass_Kutta = false;
00326   return;
00327     }
00328 
00329     k1 = xdot(x) * h;
00330     time += h2;
00331     k2 = xdot(x+k1*0.5)*h;
00332     k3 = xdot(x+k2*0.5)*h;
00333     time += h2;
00334     k4 = xdot(x+k3)*h;
00335     x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00336 }
00337 
00338 void Dynamics::Runge_Kutta4()
00339 //! @brief Runge Kutta solver.
00340 {
00341    x = set_robot_on_first_point_of_splines();
00342    h = (tf_cont - to)/nsteps;
00343    h2 = h/2.0;
00344    time = to;
00345 
00346    for (int i = 1; i <= nsteps; i++) {
00347            k1 = xdot(x) * h;
00348       k2 = xdot(x+k1*0.5)*h;
00349       k3 = xdot(x+k2*0.5)*h;
00350       k4 = xdot(x+k3)*h;
00351       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00352       time += h;
00353    }
00354 }
00355 
00356 #ifdef use_namespace
00357 }
00358 #endif
00359 
00360 
00361 
00362 
00363 
00364 
00365 
00366 
00367 
00368 
00369 

ROBOOP v1.21a
Generated Tue Aug 16 16:32:14 2005 by Doxygen 1.4.4