OrocosComponentLibrary: kuka361InvDynnf.hpp Source File

00001 
00002 #include <vector>
00003 #include "math.h"
00004 //namespace Orocos
00005 //{
00006     using namespace std;
00007 
00008 
00012     class kuka361InvDynnf {
00013     
00014     
00015     public:
00016         kuka361InvDynnf(double dqm_);
00017         ~kuka361InvDynnf(){};
00018     
00029         vector<double> invdyn361(vector<double> &q, vector<double> &dq, vector<double> &dq_des, vector<double> &ddq, vector<double> &torque_scale, vector<double> &torque_offset);
00030 
00031     private:
00032         //Joint acceleration calculated by fwdyn361()
00033         vector<double> _tau;
00034 
00035         //Parameters of the kuka 361 forward dynamic model
00036         double dqm, D13, g1, r3, l, r;
00037         double _y_ls[26];
00038         double _y_ls2[12];
00039     
00040         //doubles necessary to calculate the matrix multiplication in an efficient way in fwdyn361() 
00041         double t1, t3, t4, t6, t8;
00042         double t12, t16, t17, t18, t30, t31, t32, t33, t36, t37, t38, t39, t40, t43, t49;
00043         double t52, t53, t57, t71, t87, tau1, t88, t89, t90, t94, t99;
00044         double t105, t106, t110, t111, t114, t115, t119, t122, t123, t126, t127, t134, t137, t150, t155;
00045         double t163, tau2, tau3, tau4, tau5, tau6;
00046         double mt1, mt2, mt3, mt4, mt10, mt11, mt12, mt13, mt14, mt15, mt16, mt17, mt19, mt20, mt21, mt36, mt37, mt38, mt40, mt45;
00047         double tauM[6];
00048         double M[36];
00049     };
00050 //}//namespace Orocos