forked from kirillv/cpp-inverse-kinematics-library
/
testApp.cpp
119 lines (93 loc) · 2.75 KB
/
testApp.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#include "CRobot.h"
#include "CConfigLoader.h"
#include "CLineFactory.h"
#include "CAlgoAbstract.h"
#include "CJacobianTranspose.h"
#include "CDumpedLeastSquares.h"
#include "CAlgoFactory.h"
#include <iostream>
#define _TESTDLS
int main()
{
#ifdef _TEST1
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
//Print loaded data
robot.PrintHomogenTransformationMatrix();
robot.PrintFullTransformationMatrix();
//Rotating joint 1 for 90 degrees
robot.RotateJoint(DHINDEX(1),90.0f);
robot.PrintHomogenTransformationMatrix();
robot.PrintFullTransformationMatrix();
#endif
#ifdef _TEST2
CLine3D line;
Vector3f test2;
Vector3f vec_end(20,1,0);
Vector3f vec_start(1,2,3);
float displ = 0.01f;
test2 = line.GetNextPoint(vec_end,vec_start,displ);
while(true)
{
if (test2 == vec_end)
{
break;
}
std::cout<<test2<<std::endl;
test2 = line.GetNextPoint(vec_end,test2,displ);
}
std::cout<<test2<<std::endl;
#endif
#ifdef _TESTJACIBIANTRANSPOSE
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f ,200.0f ,0.0f ,0.0f ,0.0f ,0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(JACOBIANTRANSPOSE,des,robot);
pJpt->SetAdditionalParametr(speccfc);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
#ifdef _TESTJACIBIANPSEUDO
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f , 200.0f , 0.0f , 0.0f , 0.0f , 0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(JACOBIANPSEVDOINVERSE,des,robot);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
#ifdef _TESTDLS
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f , 200.0f , 0.0f , 0.0f , 0.0f , 0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(DUMPEDLEASTSQUARES,des,robot);
pJpt->SetAdditionalParametr(speccfc);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
return 0;
}