Skip to content

Commit 505eb7a

Browse files
sea-basssea-bass
committed
Fixed sign convention issues in joint angles
1 parent 952bc32 commit 505eb7a

40 files changed

+1830
-1830
lines changed

‎ControlDesign/InverseKinematics/animateFootGait.m

Lines changed: 107 additions & 107 deletions
Original file line numberDiff line numberDiff line change
@@ -1,108 +1,108 @@
1-
% Visualize Walking Gait and Inverse Kinematics
2-
% Copyright 2019 The MathWorks, Inc.
3-
4-
close all
5-
robotParametersCtrl;
6-
7-
%% Plot the foot trajectory
8-
gaitPeriod = 1;
9-
stepLength = 0.1;
10-
stepHeight = 0.025;
11-
numPoints = 150;
12-
13-
tVec = linspace(0,gaitPeriod,numPoints);
14-
foot_height_offset = sqrt( (lower_leg_length+upper_leg_length)^2 ...
15-
- ((stepLength/2)*100)^2 ) - 1e-3;
16-
17-
figure, hold on
18-
x = zeros(numPoints,1);
19-
y = zeros(numPoints,1);
20-
for idx = 1:numPoints
21-
[x(idx),y(idx)] = evalFootGait(tVec(idx),stepLength,stepHeight,gaitPeriod);
22-
end
23-
24-
plot(x,y,'.-');
25-
axis equal
26-
title('Foot Gait');
27-
xlabel('x [m]')
28-
ylabel('y [m]')
29-
30-
%% Calculate joint angles
31-
theta_hip = zeros(numPoints,1);
32-
theta_knee = zeros(numPoints,1);
33-
theta_ankle = zeros(numPoints,1);
34-
35-
for idx = 1:numPoints
36-
37-
pitch = 0; % Assume zero body pitch
38-
39-
% Calculate inverse kinematics
40-
theta = legInvKin(upper_leg_length/100, lower_leg_length/100 , ...
41-
x(idx), y(idx) - (foot_height_offset/100));
42-
43-
% Address multiple solutions by preventing knee bending backwards
44-
if size(theta,1) == 2
45-
if theta(1,2) > 0
46-
t1 = theta(2,1);
47-
t2 = theta(2,2);
48-
else
49-
t1 = theta(1,1);
50-
t2 = theta(1,2);
51-
end
52-
else
53-
t1 = theta(1);
54-
t2 = theta(2);
55-
end
56-
57-
% Pack the results. Ensure the ankle angle is set so the foot always
58-
% lands flat on the ground
59-
theta_hip(idx) = t1;
60-
theta_knee(idx) = t2;
61-
theta_ankle(idx) = -(t1+t2);
62-
63-
end
64-
65-
% Display joint angles
66-
figure
67-
subplot(311)
68-
plot(tVec,rad2deg(theta_hip))
69-
title('Hip Angle [deg]');
70-
subplot(312)
71-
plot(tVec,rad2deg(theta_knee))
72-
title('Knee Angle [deg]');
73-
subplot(313)
74-
plot(tVec,rad2deg(theta_ankle))
75-
title('Ankle Angle [deg]');
76-
77-
%% Animate the walking gait
78-
figure(3), clf, hold on
79-
80-
% Initialize plot
81-
plot(x,y-foot_height_offset/100,'k:','LineWidth',1);
82-
h1 = plot([0 0],[0 0],'r-','LineWidth',4);
83-
h2 = plot([0 0],[0 0],'b-','LineWidth',4);
84-
85-
% Calculate knee and ankle (x,y) positions
86-
xKnee = sin(theta_hip)*upper_leg_length/100;
87-
yKnee = -cos(theta_hip)*upper_leg_length/100;
88-
xAnkle = xKnee + sin(theta_hip+theta_knee)*lower_leg_length/100;
89-
yAnkle = yKnee - cos(theta_hip+theta_knee)*lower_leg_length/100;
90-
91-
% Define axis limits
92-
xMin = min([xKnee;xAnkle]) -0.025;
93-
xMax = max([xKnee;xAnkle]) +0.025;
94-
yMin = min([yKnee;yAnkle]) -0.025;
95-
yMax = max([0;yKnee;yAnkle]) +0.025;
96-
97-
% Animate the walking gait
98-
numAnimations = 5;
99-
for anim = 1:numAnimations
100-
for idx = 1:numPoints
101-
set(h1,'xdata',[0 xKnee(idx)],'ydata',[0 yKnee(idx)]);
102-
set(h2,'xdata',[xKnee(idx) xAnkle(idx)],'ydata',[yKnee(idx) yAnkle(idx)]);
103-
xlim([xMin xMax]), ylim([yMin yMax]);
104-
title('Walking Gait Animation');
105-
axis equal
106-
drawnow
107-
end
1+
% Visualize Walking Gait and Inverse Kinematics
2+
% Copyright 2019 The MathWorks, Inc.
3+
4+
close all
5+
robotParametersCtrl;
6+
7+
%% Plot the foot trajectory
8+
gaitPeriod = 1;
9+
stepLength = 0.1;
10+
stepHeight = 0.025;
11+
numPoints = 150;
12+
13+
tVec = linspace(0,gaitPeriod,numPoints);
14+
foot_height_offset = sqrt( (lower_leg_length+upper_leg_length)^2 ...
15+
- ((stepLength/2)*100)^2 ) - 1e-3;
16+
17+
figure, hold on
18+
x = zeros(numPoints,1);
19+
y = zeros(numPoints,1);
20+
for idx = 1:numPoints
21+
[x(idx),y(idx)] = evalFootGait(tVec(idx),stepLength,stepHeight,gaitPeriod);
22+
end
23+
24+
plot(x,y,'.-');
25+
axis equal
26+
title('Foot Gait');
27+
xlabel('x [m]')
28+
ylabel('y [m]')
29+
30+
%% Calculate joint angles
31+
theta_hip = zeros(numPoints,1);
32+
theta_knee = zeros(numPoints,1);
33+
theta_ankle = zeros(numPoints,1);
34+
35+
for idx = 1:numPoints
36+
37+
pitch = 0; % Assume zero body pitch
38+
39+
% Calculate inverse kinematics
40+
theta = legInvKin(upper_leg_length/100, lower_leg_length/100 , ...
41+
x(idx), y(idx) - (foot_height_offset/100));
42+
43+
% Address multiple solutions by preventing knee bending backwards
44+
if size(theta,1) == 2
45+
if theta(1,2) > 0
46+
t1 = theta(2,1);
47+
t2 = theta(2,2);
48+
else
49+
t1 = theta(1,1);
50+
t2 = theta(1,2);
51+
end
52+
else
53+
t1 = theta(1);
54+
t2 = theta(2);
55+
end
56+
57+
% Pack the results. Ensure the ankle angle is set so the foot always
58+
% lands flat on the ground
59+
theta_hip(idx) = t1;
60+
theta_knee(idx) = t2;
61+
theta_ankle(idx) = -(t1+t2);
62+
63+
end
64+
65+
% Display joint angles
66+
figure
67+
subplot(311)
68+
plot(tVec,rad2deg(theta_hip))
69+
title('Hip Angle [deg]');
70+
subplot(312)
71+
plot(tVec,rad2deg(theta_knee))
72+
title('Knee Angle [deg]');
73+
subplot(313)
74+
plot(tVec,rad2deg(theta_ankle))
75+
title('Ankle Angle [deg]');
76+
77+
%% Animate the walking gait
78+
figure(3), clf, hold on
79+
80+
% Initialize plot
81+
plot(x,y-foot_height_offset/100,'k:','LineWidth',1);
82+
h1 = plot([0 0],[0 0],'r-','LineWidth',4);
83+
h2 = plot([0 0],[0 0],'b-','LineWidth',4);
84+
85+
% Calculate knee and ankle (x,y) positions
86+
xKnee = sin(theta_hip)*upper_leg_length/100;
87+
yKnee = -cos(theta_hip)*upper_leg_length/100;
88+
xAnkle = xKnee + sin(theta_hip+theta_knee)*lower_leg_length/100;
89+
yAnkle = yKnee - cos(theta_hip+theta_knee)*lower_leg_length/100;
90+
91+
% Define axis limits
92+
xMin = min([xKnee;xAnkle]) -0.025;
93+
xMax = max([xKnee;xAnkle]) +0.025;
94+
yMin = min([yKnee;yAnkle]) -0.025;
95+
yMax = max([0;yKnee;yAnkle]) +0.025;
96+
97+
% Animate the walking gait
98+
numAnimations = 5;
99+
for anim = 1:numAnimations
100+
for idx = 1:numPoints
101+
set(h1,'xdata',[0 xKnee(idx)],'ydata',[0 yKnee(idx)]);
102+
set(h2,'xdata',[xKnee(idx) xAnkle(idx)],'ydata',[yKnee(idx) yAnkle(idx)]);
103+
xlim([xMin xMax]), ylim([yMin yMax]);
104+
title('Walking Gait Animation');
105+
axis equal
106+
drawnow
107+
end
108108
end

