-
Notifications
You must be signed in to change notification settings - Fork 0
/
Kinematics2d.cpp
128 lines (106 loc) · 4.67 KB
/
Kinematics2d.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
120
121
122
123
124
125
126
127
128
#include <cstdlib>
#include <iostream>
#include <math.h>
#define _USE_MATH_DEFINES
#include "Kinematics2d.h"
#include "Utility.h"
using namespace std;
//============================================================================
void Kinematics2d::setInputForward(GeometryArms g) {
cout << "+-------------------------------------+ \n";
cout << "| Selected: \"2D Forward Kinematics\" | \n";
cout << "+-------------------------------------+ \n\n";
if(g.sGeoA.L1 != 0 && g.sGeoA.L2 != 0) {
cout << "+-------------------------------+ \n";
cout << "| Enter theta 1 angle (Degree) | \n";
cout << "+-------------------------------+ \n";
cin >> inF_2d.theta1;
cout << "+-------------------------------+ \n";
cout << "| Enter theta 2 angle (Degree) | \n";
cout << "+-------------------------------+ \n";
cin >> inF_2d.theta2;
cout << "+-------------------------------+ \n";
cout << "| Enter theta 3 angle (Degree) | \n";
cout << "+-------------------------------+ \n";
cin >> inF_2d.theta3;
inF_2d.theta1 = DegreeToRadiant(inF_2d.theta1);
inF_2d.theta2 = DegreeToRadiant(inF_2d.theta2);
inF_2d.theta3 = DegreeToRadiant(inF_2d.theta3);
calculateForwardKinematics(g);
} else {
cout << "You should first set Arm Geometry!\n\n";
}
//system("PAUSE");
//system("cls");
cls(); //clear screen
}
//============================================================================
void Kinematics2d::calculateForwardKinematics(GeometryArms g) {
double x = g.sGeoA.L1 * cos(inF_2d.theta1) + g.sGeoA.L2 * cos(inF_2d.theta1 + inF_2d.theta2);
double y = g.sGeoA.L1 * sin(inF_2d.theta1) + g.sGeoA.L2 * sin(inF_2d.theta1 + inF_2d.theta2);
double alpha = inF_2d.theta1 + inF_2d.theta2 + inF_2d.theta3;
cout << "\n+---------------------------------------------------------------------+ \n\n";
cout << "+--------------------------------------+ \n";
cout << "| Point X : " << x << "\n";
cout << "+--------------------------------------+ \n";
cout << "| Point Y : " << y << "\n";
cout << "+--------------------------------------+ \n";
cout << "| Alpha : " << RadiantToDegree(alpha) << " (Gradi)\n";
cout << "+--------------------------------------+ \n\n";
}
//============================================================================
void Kinematics2d::setInputInverse(GeometryArms g) {
cout << "+-------------------------------------+ \n";
cout << "| Selected: \"2D Inverse Kinematics\" | \n";
cout << "+-------------------------------------+ \n\n";
if(g.sGeoA.L1 != 0 && g.sGeoA.L2 != 0) {
cout << "+-----------------------------+ \n";
cout << "| Enter point X | \n";
cout << "+-----------------------------+ \n";
cin >> inI_2d.x;
cout << "+-----------------------------+ \n";
cout << "| Enter point Y | \n";
cout << "+-----------------------------+ \n";
cin >> inI_2d.y;
cout << "+-----------------------------+ \n";
cout << "| Enter Alpha angle (Degree) | \n";
cout << "+-----------------------------+ \n";
cin >> inI_2d.alpha;
inI_2d.alpha = DegreeToRadiant(inI_2d.alpha);
calculateInverseKinematics(g);
} else {
cout << "You should first set Arm Geometry!\n\n";
}
//system("PAUSE");
//system("cls");
cls(); //clear screen
}
//============================================================================
void Kinematics2d::calculateInverseKinematics(GeometryArms g) {
/* Cacolo Theta 2 */
int direction = 1;
double x_2 = pow(inI_2d.x, 2.0);
double y_2 = pow(inI_2d.y, 2.0);
double l1_2 = pow(g.sGeoA.L1, 2.0);
double l2_2 = pow(g.sGeoA.L2, 2.0);
double tmp2 = (x_2 + y_2 - l1_2 - l2_2)/(2 * g.sGeoA.L1 * g.sGeoA.L2);
double tmp2_2 = pow(tmp2, 2.0);
if(tmp2_2 <= 1) {
double tmp1 = sqrt(1 - tmp2_2) * direction;
double theta2 = atan2(tmp1, tmp2);
/* Calcolo Theta 1 */
double theta1 = (atan2(inI_2d.y, inI_2d.x) - atan2((g.sGeoA.L2 * sin(theta2)), (g.sGeoA.L1 + g.sGeoA.L2 * cos(theta2))));
/* Calcolo Theta 3 */
double theta3 = (inI_2d.alpha - theta1 - theta2);
cout << "\n+---------------------------------------------------------------------+ \n\n";
cout << "+------------------------------------------------------------+ \n";
cout << "| Theta 1 : " << theta1 << " (Radiant) , " << RadiantToDegree(theta1) << " (Degree) \n";
cout << "+------------------------------------------------------------+ \n";
cout << "| Theta 2 : " << theta2 << " (Radiant) , " << RadiantToDegree(theta2) << " (Degree) \n";
cout << "+------------------------------------------------------------+ \n";
cout << "| Theta 3 : " << theta3 << " (Radiant) , " << RadiantToDegree(theta3) << " (Degree) \n";
cout << "+------------------------------------------------------------+ \n\n";
} else {
cout << "\nPoint unreachable !" << endl;
}
}