dynamics_sim.h

Go to the documentation of this file.

/*
Copyright (C) 2002-2004  Etienne Lachance

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


Report problems and direct all questions to:

email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca

-------------------------------------------------------------------------------
Revision_history:

2005/06/10: Etienne Lachance
    -The desired joint acceleration was missing in the computed torque method.
-------------------------------------------------------------------------------
*/


#ifndef DYNAMICS_SIM_H
#define DYNAMICS_SIM_H


static const char header_dynamics_sim_rcsid[] = "$Id: dynamics_sim.h,v 1.4 2006/05/16 16:11:15 gourdeau Exp $";

#include "control_select.h"
#include "quaternion.h"
#include "trajectory.h"
#include "utils.h"

#ifdef use_namespace
namespace ROBOOP {
  using namespace NEWMAT;
#endif

class Robot_basic;


class Dynamics
{
public:
    Dynamics(Robot_basic *robot_);
    static Dynamics *Instance();
    virtual ~Dynamics(){}
    void set_dof(Robot_basic *robot_);
    short set_controller(const Control_Select & x);
    short set_trajectory(const Trajectory_Select & x);
    ReturnMatrix set_robot_on_first_point_of_splines();
    void set_time_frame(const int nsteps);
    void set_final_time(const double tf);
    void reset_time();
    void Runge_Kutta4_Real_time();
    void Runge_Kutta4();

    virtual void plot(){} 

// private:
    ReturnMatrix xdot(const Matrix & xin);

    bool first_pass_Kutta; 
    int ndof,              
        dof_fix,           
        nsteps;            
    double h,              
           h2,             
           time,           
           to,             
           tf,             
           tf_cont,        
           dt;             
    Matrix k1,             
           k2,             
           k3,             
           k4,             
           x,              
           xd;             
    ColumnVector q,        
                 qp,       
                 qpp,      
                 qd,       
                 qpd,      
                 qppd,     
                 tau,      
                 pd,       
                 ppd,      
                 pppd,     
                 wd,       
                 wpd;      
    Quaternion quatd;      
    Control_Select control;        
    Trajectory_Select path_select; 
    Robot_basic *robot;            

    static Dynamics *instance;     
};

#ifdef use_namespace
}
#endif

#endif