Katana6M180.cpp
Go to the documentation of this file.
00001 #include "gnugraph.h"
00002 #include "quaternion.h"
00003 #include "robot.h"
00004 #include "utils.h"
00005 
00006 #ifdef use_namespace
00007 using namespace ROBOOP;
00008 #endif
00009 void homogen_demo(void);
00010 
00011 int main(void)
00012 {
00013    cout << "=====================================================\n";
00014    cout << " Katana6M180  \n";
00015    cout << " DEMO program \n";
00016    cout << "=====================================================\n";
00017    cout << "\n";
00018 
00019    homogen_demo();
00020    //kinematics_demo();
00021    //dynamics_demo();
00022 
00023    return(0);
00024 }
00025 // 1    sigma           joint type (revolute=0, prismatic=1)
00026 // 2    theta           Denavit-Hartenberg parameter
00027 // 3    d               Denavit-Hartenberg parameter
00028 // 4    a               Denavit-Hartenberg parameter
00029 // 5    alpha           Denavit-Hartenberg parameter
00030 // 6    theta_{min}     minimum value of joint variable
00031 // 7    theta_{max}     maximum value of joint variable
00032 // 8    theta_{off}     joint offset
00033 // 9    m               mass of the link
00034 // 10   c_x             center of mass along axis $x$
00035 // 11   c_y             center of mass along axis $y$
00036 // 12   c_z             center of mass along axis $z$
00037 // 13   I_{xx}          element $xx$ of the inertia tensor matrix
00038 // 14   I_{xy}          element $xy$ of the inertia tensor matrix
00039 // 15   I_{xz}          element $xz$ of the inertia tensor matrix
00040 // 16   I_{yy}          element $yy$ of the inertia tensor matrix
00041 // 17   I_{yz}          element $yz$ of the inertia tensor matrix
00042 // 18   I_{zz}          element $zz$ of the inertia tensor matrix
00043 // 19   I_m             motor rotor inertia
00044 // 20   Gr              motor gear ratio
00045 // 21   B               motor viscous friction coefficient
00046 // 22   C_f             motor Coulomb friction coefficient
00047 // 23   immobile        flag for the kinematics and inverse kinematics
00048 //                      (if true joint is locked, if false joint is free)
00049 
00050 //  const Real Katana180_data[] =
00051 //    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00052 //     0, 0, 0, 0, M_PI/2.0, -31000, 94976-31000, -31000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00053 //     0, 0, 0, 0.19, 0, -31000, 47488-31000, -31000, 0.926,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00054 //     0, 0, 0, 0.139,0 , 31000, 51200+31000, 31000, 0.745,0.69, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00055 //     0, 0, 0.1473, 0, -M_PI/2.0, 31000, 31000+51200, 31000, 0.366,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00056 //     0, 0, 0.166, 0, 0, 31000, 31000+51200, 31000, 0.181,0, 0, 0.08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00057 
00058 const Real Katana180_data[] =
00059   {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00060    0, 0, 0, 0, M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00061    0, 0, 0, 0.19, 0, 0, 0, 0, 0.926,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00062    0, 0, 0, 0.139, 0, 0, 0, 0, 0.745,0.69, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00063    0, 0, 0.1473, 0, -M_PI/2.0, 0, 0, 0, 0.366,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
00064    0, 0, 0.166, 0, 0, 0, 0, 0,0.181,0, 0, 0.08, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
00065 
00066 void homogen_demo(void)
00067 {
00068    Matrix initrob(6,23), Tobj;
00069    ColumnVector qs, qr;
00070    int dof = 0;
00071 
00072    cout << "\n";
00073    cout << "=====================================================\n";
00074    cout << "Katana 6M180 kinematics\n";
00075    cout << "=====================================================\n";
00076    initrob << Katana180_data;
00077    mRobot robot = mRobot(initrob);
00078    dof = robot.get_dof();
00079 
00080    cout << "Robot D-H parameters\n";
00081    cout << "   type     theta      d        a      alpha\n";
00082    cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
00083    cout << "\n";
00084 
00085    cout << "DOF = " << dof;
00086    cout << "\n";
00087 
00088    cout << "Robot joints variables\n";
00089    cout << setw(7) << setprecision(3) << robot.get_q();
00090    cout << "\n";
00091 
00092    cout << "Robot position\n";
00093    cout << setw(7) << setprecision(3) << robot.kine(); //Gibt homogene Trafomatrix aus
00094    cout << "\n";
00095 
00096    qr = ColumnVector(dof);
00097    qr(1)=M_PI/2.0;
00098    qr(2)=0.0;
00099    qr(3)=0.0;
00100    qr(4)=0.0;
00101    qr(5)=0.0;
00102    robot.set_q(qr);
00103    cout << "Robot joints variables\n";
00104    cout << setw(7) << setprecision(3) << robot.get_q();
00105    cout << "\n";
00106 
00107    cout << "Robot Pose\n";
00108    Matrix Pos=robot.kine();
00109    cout << setw(7) << setprecision(3) << Pos;
00110    cout << "\n";
00111 
00112    cout << "x,y,z coordinates\n";
00113    cout << Pos(1,4) << "\n" << Pos(2,4) << "\n" << Pos(3,4) << "\n";
00114    cout << "\n";
00115 
00116    Matrix Xobj = robot.kine(); //Aktuelle Position
00117    qs = ColumnVector(dof);
00118    qs = M_PI/16.0;
00119    //robot.set_q(qs); // Ändert Position
00120    cout << "Robot inverse kinematics\n";
00121    cout << "  q start  q final  q real\n";
00122    cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Xobj) | qr); //Jetztige Winkel | IK der vorherigen Postion | Vorherige Winkel
00123    cout << "\n";
00124    cout << "\n";
00125 
00126 }
00127 
00128 


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