-
Notifications
You must be signed in to change notification settings - Fork 0
/
jumpOptCasadiWithFlightWithTime.m
582 lines (478 loc) · 18 KB
/
jumpOptCasadiWithFlightWithTime.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
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
close all
clear
% Objective functions
% 1 - final x com position based on ballistic traj
obj_func = 1;
use_boom = 0;
%% Add Libraries
% add casadi library
addpath(genpath('casadi'));
addpath(genpath('dynamics_gen_boom'));
%% Set Auxillary Data
m1 =.0393 + .2; % 0.2 is motor mass
m2 =.0368;
m3 = .00783;
m4 = .0155;
I1 = 25.1 * 10^-6; I2 = 53.5 * 10^-6;
I3 = 9.25 * 10^-6; I4 = 22.176 * 10^-6;
l_OA=.011; l_OB=.042;
l_AC=.096; l_DE=.091;
l_O_m1=0.032; l_B_m2=0.0344;
l_A_m3=0.0622; l_C_m4=0.0610;
N = 18.75;
Ir = 0.0035/N^2;
g = 9.81;
% parameters to be adjusted according to design
m_body = 0.186 +0.211;
l_body = 68.1/1000;
l_arm = 5*0.0254;% dummy assignation for some initializations
l_cm_arm = 1*l_arm;% dummy assignation for some initializations
l_cm_body = l_body/2;% assume body com is at half of the body length (makes sense since main body is composed of two motors (hip+arm) + brackets. com will be ~between both motors
m_arm = 0.2; % 100 grams ?
I_arm = m_arm*l_cm_arm^2; % dummy assignation for some initializations
ground_height = 0;
mu = 0.8; % friction coef
max_voltage = 20; % volts
motor_kt = 0.18;
motor_R = 2;
tau_max = (max_voltage)*motor_kt/motor_R*1.2;
m_offset_x = use_boom * 0.4;
m_offset_y = use_boom * 0.16;
l_boom = 8*0.0254;
h_boom = 0.1921; % to be adjusted for ground.
hob = 91.3/1000;
boom_stiffness = use_boom * 0.2877/1.2; % Nm/rad
boom_angle_min = -deg2rad(70);
boom_angle_max = deg2rad(70);
%% Parameter vector
p = [m1 m2 m3 m4 m_body m_arm I1 I2 I3 I4 I_arm Ir N l_O_m1 l_B_m2...
l_A_m3 l_C_m4 l_cm_arm l_cm_body l_OA l_OB l_AC l_DE l_body l_arm g...
m_offset_x m_offset_y l_boom h_boom hob boom_stiffness motor_kt motor_R]'; % parameters
%% Initial conditions
desired_hip_pos0 = [0.0;0.1];%[0.03;0.06];
guess_leg_angle = [10*pi/180; 10*pi/180];
init_leg_angle = fsolve(@(x)solve_init_pose(x,desired_hip_pos0,p),guess_leg_angle);
init_arm_angle = pi;
z0 = [desired_hip_pos0;init_leg_angle;init_arm_angle;0;0;0;0;0];
% Test for valid initial condition
if(init_leg_angle(1) > deg2rad(75) || init_leg_angle(1) < -deg2rad(75) ||...
init_leg_angle(2) > deg2rad(142) || init_leg_angle(2) < -deg2rad(32))
error('Infeasible starting position')
end
%% State/Control Bounds
q_min = [-0.02 0.04 -deg2rad(75) deg2rad(32) -2*pi -1.5 -1.5 -150 -150 -150]';
q_max = [0.35 0.5 deg2rad(75) deg2rad(142) 2*pi 3 3 150 150 150]';
u_min = -[tau_max tau_max tau_max]';
u_max = [tau_max tau_max tau_max]';
% Good result
% t_stance = 17
% l_arm = 18.4500*0.0254
t_stance_vec = linspace(8,70,13);
l_arm_vec = linspace(1,15,13)*0.0254;
tic
for aa = 1:length(l_arm_vec)
l_arm = l_arm_vec(aa);
l_cm_arm = 1*l_arm;
I_arm = m_arm*l_cm_arm^2;
p = [m1 m2 m3 m4 m_body m_arm I1 I2 I3 I4 I_arm Ir N l_O_m1 l_B_m2...
l_A_m3 l_C_m4 l_cm_arm l_cm_body l_OA l_OB l_AC l_DE l_body l_arm g...
m_offset_x m_offset_y l_boom h_boom hob boom_stiffness motor_kt motor_R]'; % parameters
for j = 1:length(t_stance_vec)
aa
j
%% Integration Settings
res = 5;
dt = 0.01/res;
N_stance = floor(res * t_stance_vec(j)); % for now, let's fix stance time
N_steps = N_stance + 15;
%% Optimization Variables
% create optimization object
opti = casadi.Opti();
% create optimization variables
X = opti.variable(22,N_steps);
% names for optimization variables
qdd = X(1:5,:); % joint acceleration (includes floating base coordinates)
qd = X(6:10,:); % joint velocity
q = X(11:15,:); % joint position
f = X(16:17,:); % foot force
tau_motor = X(18:20,:); % actuator torques
boom_angle = X(21,:);
boom_force = X(22,:);
%% Initial Constraints
opti.subject_to(q(:,1) == z0(1:5))
opti.subject_to(qd(:,1) == z0(6:10))
%% Path Costs/Constraints
for k = 1:N_steps
disp([num2str(k),' of ',num2str(N_steps)]);
% This iteration
qk = q(:,k);
qdk = qd(:,k);
qddk = qdd(:,k);
tauk = tau_motor(:,k);
fk = f(:,k);
boom_angle_k = boom_angle(:,k);
boom_force_k = boom_force(:,k);
% Dynamics
if k <= N_stance
Ak = A_stance([qk;qdk],p);
bk = b_stance([qk;qdk],tauk,p);
xk_augmented = Ak\(bk);
opti.subject_to(qddk == xk_augmented(1:5));
opti.subject_to(fk == xk_augmented(6:7));
else
Ak = A_floating([qk;qdk],p);
bk = b_floating([qk;qdk],tauk,p,[0;0]);
xk = Ak\(bk);
opti.subject_to(qddk == xk);
opti.subject_to(fk == [0;0]);
end
if k < N_steps
opti.subject_to(q(:,k+1) == qk + dt*qdk) % position integration
opti.subject_to(qd(:,k+1) == qdk + dt*qddk) % velocity integration
end
% Reaction Force
opti.subject_to(fk(1) <= mu*fk(2)); % friction
opti.subject_to(fk(1) >= -mu*fk(2));
opti.subject_to(fk(2) >= 0); % unilateral
opti.subject_to(fk(2) <= 80);
% Voltage Inequality
opti.subject_to((tauk(1))*motor_R/motor_kt + motor_kt*qdk(3) <= max_voltage);
opti.subject_to((tauk(1))*motor_R/motor_kt + motor_kt*qdk(3) >= -max_voltage);
opti.subject_to((tauk(2))*motor_R/motor_kt + motor_kt*qdk(4) <= max_voltage);
opti.subject_to((tauk(2))*motor_R/motor_kt + motor_kt*qdk(4) >= -max_voltage);
opti.subject_to((tauk(3))*motor_R/motor_kt + motor_kt*qdk(5) <= max_voltage);
opti.subject_to((tauk(3))*motor_R/motor_kt + motor_kt*qdk(5) >= -max_voltage);
% Boom angle constraint
opti.subject_to(boom_angle_k == angle_boom([qk;qdk],p));
opti.subject_to(boom_force_k == Force_boom([qk;qdk],p));
opti.subject_to(boom_angle_k <= boom_angle_max);
opti.subject_to(boom_angle_k >= boom_angle_min);
% State Bounds
opti.subject_to(qk <= q_max(1:5));
opti.subject_to(qk >= q_min(1:5));
opti.subject_to(qdk <= q_max(6:10));
opti.subject_to(qdk >= q_min(6:10));
% Control Bounds
opti.subject_to(tauk <= u_max);
opti.subject_to(tauk >= u_min);
% Foot above ground
foot_height_k = position_foot([qk;qdk],p);
opti.subject_to(foot_height_k(2) >= 0);
% Regularization
if k == 1
cost = (0.1*qdk(5)*qdk(5) + 0.1*tauk(3)*tauk(3))*dt;
elseif k > N_stance
cost = cost + (0.01*qdk(3:5)'*qdk(3:5) + 0.01*tauk(1:3)'*tauk(1:3))*dt;
else
cost = cost + (0.01*qdk(5)*qdk(5) + 0.01*tauk(3)*tauk(3))*dt;
end
end
%% Terminal Costs/Constraints
qN = q(:,N_stance);
qdN = qd(:,N_stance);
com_fin = com_pos([qN;qdN],p);
dcom_fin = com_vel([qN;qdN],p);
arm_vel = arm_tip_vel([qN;qdN],p);
% Takeoff conditions
opti.subject_to(com_fin(1) >= 0.0)
opti.subject_to(dcom_fin(1) >= 0.0)
opti.subject_to(dcom_fin(2) >= 0.0)
switch obj_func
case 1
if use_boom ==1
g_with_boom = 0.5*g;
else
g_with_boom = g;
end
vz = dcom_fin(2);
z = com_fin(2);
vx = dcom_fin(1);
x = com_fin(1);
t_flight = (1/g_with_boom) * (vz + sqrt(vz^2 + 2*z*g_with_boom));
x_land = x + vx*t_flight;
cost = cost - x_land; % horizontal landing position
end
opti.minimize(cost);
%% Initial guess
if j > 1
guess = repmat(Xs{j-1,aa}(:,end),1,N_steps);
guess(:,1:length(Xs{j-1,aa})) = Xs{j-1,aa};
opti.set_initial(X,guess);
elseif aa > 1
guess = repmat(Xs{1,aa-1}(:,end),1,N_steps);
guess(:,1:length(Xs{1,aa-1})) = Xs{1,aa-1};
opti.set_initial(X,guess);
else
opti.set_initial(q,repmat(z0(1:5),1,N_steps));
opti.set_initial(qd,repmat(z0(6:10),1,N_steps));
end
%% Solve!
opti.solver('ipopt');
sol = opti.solve();
%% Decompose solution
Xs{j,aa} = sol.value(X); % full TO output
qs{j,aa} = sol.value(q); % position vector
qds{j,aa} = sol.value(qd); % velocity vector
qdds{j,aa} = sol.value(qdd); % acceleration vector
fs{j,aa} = sol.value(f); % front foot GRF
taus{j,aa} = sol.value(tau_motor); % Joint torques
ts{j,aa} = 0:dt:dt*(N_steps-1);
t_stances{j,aa} = 0:dt:dt*(N_stance-1);
boom_angs{j,aa} = sol.value(boom_angle); % boom angle
boom_fs{j,aa} = sol.value(boom_force); % boom force
landing_pos(j,aa) = -sol.value(cost);
% CoM at takeoff
takeoff_pos_x(j,aa) = sol.value(com_fin(1));
takeoff_pos_y(j,aa) = sol.value(com_fin(2));
takeoff_vel_x(j,aa) = sol.value(dcom_fin(1));
takeoff_vel_y(j,aa) = sol.value(dcom_fin(2));
takeoff_vel_ratio(j,aa) = sol.value(dcom_fin(1)/dcom_fin(2));
% Body at takeoff
takeoff_pos_body_x(j,aa) = sol.value(qN(1));
takeoff_pos_body_y(j,aa) = sol.value(qN(2));
takeoff_vel_body_x(j,aa) = sol.value(qdN(1));
takeoff_vel_body_y(j,aa) = sol.value(qdN(2));
takeoff_vel_body_ratio(j,aa) = sol.value(qdN(1)/qdN(2));
% Arm at takeoff
takeoff_ang_arm(j,aa) = sol.value(qN(5));
takeoff_ang_vel_arm(j,aa) = sol.value(qdN(5));
takeoff_vel_arm_x(j,aa) = sol.value(arm_vel(1));
takeoff_vel_arm_y(j,aa) = sol.value(arm_vel(2));
takeoff_vel_arm_mag(j,aa) = sqrt(sol.value(arm_vel(1))^2+sol.value(arm_vel(2))^2);
takeoff_vel_arm_ratio(j,aa) = sol.value(arm_vel(1))/sol.value(arm_vel(2));
stance_time(j,aa) = t_stances{j,aa}(end);
end
end
toc
%% Contour plots comparing different stance times and arm lengths
% CoM Data
figure()
% subplot(2,1,1)
contourf(t_stance_vec./100,100.*l_arm_vec,landing_pos')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Horizontal Landing Position - Predicted (m)')
colorbar
%
% subplot(2,1,2)
% contourf(t_stance_vec./100,100.*l_arm_vec,landing_pos') % actual landing pos - from charles
% xlabel('Stance Time (s)');
% ylabel('Arm Length (cm)');
% title('Horizontal Landing Position - Predicted (m)')
% colorbar
% Arm Data
figure()
subplot(3,2,1)
contourf(t_stance_vec./100,100.*l_arm_vec,rad2deg(takeoff_ang_arm)')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Position at Takeoff (deg)')
colorbar
subplot(3,2,2)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_ang_vel_arm')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Angular velocity at Takeoff (rad/s)')
colorbar
subplot(3,2,3)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_arm_x')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Tip v_x at Takeoff (m/s)')
colorbar
subplot(3,2,4)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_arm_y')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Tip v_y at Takeoff (m/s)')
colorbar
subplot(3,2,5)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_arm_ratio')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Tip Velocity Ratio at Takeoff')
colorbar
subplot(3,2,6)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_arm_mag')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Arm Tip ||v|| at Takeoff (m/s)')
colorbar
% Body Data
figure()
subplot(3,2,1)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_pos_body_x')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Body Position - x at Takeoff (m)')
colorbar
subplot(3,2,2)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_pos_body_y')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Body Position - y at Takeoff (m)')
colorbar
subplot(3,2,3)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_body_x')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Body v_x at Takeoff (m/s)')
colorbar
subplot(3,2,4)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_body_y')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Body v_y at Takeoff (m/s)')
colorbar
subplot(3,2,5)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_body_ratio')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('Body Velocity Ratio at Takeoff')
colorbar
% subplot(3,2,6)
% contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_body_mag')
% xlabel('Stance Time (s)');
% ylabel('Arm Length (cm)');
% title('Body ||v|| at Takeoff (m/s)')
% colorbar
% CoM Data
figure()
subplot(3,2,1)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_pos_x')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('CoM Position - x at Takeoff (m)')
colorbar
subplot(3,2,2)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_pos_y')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('CoM Position - y at Takeoff (m)')
colorbar
subplot(3,2,3)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_x')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('CoM v_x at Takeoff (m/s)')
colorbar
subplot(3,2,4)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_y')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('CoM v_y at Takeoff (m/s)')
colorbar
subplot(3,2,5)
contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_ratio')
xlabel('Stance Time (s)');
ylabel('Arm Length (cm)');
title('CoM Velocity Ratio at Takeoff')
colorbar
% subplot(3,2,6)
% contourf(t_stance_vec./100,100.*l_arm_vec,takeoff_vel_mag')
% xlabel('Stance Time (s)');
% ylabel('Arm Length (cm)');
% title('CoM ||v|| at Takeoff (m/s)')
% colorbar
%% Compare different stance times for a given arm length
% for each stance time, find the optimal arm length
indx_arm = 1; % do this for a given arm length
[~,indx_stance] = max(landing_pos(:,indx_arm));
figure()
subplot(3,2,1)
plot(stance_time,landing_pos(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('Horizontal Jump Distance');
subplot(3,2,2)
plot(stance_time,takeoff_vel_ratio(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('Takeoff Velocity Ratio');
subplot(3,2,3)
plot(stance_time,takeoff_pos_x(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('X Position at Takeoff (m)');
subplot(3,2,4)
plot(stance_time,takeoff_pos_y(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('Y Position at Takeoff (m)');
subplot(3,2,5)
plot(stance_time,takeoff_vel_x(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('X Velocity at Takeoff (m/s)');
subplot(3,2,6)
plot(stance_time,takeoff_vel_y(:,indx_arm),'ro')
xlabel('Stance Time (s)')
ylabel('Y Velocity at Takeoff (m)');
%% Compare different arm length for a given stance time
indx_stance = 2; % do this for a given arm length
last_arm_idx = length(l_arm_vec);
l_arm_vec =l_arm_vec(1:last_arm_idx);
figure()
title('')
subplot(3,2,1)
plot(l_arm_vec,landing_pos(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('Horizontal Jump Distance');
subplot(3,2,2)
plot(l_arm_vec,takeoff_vel_ratio(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('Takeoff Velocity Ratio');
subplot(3,2,3)
plot(l_arm_vec,takeoff_pos_x(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('X Position at Takeoff (m)');
subplot(3,2,4)
plot(l_arm_vec,takeoff_pos_y(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('Y Position at Takeoff (m)');
subplot(3,2,5)
plot(l_arm_vec,takeoff_vel_x(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('X Velocity at Takeoff (m/s)');
subplot(3,2,6)
plot(l_arm_vec,takeoff_vel_y(indx_stance,:),'ro')
xlabel('Arm Length (m)')
ylabel('Y Velocity at Takeoff (m)');
%% Simulate & plot
% take correct arm length for simulation
l_arm = l_arm_vec(indx_arm);
l_cm_arm = 1*l_arm;
I_arm = m_arm*l_cm_arm^2;
p = [m1 m2 m3 m4 m_body m_arm I1 I2 I3 I4 I_arm Ir N l_O_m1 l_B_m2...
l_A_m3 l_C_m4 l_cm_arm l_cm_body l_OA l_OB l_AC l_DE l_body l_arm g...
m_offset_x m_offset_y l_boom h_boom hob boom_stiffness motor_kt motor_R]'; % parameters
[tsim, zsim, tstance, zstance] = simulate_optimal_solution_casadi(t_stances{indx_stance,indx_arm},z0,taus{indx_stance,indx_arm},p);
plot_with_bounds(ts{indx_stance,indx_arm},qs{indx_stance,indx_arm},qds{indx_stance,indx_arm},...
fs{indx_stance,indx_arm},taus{indx_stance,indx_arm},boom_angs{indx_stance,indx_arm},boom_fs{indx_stance,indx_arm},...
q_min,q_max,u_min,u_max,max_voltage,mu,...
N,motor_R,motor_kt,boom_angle_min,boom_angle_max);
figure();
plot(zsim(:,1)',zsim(:,2)','ro-')
hold on
plot(qs{indx_stance}(1,:),qs{indx_stance}(2,:),'bo-')
xlabel('Body Position - x (m)')
ylabel('Body Position - y (m)')
%% Animate
figure();
animateSol(tsim,zsim',p);
%% Save traj
save_traj(ts{indx_stance,indx_arm},[qs{indx_stance,indx_arm};qds{indx_stance,indx_arm}],taus{indx_stance,indx_arm},'test_traj_right_kt.mat',1/100)
%% simulate in loop
for aa = 1:length(l_arm_vec)
% take correct arm length for simulation
l_arm = l_arm_vec(aa);
l_cm_arm = 1*l_arm;
I_arm = m_arm*l_cm_arm^2;
p = [m1 m2 m3 m4 m_body m_arm I1 I2 I3 I4 I_arm Ir N l_O_m1 l_B_m2...
l_A_m3 l_C_m4 l_cm_arm l_cm_body l_OA l_OB l_AC l_DE l_body l_arm g...
m_offset_x m_offset_y l_boom h_boom hob boom_stiffness motor_kt motor_R]'; % parameters
[tsim, zsim, tstance, zstance] = simulate_optimal_solution_casadi(t_stances{indx_stance,aa},z0,taus{indx_stance,aa},p);
rE{aa} = position_foot(zsim',p);
end
%%
figure;
for aa = 1:length(l_arm_vec)
plot(l_arm_vec(aa),rE{aa}(1,end),'or');hold on;
end
xlabel('Arm Length (m)');
ylabel('Jumping Distance (m)'); grid on