-
Notifications
You must be signed in to change notification settings - Fork 0
/
plot_results.m
155 lines (132 loc) · 5.22 KB
/
plot_results.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
global vec_T GAM poly polyp polypp ld lg
global ZMPx R_Force qr vfoot2_toe vfoot2_heel
global R_Force_y no_slipping
% S=solve('d+L1*sin(q1)+L2*sin(q2)-L4*sin(q4)-L5*sin(q5)=0','xh+L2*sin(q2)+L1*sin(q1)=0',' yh-L2*cos(q2)-L1*cos(q1)-hp=0',' yh-L4*cos(q4)-L5*cos(q5)-hp=0','q1,q2,q4,q5')
% S=solve('d+L1*sin(q1)+L2*sin(q2)-L4*sin(q4)-L5*sin(q5)=0','xh+L2*sin(q2)+L1*sin(q1)=0',' xh+L4*sin(q4)+L5*sin(q5)-d=0',' yh-L4*cos(q4)-L5*cos(q5)-hp=0','q1,q2,q4,q5')
% S=solve('d+L1*sin(q1)+L2*sin(q2)-L4*sin(q4)-L5*sin(q5)=0','xh+L2*sin(q2)+L1*sin(q1)=0',' yh-L2*cos(q2)-L1*cos(q1)-hp=0',' xh+L4*sin(q4)+L5*sin(q5)-d=0','q1,q2,q4,q5')
% xh+L4*sin(q4)+L5*sin(q5)-d
% pause
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Joint Torques
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
hold on; grid on
xlabel('Time (sec)')
ylabel('Torque (N.m)')
plot(vec_T,GAM)
% plot(vec_T,GAM,'LineWidth',1.5)
% legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Arm 1 ','forearm 1','Arm 2 ','forearm 2','Swing Ankle')
% title('Torques','FontSize',14)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Absolute Joint Positions in Degrees
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(2)
hold on; grid on
xlabel('Time (sec)')
ylabel('Joint absolute angle (degree)')
plot(vec_T,poly*180/pi)
% legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Arm 1 ','forearm 1','Arm 2 ','forearm 2','Swing Ankle')
% title('Absolute Joint Positions','FontSize',14)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Relative Joint Positions in Degrees
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure(3)
% hold on; grid on
% xlabel('Time (sec)')
% ylabel('Joint relative angle (degree)')
% qr=Convert_to_qr(poly);
% plot(vec_T,qr*180/pi)
% % legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
% legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Arm 1 ','forearm 1','Arm 2 ','forearm 2','Swing Ankle')
% % title('Relative Joint Positions','FontSize',14)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Joint Velocities in rad/sec
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
qrr=Convert_to_qr(polyp);
figure(4)
hold on; grid on
xlabel('Time (sec)')
ylabel('Relative Joint Velocity (rad/sec)')
plot(vec_T,qrr*180/pi)
% legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Arm 1 ','forearm 1','Arm 2 ','forearm 2','Swing Ankle')
% title('Joint Velocities','FontSize',14)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Joint Accelerations in rad/sec.sec
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure(5)
% hold on; grid on
% xlabel('Time (sec)')
% ylabel('Joint Acceleration (rad/sec.sec)')
% plot(vec_T,polypp)
% % legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
% legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Arm 1 ','forearm 1','Arm 2 ','forearm 2','Swing Ankle')
% % title(' Joint Accelerations','FontSize',14)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Vertical Reaction Force Ry
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_Force_y=R_Force(2,:);
Ry=R_Force(2,:);
Rx=R_Force(1,:);
figure(6)
hold on; grid on
xlabel('Time (sec)')
ylabel('Vertical Reaction Force (N)')
% title('No Take-Off Constraint','FontSize',14)
plot(vec_T,R_Force_y)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot no-slipping constraint
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mu=0.8;
% no_slipping = mu*R_Force(2,:)'- abs(R_Force(1,:)');
figure(7)
hold on; grid on
xlabel('Time (sec)')
ylabel('mu*Ry - abs(Rx) (N)')
% title('No-Slipping Constraint','FontSize',14)
plot(vec_T,no_slipping)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot ZMPx
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lb_ZMPx=[vec_T(1),vec_T(end)];
lb_ZMPy=[-lg,-lg]*100;
ub_ZMPx=lb_ZMPx;
ub_ZMPy=[ld,ld]*100;
figure(8)
hold on; grid on
xlabel('Time (sec)')
ylabel('ZMPx (cm)')
% title('ZMPx','FontSize',14)
plot(ub_ZMPx,ub_ZMPy,'--r','LineWidth',1.5)
plot(lb_ZMPx,lb_ZMPy,'--r','LineWidth',1.5)
plot(vec_T,ZMPx*100)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot Toe and Heel Vertical Velocity
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(9)
Toe_y = vfoot2_toe(2,:);
Heel_y = vfoot2_heel(2,:);
hold on; grid on
xlabel('Time (sec)')
ylabel('Vertical Component of Velocity (m/sec)')
% title('ZMPx','FontSize',14)
plot(vec_T,[Toe_y;Heel_y])
legend('Toe Velocity','Heel Velocity')
figure(13)
hold on; grid on
xlabel('Time (sec)')
ylabel('A_b_b*ddq_b')
plot(vec_T,Abb_ddqb)
% plot(vec_T,GAM,'LineWidth',1.5)
% legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Swing Ankle')
figure(14)
hold on; grid on
xlabel('Time (sec)')
ylabel('A_b_h*ddq_h')
plot(vec_T,Abh_ddqh)
% plot(vec_T,GAM,'LineWidth',1.5)
% legend('Swing Ankle','Swing Knee','Swing Hip','Support Hip','Support Knee','Support Ankle')
legend('Support Ankle','Support Knee','Support Hip ','Swing Hip','Swing Knee','Swing Ankle')