‎ControlDesign/InverseKinematics/evalFootGait.m

Lines changed: 22 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,23 @@
1-
function [x,y] = evalFootGait(t,stepLength,stepHeight,gaitPeriod)
2-
% EVALFOOTGAIT Evaluates foot gait at a specified time
3-
%
4-
% Copyright 2019 The MathWorks, Inc.
5-
6-
% t = 0 : extended forward
7-
% 0 >= t > T/2 : dragging back
8-
% T/2 >= t > T : swinging forward
9-
10-
tEff = mod(t,gaitPeriod);
11-
12-
% Dragging back (y position is 0)
13-
if tEff < gaitPeriod/2
14-
x = stepLength * (gaitPeriod - 2*tEff)/gaitPeriod - stepLength/2;
15-
y = 0;
16-
17-
% Swinging forward (y follows curve)
18-
else
19-
x = stepLength * (2*tEff - gaitPeriod)/gaitPeriod - stepLength/2;
20-
y = stepHeight*(1 - (4*abs(tEff - 3*gaitPeriod/4)/gaitPeriod)^2);
21-
end
22-
1+
function [x,y] = evalFootGait(t,stepLength,stepHeight,gaitPeriod)
2+
% EVALFOOTGAIT Evaluates foot gait at a specified time
3+
%
4+
% Copyright 2019 The MathWorks, Inc.
5+
6+
% t = 0 : extended forward
7+
% 0 >= t > T/2 : dragging back
8+
% T/2 >= t > T : swinging forward
9+
10+
tEff = mod(t,gaitPeriod);
11+
12+
% Dragging back (y position is 0)
13+
if tEff < gaitPeriod/2
14+
x = stepLength * (gaitPeriod - 2*tEff)/gaitPeriod - stepLength/2;
15+
y = 0;
16+
17+
% Swinging forward (y follows curve)
18+
else
19+
x = stepLength * (2*tEff - gaitPeriod)/gaitPeriod - stepLength/2;
20+
y = stepHeight*(1 - (4*abs(tEff - 3*gaitPeriod/4)/gaitPeriod)^2);
21+
end
22+
2323
end

