-
Notifications
You must be signed in to change notification settings - Fork 5
/
IK_Global.m~
260 lines (227 loc) · 9.84 KB
/
IK_Global.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
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
function [ dtheta_leg, dtheta_arm,theta_leg, theta_arm,it ] = IK_Global( xi_B, xi_Fi, xi_Hi,chain_leg_R, chain_leg_L, chain_arm_R,chain_arm_L, theta_head,t_arm_0,t_leg_0,swing,s )
%Calculate joint velocities from reference twists
%REFERENCE_ANGULAR_MOMENTUM calculate the joint velocities for the arm and
%the legs
% eps_B : velocities of the waist
% eps_Fi : velocities of the feet
% eps_Hi : velocities of the hands
% dtheta_leg : joint velocities of the legs
% dtheta_arm : joint velocities of the arms
%% Declaration of the global variables
global Jac invJac;
global T_global
%%
limits=0;
limit_vel=0;
animation=1;
% Dependency on matlab toolboxes
aerospaceTB=1;
roboticTB=0;
%% For limit checking
if limits==1
global q_lim_leg_R q_lim_leg_L q_lim_arm_R q_lim_arm_L %q_lim_head
global J_arm_R J_arm_L J_leg_R J_leg_L
end
% Position
dp_Fi(:,1)=integrate(xi_Fi(1:3,1),chain_leg_R(1:3,4,end));
dp_Fi(:,2)=integrate(xi_Fi(1:3,2),chain_leg_L(1:3,4,end));
dp_Hi(:,1)=integrate(xi_Hi(1:3,1),chain_arm_R(1:3,4,end));
dp_Hi(:,2)=integrate(xi_Hi(1:3,2),chain_arm_L(1:3,4,end));
dp_B=integrate(xi_B(1:3,1),T_global(1:3,4));
p_Fi(:,1)=chain_leg_R(1:3,4,end);
p_Fi(:,2)=chain_leg_L(1:3,4,end);
p_Hi(:,1)=chain_arm_R(1:3,4,end);
p_Hi(:,2)=chain_arm_L(1:3,4,end);
p_B=T_global(1:3,4);
errorFi(1:3,1)=dp_Fi(:,1)-p_Fi(:,1);
errorFi(1:3,2)=dp_Fi(:,2)-p_Fi(:,2);
errorHi(1:3,1)=dp_Hi(:,1)-p_Hi(:,1);
errorHi(1:3,2)=dp_Hi(:,2)-p_Hi(:,2);
errorB(1:3)=dp_B-p_B;
%Orientation
do_Fr=integrateTwistEulRotm(xi_Fi(4:6,1),chain_leg_R(1:3,1:3,end));
do_Fl=integrateTwistEulRotm(xi_Fi(4:6,2),chain_leg_L(1:3,1:3,end));
% do_Hi(:,1)=integrateTwistEulRotm(xi_Hi(4:6,1),chain_arm_R(1:3,1:3,end));
% do_Hi(:,2)=integrateTwistEulRotm(xi_Hi(4:6,2),chain_arm_L(1:3,1:3,end));
do_B=integrateTwistEulRotm(xi_B(4:6,1),T_global(1:3,1:3,end));
o_Fr = rotm2quat(chain_leg_R(1:3,1:3,end));
o_Fl = rotm2quat(chain_leg_L(1:3,1:3,end));
% o_Hi(:,1) = rotm2quat(chain_arm_R(1:3,1:3,end));
% o_Hi(:,2) = rotm2quat(chain_arm_L(1:3,1:3,end));
o_B = rotm2quat(T_global(1:3,1:3,end));
% % use with robotics toolbox installed
if roboticTB==1
errorFi(6:-1:4,1)=quat2eul(quatmultiply(quatinv(o_Fr),do_Fr));
errorFi(6:-1:4,2)=quat2eul(quatmultiply(quatinv(o_Fl),do_Fl));
if swing(s)==2
errorB(6:-1:4)=-quat2eul(quatmultiply(quatinv(o_B),do_B));
else
errorB(6:-1:4)=quat2eul(quatmultiply(quatinv(o_B),do_B));
end
end
% use with aerospace toolbox installed
if aerospaceTB==1
errorFi(6:-1:4,1)=quat2angle(quatmultiply(quatinv(o_Fr),do_Fr));
errorFi(6:-1:4,2)=quat2angle(quatmultiply(quatinv(o_Fl),do_Fl));
if swing(s)==2
errorB(6:-1:4)=-quat2angle(quatmultiply(quatinv(o_B),do_B));
else
errorB(6:-1:4)=quat2angle(quatmultiply(quatinv(o_B),do_B));
end
end
errorFi(6,1)=0;%leave the error in the yaw direction free
errorFi(6,2)=0;
errorB(6)=0;
%Dont control hand orientation
errorHi(4:6,1)=zeros(3,1);
errorHi(4:6,2)=zeros(3,1);
%errorB(4:6)=zeros(3,1);
errorB=errorB';
error=sum(sum(abs(errorFi)))+sum(sum(abs(errorHi)))+sum(abs(errorB));
refError=99;
best_thArm=t_arm_0;
best_thLeg=t_leg_0;
t_arm_1 =t_arm_0;
t_leg_1 =t_leg_0;
theta_arm(:,:) = t_arm_1;
theta_leg(:,:) =t_leg_1;
it=0;
while (sum(abs(error))>.0001 && it<500)
% global invJac
if swing(s)==2
errorAll=[errorFi(:,2);errorB;errorHi(:,1);errorHi(:,2)];
dtet=invJac*errorAll;
dt_leg=[dtet(6:-1:1),dtet(6:11)];
dt_arm=[dtet(12:15),dtet(17:20)];
else
errorAll=[errorFi(:,1);errorB;errorHi(:,1);errorHi(:,2)];
dtet=invJac*errorAll;
dt_leg=[dtet(6:11),dtet(6:-1:1)];
dt_arm=[dtet(12:15),dtet(17:20)];
end
% integrate the joint velocities to get the angles values
theta_arm = dt_arm+ t_arm_1;
theta_leg = dt_leg+ t_leg_1;
%ensure that the angular value is between [-pi/2;pi/2]
theta_arm = mod_interval(theta_arm,-2*pi,2*pi);
theta_leg = mod_interval(theta_leg,-2*pi,2*pi);
%% Check limits q_lim_leg_R q_lim_leg_L q_lim_arm_R q_lim_arm_L
if limits==1
[okR,breachedR]=checkLimits(theta_leg(:,1),q_lim_leg_R)
[okL,breachedL]=checkLimits(theta_leg(:,2),q_lim_leg_L)
[okRA,breachedRA]=checkLimits(theta_arm(:,1),q_lim_arm_R)
[okLA,breachedLA]=checkLimits(theta_arm(:,2),q_lim_arm_L)
if (okR && okL && okRA && okLA)
else
disp('joint limits breached')
end
if swing(s)==2
%Avoiding limits for right leg support
theta_1=[reshape(t_leg_1,1,[]),reshape(t_arm_1,1,[])];%TODO: restructure current values of joints into one vector
dtheta=[reshape(dt_leg,1,[]),reshape(dt_arm,1,[])];%Reestructure delta theta to one vector
qd_rL=avoidJointLimits(theta_1,errorAll,Jac,invJac,[q_lim_leg_R; q_lim_leg_L; q_lim_arm_R; q_lim_arm_L],dtheta);
qt_leg=[qd_rL(6:-1:1),qd_rL(6:11)];
qt_arm=[qd_rL(12:15),qd_rL(17:20)];
else
%Avoiding limits for left leg support
theta_1=[reshape(t_leg_1,1,[]),reshape(t_arm_1,1,[])];%TODO: restructure current values of joints into one vector
dtheta=[reshape(dt_leg,1,[]),reshape(dt_arm,1,[])];%Reestructure delta theta to one vector
qd_lL=avoidJointLimits(theta_1(:,2),errorAll(:,2),Jac,invJac,[q_lim_leg_R; q_lim_leg_L; q_lim_arm_R; q_lim_arm_L],dtheta);
qt_leg=[qd_lL(6:11),qd_lL(6:-1:1)];
qt_arm=[qd_lL(12:15),qd_lL(17:20)];
end
end
%%
% move the robot with the new desired joint values, update
% jacobians and intertia matrices
[chain_leg_R, chain_leg_L, chain_arm_R, chain_arm_L, chain_head] = ...
move_robot2(theta_leg,theta_arm,theta_head,swing(s),swing(s),s);
% animate the motion
if animation == 1
animate(chain_leg_R, chain_leg_L, chain_arm_R, chain_arm_L, chain_head);
end
%% Error in position
p_Fi(:,1)=chain_leg_R(1:3,4,end);
p_Fi(:,2)=chain_leg_L(1:3,4,end);
p_Hi(:,1)=chain_arm_R(1:3,4,end);
p_Hi(:,2)=chain_arm_L(1:3,4,end);
p_B=T_global(1:3,4);
errorFi(1:3,1)=dp_Fi(:,1)-p_Fi(:,1);
errorFi(1:3,2)=dp_Fi(:,2)-p_Fi(:,2);
errorHi(1:3,1)=dp_Hi(:,1)-p_Hi(:,1);
errorHi(1:3,2)=dp_Hi(:,2)-p_Hi(:,2);
errorB(1:3)=dp_B-p_B;
%% Error in Orientation
o_Fr = rotm2quat(chain_leg_R(1:3,1:3,end));
o_Fl = rotm2quat(chain_leg_L(1:3,1:3,end));
o_B = rotm2quat(T_global(1:3,1:3,end));
% % use with robotics toolbox installed
if roboticTB==1
errorFi(6:-1:4,1)=quat2eul(quatmultiply(quatinv(o_Fr),do_Fr));
errorFi(6:-1:4,2)=quat2eul(quatmultiply(quatinv(o_Fl),do_Fl));
if swing(s)==2
errorB(6:-1:4)=-quat2eul(quatmultiply(quatinv(o_B),do_B));
else
errorB(6:-1:4)=-quat2eul(quatmultiply(quatinv(o_B),do_B));
end
end
% use with aerospace toolbox installed
if aerospaceTB==1
errorFi(6:-1:4,1)=quat2angle(quatmultiply(quatinv(o_Fr),do_Fr));
errorFi(6:-1:4,2)=quat2angle(quatmultiply(quatinv(o_Fl),do_Fl));
if swing(s)==2
errorB(6:-1:4)=-quat2angle(quatmultiply(quatinv(o_B),do_B));
else
errorB(6:-1:4)=-quat2angle(quatmultiply(quatinv(o_B),do_B));
errorB(5)=-errorB(5);
end
end
%only one joint contrls the yaw orientation for both legs which is not
%engouh for controlling that orientation fully for both legs
errorFi(6,1)=0;%leave the error in the yaw direction free
errorFi(6,2)=0;
errorB(6)=0;
errorHi(4:6,1)=zeros(3,1); %orientation of the hand unavailable
errorHi(4:6,2)=zeros(3,1);% all joint of hands still no activated
%errorB(4:6)=zeros(3,1);
%% Exit Criteria
error=sum(sum(abs(errorFi)))+sum(sum(abs(errorHi)))+sum(abs(errorB));
if error<refError
best_thArm=theta_arm;
best_thLeg=theta_leg;
refError=error;
end
it=it+1;
% if it==30
%
% % disp ( error )
% end
% disp (it)
t_arm_1 = theta_arm;
t_leg_1 = theta_leg;
end
%%
if error<.0001
% disp ( strcat('converged with ',num2str(it),' iterations', ' point ',num2str(s ), ' of trajectory'))
% disp (error)
else
disp(strcat('no convergence after 500 iterations' , ' point ',num2str(s ), ' of trajectory'))
errorFi
errorHi
errorB
end
% theta_arm(:,:)- t_arm_0
% theta_leg(:,:)-t_leg_0
dtheta_leg=differentiate( best_thLeg,t_leg_0 );
dtheta_arm=differentiate( best_thArm,t_arm_0 );
if limit_vel == 1
[dt_leg(:,1),okR] = velocity_check(dtheta_leg(:,1),max_v_leg);
[dt_leg(:,2),okL ]= velocity_check(dtheta_leg(:,2),max_v_leg);
[dt_arm(:,1),okRA] = velocity_check(dtheta_arm(:,1),max_v_arm);
[dt_arm(:,2),okLA] = velocity_check(dtheta_arm(:,2),max_v_arm);
%TODO: warnings for when an "optimal" value exceeds the vel limits
if (okR && okL && okRA && okLA)
else
disp('joint velocity limits breached')
end
end