-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmain2.m
161 lines (108 loc) · 3.8 KB
/
main2.m
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
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
%Used for testing contorl algoirthms in simulation. Change flags INV, RR,
%JT to select a given control alorithm. Goal and Start are hard coded for
%quick testing
clc;
clear;
steps = 10;
ur5 = ur5_interface();
if abs((norm(ur5.get_current_joints()) - norm([0;-1.57;0;-1.57;0;0]))) <= 1e-3
disp('UR5 is already at the home configuration')
else
disp('Moving to home loaction...')
ur5.move_joints([0;-1.57;0;-1.57;0;0],5)
pause(5)
disp('At home location')
end
disp('Moved the robot to start loaction and then press enter')
w = waitforbuttonpress;
if w == 0
start_q = [-0.63, -0.94, 1.76, -2.14, -1.63, -0.19]';
start_location = ur5FwdKin(start_q);
disp('Start location recorded')
end
disp('Move the robot to target location and then press enter')
w = waitforbuttonpress;
if w == 0
target_q = [1.26, -0.94, 1.76, -2.14, -1.63, -0.19]';
target_location = ur5FwdKin(target_q);
disp('Target location recorded')
end
disp('Moving back to home position')
ur5.move_joints([0;-1.57;0;-1.57;0;0],5);
pause(5)
disp('At home location')
s = [start_location(1,4), start_location(2,4)];
t = [target_location(1,4), target_location(2,4)];
points = intermediatePoints(s,t);
start_location1 = start_location;
target_location1 = start_location;
target_location1(1,4) = points(1,3);
target_location1(2,4) = points(1,4);
start_location2 = target_location1;
target_location2 = start_location;
target_location2(1,4) = points(2,3);
target_location2(2,4) = points(2,4);
target_location2(3,4) = target_location(3,4);
start_location3 = target_location2;
target_location3 = start_location;
target_location3(1,4) = points(3,3);
target_location3(2,4) = points(3,4);
q_start_1 = start_q;
[result, q_start_2, ind] = optimalJointConfig(ur5, ur5InvKin_wrap(start_location2));
[result, q_start_3, ind] = optimalJointConfig(ur5, ur5InvKin_wrap(start_location3));
[~, q_goal_1, ind] = optimalJointConfig(ur5,ur5InvKin_wrap(target_location1));
[result, q_goal_2, ind] = optimalJointConfig(ur5,ur5InvKin_wrap(target_location2));
q_goal_3 = target_q;
%Source of large rot error
INV = false;
RR = false;
JT = true;
%---------------- Traj A using InvKin ----------------
%
if INV
disp("Entering Control Loop for InvKin Control : Traj A")
disp("Drawing Line Segment 1");
[result1, error1] = ur5InvKcontrol(q_start_1, q_goal_1, ur5, steps);
disp("Drawing Line Segement 2");
[result2, error2] = ur5InvKcontrol(q_start_2, q_goal_2, ur5, steps);
disp("Drawing Line Segement 3");
[result3, error3] = ur5InvKcontrol(q_start_3, q_goal_3, ur5, steps);
pause(2)
disp('Moving back to home position')
ur5.move_joints([0;-1.57;0;-1.57;0;0],5);
pause(5)
disp('At home location')
end
%----------------Traj A using RR control---------------
if RR
disp("Entering Control Loop for RR Control : Traj A")
disp("Drawing Line Segment 1");
while(ur5RRcontrol(q_start_1, q_goal_1, ur5, 1) > 0.005)
disp("Moving")
end
disp("Drawing Line Segement 2");
while(ur5RRcontrol(q_start_2, q_goal_2, ur5, 1) > 0.005)
disp("Moving")
end
disp("Drawing Line Segement 3");
while(ur5RRcontrol(q_start_3, q_goal_3, ur5, 1) > 0.005)
disp("Moving")
end
pause(2)
disp('Moving back to home position')
ur5.move_joints([0;-1.57;0;-1.57;0;0],5);
pause(5)
disp('At home location')
end
%---------------Generate traj B --------------- using start_location 1 =
%target Location 1
if(JT)
disp("Entering Control Loop for JT Control : Traj B")
disp("Drawing Line Segment 1");
ur5JTcontrol(q_start_1, q_goal_3, ur5, 1);
pause(2);
disp('Moving back to home position')
ur5.move_joints([0;-1.57;0;-1.57;0;0],5);
pause(5);
disp('At home location')
end