‎ControlDesign/InverseKinematics/legInvKin.m

Lines changed: 26 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -1,26 +1,26 @@
1-
function out1 = legInvKin(L1,L2,x,y)
2-
%LEGINVKIN
3-
% OUT1 = LEGINVKIN(L1,L2,X,Y)
4-
5-
% This function was generated by the Symbolic Math Toolbox version 8.2.
6-
% 02-Nov-2018 22:21:58
7-
8-
t2 = L1.^2;
9-
t3 = L2.^2;
10-
t4 = x.^2;
11-
t5 = y.^2;
12-
t6 = L1.*L2.*2.0;
13-
t7 = -t2-t3+t4+t5+t6;
14-
t8 = t2+t3-t4-t5+t6;
15-
t9 = t7.*t8;
16-
t10 = sqrt(t9);
17-
t11 = 1.0./t7;
18-
t12 = t2-t3+t4+t5-L1.*y.*2.0;
19-
t13 = 1.0./t12;
20-
t14 = L1.*x.*2.0;
21-
t15 = t4.*t10.*t11;
22-
t16 = t5.*t10.*t11;
23-
t17 = L1.*L2.*t10.*t11.*2.0;
24-
t18 = t10.*t11;
25-
t19 = atan(t18);
26-
out1 = reshape([atan(t13.*(t14+t15+t16+t17-t2.*t10.*t11-t3.*t10.*t11)).*2.0,atan(t13.*(t14-t15-t16-t17+t2.*t10.*t11+t3.*t10.*t11)).*2.0,t19.*-2.0,t19.*2.0],[2,2]);
1+
function out1 = legInvKin(L1,L2,x,y)
2+
%LEGINVKIN
3+
% OUT1 = LEGINVKIN(L1,L2,X,Y)
4+
5+
% This function was generated by the Symbolic Math Toolbox version 8.2.
6+
% 02-Nov-2018 22:21:58
7+
8+
t2 = L1.^2;
9+
t3 = L2.^2;
10+
t4 = x.^2;
11+
t5 = y.^2;
12+
t6 = L1.*L2.*2.0;
13+
t7 = -t2-t3+t4+t5+t6;
14+
t8 = t2+t3-t4-t5+t6;
15+
t9 = t7.*t8;
16+
t10 = sqrt(t9);
17+
t11 = 1.0./t7;
18+
t12 = t2-t3+t4+t5-L1.*y.*2.0;
19+
t13 = 1.0./t12;
20+
t14 = L1.*x.*2.0;
21+
t15 = t4.*t10.*t11;
22+
t16 = t5.*t10.*t11;
23+
t17 = L1.*L2.*t10.*t11.*2.0;
24+
t18 = t10.*t11;
25+
t19 = atan(t18);
26+
out1 = reshape([atan(t13.*(t14+t15+t16+t17-t2.*t10.*t11-t3.*t10.*t11)).*2.0,atan(t13.*(t14-t15-t16-t17+t2.*t10.*t11+t3.*t10.*t11)).*2.0,t19.*-2.0,t19.*2.0],[2,2]);

0 commit comments

Comments
 (0)