-
Notifications
You must be signed in to change notification settings - Fork 1
/
QuadrupedInverseKinematic.cpp
114 lines (98 loc) · 2.65 KB
/
QuadrupedInverseKinematic.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
/*
*author:Technician13
*date:2020.4.10
*/
#include"QuadrupedInverseKinematic.h"
#include<iostream>
#include<cmath>
QuadrupedInverseKinematic::QuadrupedInverseKinematic()
{
std::cout<<"Please input parameters:";
std::cout<<std::endl<<"X-axis: ";
std::cin>>pos_x;
std::cout<<std::endl<<"Y-axis: ";
std::cin>>pos_y;
std::cout<<std::endl<<"Z-axis: ";
std::cin>>pos_z;
std::cout<<std::endl<<"The length of the first link: ";
std::cin>>h;
std::cout<<std::endl<<"The length of the second link: ";
std::cin>>hu;
std::cout<<std::endl<<"The length of the third link: ";
std::cin>>hl;
}
QuadrupedInverseKinematic::~QuadrupedInverseKinematic()
{
std::cout<<"The result is: "<<std::endl
<<"LEFT:"<<std::endl
<<"The first joint angle rotation angle gamma: "
<<L_gamma<<std::endl
<<"The second joint angle rotation angle alpha: "
<<alpha<<std::endl
<<"The third joint angle rotation angle beta: "
<<beta<<std::endl
<<"RIGHT:"<<std::endl
<<"The first joint angle rotation angle gamma: "
<<R_gamma<<std::endl
<<"The second joint angle rotation angle alpha: "
<<alpha<<std::endl
<<"The third joint angle rotation angle beta: "
<<beta<<std::endl;
}
double QuadrupedInverseKinematic::calc_dyz()
{
dyz = sqrt(pos_y*pos_y+pos_z*pos_z);
std::cout<<dyz<<std::endl;
return dyz;
}
double QuadrupedInverseKinematic::calc_lyz()
{
lyz = sqrt(dyz*dyz-h*h);
std::cout<<lyz<<std::endl;
return lyz;
}
float QuadrupedInverseKinematic::calc_R_gamma()
{
float gamma_1, gamma_2;
gamma_1=atan(pos_y/pos_z);
gamma_2=atan(h/lyz);
R_gamma = gamma_1 - gamma_2;
std::cout<<R_gamma<<std::endl;
return R_gamma;
}
float QuadrupedInverseKinematic::calc_L_gamma()
{
float gamma_1, gamma_2;
gamma_1=atan(pos_y/pos_z);
gamma_2=atan(h/lyz);
L_gamma = gamma_1 + gamma_2;
std::cout<<L_gamma<<std::endl;
return L_gamma;
}
double QuadrupedInverseKinematic::calc_lxz()
{
lxz = sqrt(lyz*lyz+pos_x*pos_x);
std::cout<<lxz<<std::endl;
return lxz;
}
double QuadrupedInverseKinematic::calc_n()
{
n = (lxz*lxz-hl*hl-hu*hu)/(2*hu);
std::cout<<n<<std::endl;
return n;
}
float QuadrupedInverseKinematic::calc_beta()
{
beta = acos(n/hl);
std::cout<<beta<<std::endl;
return beta;
}
float QuadrupedInverseKinematic::calc_alpha()
{
float alpha_1, alpha_2;
alpha_1 = atan(pos_x/lyz);
alpha_2 = -acos((hu+n)/lxz);
alpha = alpha_1 + alpha_2;
std::cout<<alpha<<std::endl;
return alpha;
}