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

ROBOOP v1.21a
Generated Tue Oct 19 14:18:25 2004 by Doxygen 1.3.9.1