Katana6M180.cpp
Go to the documentation of this file.
1 #include "gnugraph.h"
2 #include "quaternion.h"
3 #include "robot.h"
4 #include "utils.h"
5 
6 #ifdef use_namespace
7 using namespace ROBOOP;
8 #endif
9 void homogen_demo(void);
10 
11 int main(void)
12 {
13  cout << "=====================================================\n";
14  cout << " Katana6M180 \n";
15  cout << " DEMO program \n";
16  cout << "=====================================================\n";
17  cout << "\n";
18 
19  homogen_demo();
20  //kinematics_demo();
21  //dynamics_demo();
22 
23  return(0);
24 }
25 // 1 sigma joint type (revolute=0, prismatic=1)
26 // 2 theta Denavit-Hartenberg parameter
27 // 3 d Denavit-Hartenberg parameter
28 // 4 a Denavit-Hartenberg parameter
29 // 5 alpha Denavit-Hartenberg parameter
30 // 6 theta_{min} minimum value of joint variable
31 // 7 theta_{max} maximum value of joint variable
32 // 8 theta_{off} joint offset
33 // 9 m mass of the link
34 // 10 c_x center of mass along axis $x$
35 // 11 c_y center of mass along axis $y$
36 // 12 c_z center of mass along axis $z$
37 // 13 I_{xx} element $xx$ of the inertia tensor matrix
38 // 14 I_{xy} element $xy$ of the inertia tensor matrix
39 // 15 I_{xz} element $xz$ of the inertia tensor matrix
40 // 16 I_{yy} element $yy$ of the inertia tensor matrix
41 // 17 I_{yz} element $yz$ of the inertia tensor matrix
42 // 18 I_{zz} element $zz$ of the inertia tensor matrix
43 // 19 I_m motor rotor inertia
44 // 20 Gr motor gear ratio
45 // 21 B motor viscous friction coefficient
46 // 22 C_f motor Coulomb friction coefficient
47 // 23 immobile flag for the kinematics and inverse kinematics
48 // (if true joint is locked, if false joint is free)
49 
50 // const Real Katana180_data[] =
51 // {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
52 // 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,
53 // 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,
54 // 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,
55 // 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,
56 // 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};
57 
59  {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
60  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,
61  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,
62  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,
63  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,
64  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};
65 
66 void homogen_demo(void)
67 {
68  Matrix initrob(6,23), Tobj;
69  ColumnVector qs, qr;
70  int dof = 0;
71 
72  cout << "\n";
73  cout << "=====================================================\n";
74  cout << "Katana 6M180 kinematics\n";
75  cout << "=====================================================\n";
76  initrob << Katana180_data;
77  mRobot robot = mRobot(initrob);
78  dof = robot.get_dof();
79 
80  cout << "Robot D-H parameters\n";
81  cout << " type theta d a alpha\n";
82  cout << setw(7) << setprecision(3) << initrob.SubMatrix(1,dof,1,5);
83  cout << "\n";
84 
85  cout << "DOF = " << dof;
86  cout << "\n";
87 
88  cout << "Robot joints variables\n";
89  cout << setw(7) << setprecision(3) << robot.get_q();
90  cout << "\n";
91 
92  cout << "Robot position\n";
93  cout << setw(7) << setprecision(3) << robot.kine(); //Gibt homogene Trafomatrix aus
94  cout << "\n";
95 
96  qr = ColumnVector(dof);
97  qr(1)=M_PI/2.0;
98  qr(2)=0.0;
99  qr(3)=0.0;
100  qr(4)=0.0;
101  qr(5)=0.0;
102  robot.set_q(qr);
103  cout << "Robot joints variables\n";
104  cout << setw(7) << setprecision(3) << robot.get_q();
105  cout << "\n";
106 
107  cout << "Robot Pose\n";
108  Matrix Pos=robot.kine();
109  cout << setw(7) << setprecision(3) << Pos;
110  cout << "\n";
111 
112  cout << "x,y,z coordinates\n";
113  cout << Pos(1,4) << "\n" << Pos(2,4) << "\n" << Pos(3,4) << "\n";
114  cout << "\n";
115 
116  Matrix Xobj = robot.kine(); //Aktuelle Position
117  qs = ColumnVector(dof);
118  qs = M_PI/16.0;
119  //robot.set_q(qs); // Ändert Position
120  cout << "Robot inverse kinematics\n";
121  cout << " q start q final q real\n";
122  cout << setw(7) << setprecision(3) << (qs | robot.inv_kin(Xobj) | qr); //Jetztige Winkel | IK der vorherigen Postion | Vorherige Winkel
123  cout << "\n";
124  cout << "\n";
125 
126 }
127 
128 
int get_dof() const
Return dof.
Definition: robot.h:240
const Real Katana180_data[]
Definition: Katana6M180.cpp:58
Robots class definitions.
double Real
Definition: include.h:307
Utility header file.
ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Overload inv_kin function.
Definition: invkine.cpp:617
void set_q(const ColumnVector &q)
Set the joint position vector.
Definition: robot.cpp:1137
Modified DH notation robot class.
Definition: robot.h:389
void homogen_demo(void)
Definition: Katana6M180.cpp:66
The usual rectangular matrix.
Definition: newmat.h:625
Quaternion class.
Header file for graphics definitions.
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
Definition: kinemat.cpp:92
int main(void)
Definition: Katana6M180.cpp:11
Column vector.
Definition: newmat.h:1008
Real get_q(const int i) const
Definition: robot.h:235
Robot robot
Definition: demo.cpp:227


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16