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
00021
00022
00023 return(0);
00024 }
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
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();
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();
00117 qs = ColumnVector(dof);
00118 qs = M_PI/16.0;
00119
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);
00123 cout << "\n";
00124 cout << "\n";
00125
00126 }
00127
00128