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 Revision_history:
00025 
00026 2005/06/10: Etienne Lachance
00027     -The desired joint acceleration was missing in the computed torque method.
00028 
00029 2006/05/19: Richard Gourdeau
00030     -Fix set_q, setq_p bug in xdot (reported by Philip Gruebele)
00031 -------------------------------------------------------------------------------
00032 */
00033 
00039 
00040 static const char rcsid[] = "$Id: dynamics_sim.cpp,v 1.6 2006/05/19 21:05:57 gourdeau Exp $";
00041 
00042 #include "dynamics_sim.h"
00043 #include "robot.h"
00044 
00045 #ifdef use_namespace
00046 namespace ROBOOP {
00047   using namespace NEWMAT;
00048 #endif
00049 
00050 Dynamics *Dynamics::instance = 0;
00051 
00052 
00057 Dynamics::Dynamics(Robot_basic *robot_): robot(robot_)
00058 {
00059   ndof = 1;
00060   dof_fix = 1;
00061   if(robot)
00062     {
00063       ndof = robot->get_dof();
00064       dof_fix = ndof + robot->get_fix();
00065     }
00066 
00067   q = ColumnVector(ndof); 
00068   q = 0;
00069   qp = ColumnVector(ndof);
00070   qp = 0;
00071   qpp = ColumnVector(ndof);
00072   qpp = 0;
00073   qd = ColumnVector(ndof);
00074   qd = 0.0;
00075   qpd = ColumnVector(ndof);
00076   qpd = 0;
00077   qppd = ColumnVector(ndof);
00078   qppd = 0;
00079   tau = ColumnVector(ndof);
00080   tau = 0.0;
00081   pd = ColumnVector(3); pd = 0;
00082   ppd = ColumnVector(3);
00083   ppd = 0;
00084   pppd = ColumnVector(3);
00085   pppd = 0;
00086   wd = ColumnVector(3);
00087   wd = 0;
00088   wpd = ColumnVector(3);
00089   wpd = 0;
00090   nsteps = 50;
00091   to = 0;
00092   tf = 0.1;
00093   dt = ((tf-to)/nsteps)/4.0;
00094   tf_cont = 1;
00095 
00096   first_pass_Kutta = true;
00097   instance = this;
00098 }
00099 
00100 Dynamics *Dynamics::Instance()
00105 {
00106     return instance;
00107 }
00108 
00109 void Dynamics::set_dof(Robot_basic *robot_)
00116 {
00117   ndof = 1;
00118   dof_fix = 1;
00119     robot = robot_;
00120   if(robot)
00121     {
00122       ndof = robot->get_dof();
00123       dof_fix = ndof + robot->get_fix();
00124     }
00125   q = ColumnVector(ndof); 
00126   q = 0;
00127   qp = ColumnVector(ndof);
00128   qp = 0;
00129   qpp = ColumnVector(ndof);
00130   qpp = 0;
00131   qd = ColumnVector(ndof);
00132   qd = 0.0;
00133   qpd = ColumnVector(ndof);
00134   qpd = 0;
00135   qppd = ColumnVector(ndof);
00136   qppd = 0;
00137   tau = ColumnVector(ndof);
00138   tau = 0.0;
00139   
00140   first_pass_Kutta = true;
00141 }
00142 
00143 void Dynamics::set_time_frame(const int nsteps_)
00145 { 
00146     nsteps = nsteps_; 
00147 }
00148 
00149 void Dynamics::set_final_time(const double tf)
00151 { 
00152     tf_cont = tf; 
00153 }
00154 
00155 void Dynamics::reset_time()
00157 {
00158     first_pass_Kutta = true;
00159 }
00160 
00161 short Dynamics::set_controller(const Control_Select & control_)
00163 {
00164     control = control_;
00165     if( (ndof != control.get_dof()) && (control.type != NONE) )
00166       {
00167         control.type = NONE;
00168         cerr << "Dynamics::set_controller: mismatch degree of freedom between robot and controller." << endl;
00169         return -1;
00170       }
00171     return 0;
00172 }
00173 
00174 short Dynamics::set_trajectory(const Trajectory_Select & path_select_)
00176 {
00177     path_select = path_select_;
00178 
00179     if (control.space_type != path_select.type)
00180       {
00181         control.type = NONE;
00182         path_select.type = NONE;
00183         cerr << "Dynamics::set_trajectory: controller space and trajectory space are not compatible.\n" 
00184              << "                          Both should be in joint space or in cartesian space." << endl;
00185         return -1;
00186       }
00187     return 0;
00188 }
00189 
00190 ReturnMatrix Dynamics::set_robot_on_first_point_of_splines()
00201 {
00202   ColumnVector qs(2*ndof);
00203   
00204   if(path_select.type == JOINT_SPACE)
00205     {
00206       if(path_select.path.p_pdot(0.0, qd, qpd))
00207         cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid joint space path." << endl;
00208       else
00209         {
00210           tf_cont = path_select.path.get_final_time();
00211           robot->set_q(qd);
00212           qs.SubMatrix(1,ndof,1,1) = qd;
00213           qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00214           qs.Release();
00215           return qs;
00216         }
00217     }
00218   else if(path_select.type == CARTESIAN_SPACE) // && quaternion active
00219     {
00220       if( (path_select.path.p_pdot_pddot(0.0, pd, ppd, pppd) == 0) && 
00221           (path_select.path_quat.quat_w(0.0, quatd, wd) == 0) )
00222         {
00223           Matrix Tobj(4,4); Tobj = 0;
00224           Tobj.SubMatrix(1,3,1,3) = quatd.R();
00225           Tobj.SubMatrix(1,3,4,4) = pd;
00226           Tobj(4,4) = 1;
00227           bool converge;
00228           robot->inv_kin(Tobj, 0, converge);
00229           
00230           if(converge)
00231             {
00232               tf_cont = path_select.path.get_final_time();
00233               q = robot->get_q();
00234               qs.SubMatrix(1,ndof,1,1) = q;
00235               qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00236               
00237               qs.Release();
00238               return qs;
00239             }
00240         }
00241       else
00242         cerr << "Dynamics::set_robot_on_first_point_of_spines: invalid cartesian path." << endl;
00243     }
00244   
00245   q = robot->get_q();
00246   qs.SubMatrix(1,ndof,1,1) = q;
00247   qs.SubMatrix(ndof+1,2*ndof,1,1) = 0;
00248   qs.Release();
00249   return qs;
00250 }
00251 
00252 ReturnMatrix Dynamics::xdot(const Matrix & x)
00260 {
00261    q = x.SubMatrix(1,ndof,1,1);        // joint position from state vecteur
00262    qp = x.SubMatrix(ndof+1,2*ndof,1,1);// " "   velocity    "           "
00263    robot->set_q(q);
00264    robot->set_qp(qp);
00265 
00266    if(robot)
00267    {
00268        switch (control.type)
00269        {
00270            case PD:
00271                if(path_select.type == JOINT_SPACE)
00272                {
00273                    if(path_select.path.p_pdot(time, qd, qpd))
00274                    {
00275                        xd = qp & qpp;                      
00276                        xd.Release(); 
00277                        return xd;
00278                    }
00279                }
00280                else if(path_select.type == CARTESIAN_SPACE)
00281                    cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00282 
00283                tau = control.pd.torque_cmd(*robot, qd, qpd);
00284 
00285                break;
00286 
00287            case CTM:
00288                if(path_select.type == JOINT_SPACE)
00289                {
00290                    if(path_select.path.p_pdot(time, qd, qpd))
00291                    {
00292                        xd = qp & qpp;                      
00293                        xd.Release(); 
00294                        return xd;
00295                    }
00296                }
00297                else if(path_select.type == CARTESIAN_SPACE)
00298                    cerr << "Dynamics::xdot: Cartesian path can not be used with CTM controller." << endl;
00299 
00300                tau = control.ctm.torque_cmd(*robot, qd, qpd, qppd);
00301 
00302                break;
00303 
00304            case RRA:
00305                if(path_select.type == CARTESIAN_SPACE)
00306                {
00307                    if (path_select.path.p_pdot_pddot(time, pd, ppd, pppd) ||
00308                        path_select.path_quat.quat_w(time, quatd, wd)) 
00309                    {
00310                        xd = qp & qpp;
00311                        xd.Release();
00312                        return xd;
00313                    }
00314                }
00315                else if(path_select.type == JOINT_SPACE)
00316                    cerr << "Dynamics::xdot: Joint Space path can not be used with RRA controller." << endl;
00317 
00318                tau = control.rra.torque_cmd(*robot, pppd, ppd, pd, wpd, wd, quatd, dof_fix, dt);
00319                break;
00320            default:
00321                tau = 0;
00322        }
00323        qpp = robot->acceleration(q, qp, tau);
00324    }
00325 
00326    plot();
00327 
00328    xd = qp & qpp;  // state derivative
00329 
00330    xd.Release(); 
00331    return xd;
00332 }
00333 
00334 void Dynamics::Runge_Kutta4_Real_time()
00336 {
00337     if(first_pass_Kutta)
00338     {
00339         h = (tf-to)/nsteps;
00340         h2 = h/2.0;
00341         time = to;
00342         x = set_robot_on_first_point_of_splines();
00343         first_pass_Kutta = false;
00344         return;
00345     }
00346 
00347     k1 = xdot(x) * h;
00348     time += h2;
00349     k2 = xdot(x+k1*0.5)*h;
00350     k3 = xdot(x+k2*0.5)*h;
00351     time += h2;
00352     k4 = xdot(x+k3)*h;
00353     x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00354 }
00355 
00356 void Dynamics::Runge_Kutta4()
00358 {
00359    x = set_robot_on_first_point_of_splines();
00360    h = (tf_cont - to)/nsteps;
00361    h2 = h/2.0;
00362    time = to;
00363 
00364    for (int i = 1; i <= nsteps; i++) {
00365            k1 = xdot(x) * h;
00366       k2 = xdot(x+k1*0.5)*h;
00367       k3 = xdot(x+k2*0.5)*h;
00368       k4 = xdot(x+k3)*h;
00369       x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00370       time += h;
00371    }
00372 }
00373 
00374 #ifdef use_namespace
00375 }
00376 #endif
00377 
00378 
00379 
00380 
00381 
00382 
00383 
00384 
00385 
00386 
00387 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32