Commit 756ea253 authored by Amira Abdel-Rahman's avatar Amira Abdel-Rahman
Browse files

final

parent 5b682761
......@@ -3,26 +3,26 @@ function A = A_leg(in1,in2)
% A = A_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:16
% 02-Dec-2021 21:48:12
I1 = in2(6,:);
I2 = in2(7,:);
I3 = in2(8,:);
I4 = in2(9,:);
Ir = in2(10,:);
N = in2(11,:);
l_AC = in2(18,:);
l_A_m3 = in2(14,:);
l_B_m2 = in2(13,:);
l_C_m4 = in2(15,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
l_O_m1 = in2(12,:);
m0 = in2(1,:);
m1 = in2(2,:);
m2 = in2(3,:);
m3 = in2(4,:);
m4 = in2(5,:);
I1 = in2(10,:);
I2 = in2(11,:);
I3 = in2(12,:);
I4 = in2(13,:);
Ir = in2(14,:);
N = in2(15,:);
l_AC = in2(22,:);
l_A_m3 = in2(18,:);
l_B_m2 = in2(17,:);
l_C_m4 = in2(19,:);
l_OA = in2(20,:);
l_OB = in2(21,:);
l_O_m1 = in2(16,:);
m0 = in2(5,:);
m1 = in2(6,:);
m2 = in2(7,:);
m3 = in2(8,:);
m4 = in2(9,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,21 +3,21 @@ function Corr_Joint_Sp = Corr_leg(in1,in2)
% CORR_JOINT_SP = CORR_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:19
% 02-Dec-2021 21:48:14
dth1 = in1(5,:);
dth2 = in1(6,:);
l_AC = in2(18,:);
l_A_m3 = in2(14,:);
l_B_m2 = in2(13,:);
l_C_m4 = in2(15,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
l_O_m1 = in2(12,:);
m1 = in2(2,:);
m2 = in2(3,:);
m3 = in2(4,:);
m4 = in2(5,:);
l_AC = in2(22,:);
l_A_m3 = in2(18,:);
l_B_m2 = in2(17,:);
l_C_m4 = in2(19,:);
l_OA = in2(20,:);
l_OB = in2(21,:);
l_O_m1 = in2(16,:);
m1 = in2(6,:);
m2 = in2(7,:);
m3 = in2(8,:);
m4 = in2(9,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,25 +3,29 @@ function Grav_Joint_Sp = Grav_leg(in1,in2)
% GRAV_JOINT_SP = GRAV_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
g = in2(20,:);
l_AC = in2(18,:);
l_A_m3 = in2(14,:);
l_B_m2 = in2(13,:);
l_C_m4 = in2(15,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
l_O_m1 = in2(12,:);
m0 = in2(1,:);
m1 = in2(2,:);
m2 = in2(3,:);
m3 = in2(4,:);
m4 = in2(5,:);
g = in2(24,:);
kappa1 = in2(3,:);
kappa2 = in2(4,:);
l_AC = in2(22,:);
l_A_m3 = in2(18,:);
l_B_m2 = in2(17,:);
l_C_m4 = in2(19,:);
l_OA = in2(20,:);
l_OB = in2(21,:);
l_O_m1 = in2(16,:);
m0 = in2(5,:);
m1 = in2(6,:);
m2 = in2(7,:);
m3 = in2(8,:);
m4 = in2(9,:);
th01 = in2(1,:);
th1 = in1(1,:);
th02 = in2(2,:);
th2 = in1(2,:);
t2 = sin(th1);
t3 = th1+th2;
t4 = l_OA.*t2;
t5 = sin(t3);
Grav_Joint_Sp = [g.*m2.*(l_B_m2.*t5+l_OB.*t2)+g.*m3.*(t4+l_A_m3.*t5)+g.*m4.*(t4+l_AC.*t5+l_C_m4.*t2)+g.*l_O_m1.*m1.*t2;g.*t5.*(l_AC.*m4+l_A_m3.*m3+l_B_m2.*m2);0.0;g.*(m0+m1+m2+m3+m4)];
Grav_Joint_Sp = [kappa1.*(th01.*2.0-th1.*2.0).*(-1.0./2.0)+g.*m2.*(l_B_m2.*t5+l_OB.*t2)+g.*m3.*(t4+l_A_m3.*t5)+g.*m4.*(t4+l_AC.*t5+l_C_m4.*t2)+g.*l_O_m1.*m1.*t2;kappa2.*(th02.*2.0-th2.*2.0).*(-1.0./2.0)+g.*l_AC.*m4.*t5+g.*l_A_m3.*m3.*t5+g.*l_B_m2.*m2.*t5;0.0;g.*(m0+m1+m2+m3+m4)];
......@@ -3,28 +3,32 @@ function b = b_leg(in1,in2,in3)
% B = B_LEG(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:17
% 02-Dec-2021 21:48:13
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
g = in3(20,:);
l_AC = in3(18,:);
l_A_m3 = in3(14,:);
l_B_m2 = in3(13,:);
l_C_m4 = in3(15,:);
l_OA = in3(16,:);
l_OB = in3(17,:);
l_O_m1 = in3(12,:);
m0 = in3(1,:);
m1 = in3(2,:);
m2 = in3(3,:);
m3 = in3(4,:);
m4 = in3(5,:);
g = in3(24,:);
kappa1 = in3(3,:);
kappa2 = in3(4,:);
l_AC = in3(22,:);
l_A_m3 = in3(18,:);
l_B_m2 = in3(17,:);
l_C_m4 = in3(19,:);
l_OA = in3(20,:);
l_OB = in3(21,:);
l_O_m1 = in3(16,:);
m0 = in3(5,:);
m1 = in3(6,:);
m2 = in3(7,:);
m3 = in3(8,:);
m4 = in3(9,:);
tau1 = in2(1,:);
tau2 = in2(2,:);
th01 = in3(1,:);
th1 = in1(1,:);
th02 = in3(2,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
......@@ -105,7 +109,8 @@ t75 = -t71;
t76 = -t72;
t79 = -t78;
et1 = tau1+dth1.*((m3.*(t41.*t61.*2.0-t43.*t59.*2.0-t41.*t63.*2.0+t43.*t65.*2.0))./2.0+(m2.*(t42.*t62.*2.0-t44.*t60.*2.0-t42.*t64.*2.0+t44.*t66.*2.0))./2.0+(m4.*(t52.*t68.*2.0-t52.*t70.*2.0-t56.*t67.*2.0+t56.*t69.*2.0))./2.0+(m1.*(l_O_m1.*t3.*t21.*2.0-l_O_m1.*t2.*t28.*2.0))./2.0)-(m1.*(t14.*t21.*2.0-t13.*t28.*2.0))./2.0+(m3.*(t59.*t63.*2.0-t61.*t65.*2.0))./2.0+(m2.*(t60.*t64.*2.0-t62.*t66.*2.0))./2.0+(m4.*(t67.*t70.*2.0-t68.*t69.*2.0))./2.0;
et2 = dth2.*((m3.*(t71-t73-t43.*t50.*2.0+t41.*t54.*2.0))./2.0+(m2.*(t72-t74-t44.*t51.*2.0+t42.*t55.*2.0))./2.0+(m4.*(t77+t79-t49.*t56.*2.0+t52.*t53.*2.0))./2.0)-g.*m2.*t44-g.*m3.*t43-g.*m4.*t56-g.*l_O_m1.*m1.*t3;
mt1 = [et1+et2,tau2+(m3.*(t50.*t63.*2.0-t54.*t65.*2.0))./2.0+(m2.*(t51.*t64.*2.0-t55.*t66.*2.0))./2.0+(m4.*(t49.*t70.*2.0-t53.*t69.*2.0))./2.0+dth2.*((m3.*(t71-t73-t19.*t50.*2.0+t16.*t54.*2.0))./2.0+(m2.*(t72-t74-t20.*t51.*2.0+t17.*t55.*2.0))./2.0+(m4.*(t77+t79-t18.*t49.*2.0+t15.*t53.*2.0))./2.0)+dth1.*((m3.*(t71-t73+t16.*t61.*2.0-t19.*t59.*2.0))./2.0+(m2.*(t72-t74+t17.*t62.*2.0-t20.*t60.*2.0))./2.0+(m4.*(t77+t79+t15.*t68.*2.0-t18.*t67.*2.0))./2.0)-g.*m2.*t20-g.*m3.*t19-g.*m4.*t18];
mt2 = [dth2.*((m4.*(t29.*2.0+t38))./2.0+(m3.*(t31.*2.0+t39))./2.0+(m2.*(t33.*2.0+t40))./2.0)+dth1.*(m1.*t14+(m3.*(t39+t47.*2.0))./2.0+(m2.*(t40+t48.*2.0))./2.0+(m4.*(t38+t57.*2.0))./2.0),-g.*m0-g.*m1-g.*m2-g.*m3-g.*m4-dth2.*((m4.*(t22.*2.0+t35))./2.0+(m3.*(t24.*2.0+t36))./2.0+(m2.*(t26.*2.0+t37))./2.0)-dth1.*(m1.*t13+(m3.*(t36+t45.*2.0))./2.0+(m2.*(t37+t46.*2.0))./2.0+(m4.*(t35+t58.*2.0))./2.0)];
b = reshape([mt1,mt2],4,1);
et2 = dth2.*((m3.*(t71-t73-t43.*t50.*2.0+t41.*t54.*2.0))./2.0+(m2.*(t72-t74-t44.*t51.*2.0+t42.*t55.*2.0))./2.0+(m4.*(t77+t79-t49.*t56.*2.0+t52.*t53.*2.0))./2.0)+(kappa1.*(th01.*2.0-th1.*2.0))./2.0-g.*m2.*t44-g.*m3.*t43-g.*m4.*t56-g.*l_O_m1.*m1.*t3;
mt1 = [et1+et2];
mt2 = [tau2+(m3.*(t50.*t63.*2.0-t54.*t65.*2.0))./2.0+(m2.*(t51.*t64.*2.0-t55.*t66.*2.0))./2.0+(m4.*(t49.*t70.*2.0-t53.*t69.*2.0))./2.0+dth2.*((m3.*(t71-t73-t19.*t50.*2.0+t16.*t54.*2.0))./2.0+(m2.*(t72-t74-t20.*t51.*2.0+t17.*t55.*2.0))./2.0+(m4.*(t77+t79-t18.*t49.*2.0+t15.*t53.*2.0))./2.0)+dth1.*((m3.*(t71-t73+t16.*t61.*2.0-t19.*t59.*2.0))./2.0+(m2.*(t72-t74+t17.*t62.*2.0-t20.*t60.*2.0))./2.0+(m4.*(t77+t79+t15.*t68.*2.0-t18.*t67.*2.0))./2.0)+(kappa2.*(th02.*2.0-th2.*2.0))./2.0-g.*m2.*t20-g.*m3.*t19-g.*m4.*t18];
mt3 = [dth2.*((m4.*(t29.*2.0+t38))./2.0+(m3.*(t31.*2.0+t39))./2.0+(m2.*(t33.*2.0+t40))./2.0)+dth1.*(m1.*t14+(m3.*(t39+t47.*2.0))./2.0+(m2.*(t40+t48.*2.0))./2.0+(m4.*(t38+t57.*2.0))./2.0),-g.*m0-g.*m1-g.*m2-g.*m3-g.*m4-dth2.*((m4.*(t22.*2.0+t35))./2.0+(m3.*(t24.*2.0+t36))./2.0+(m2.*(t26.*2.0+t37))./2.0)-dth1.*(m1.*t13+(m3.*(t36+t45.*2.0))./2.0+(m2.*(t37+t46.*2.0))./2.0+(m4.*(t35+t58.*2.0))./2.0)];
b = reshape([mt1,mt2,mt3],4,1);
......@@ -3,32 +3,36 @@ function E = energy_leg(in1,in2)
% E = ENERGY_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
I1 = in2(6,:);
I2 = in2(7,:);
I3 = in2(8,:);
I4 = in2(9,:);
Ir = in2(10,:);
N = in2(11,:);
I1 = in2(10,:);
I2 = in2(11,:);
I3 = in2(12,:);
I4 = in2(13,:);
Ir = in2(14,:);
N = in2(15,:);
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
g = in2(20,:);
l_AC = in2(18,:);
l_A_m3 = in2(14,:);
l_B_m2 = in2(13,:);
l_C_m4 = in2(15,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
l_O_m1 = in2(12,:);
m0 = in2(1,:);
m1 = in2(2,:);
m2 = in2(3,:);
m3 = in2(4,:);
m4 = in2(5,:);
g = in2(24,:);
kappa1 = in2(3,:);
kappa2 = in2(4,:);
l_AC = in2(22,:);
l_A_m3 = in2(18,:);
l_B_m2 = in2(17,:);
l_C_m4 = in2(19,:);
l_OA = in2(20,:);
l_OB = in2(21,:);
l_O_m1 = in2(16,:);
m0 = in2(5,:);
m1 = in2(6,:);
m2 = in2(7,:);
m3 = in2(8,:);
m4 = in2(9,:);
th01 = in2(1,:);
th1 = in1(1,:);
th02 = in2(2,:);
th2 = in1(2,:);
y = in1(4,:);
t2 = cos(th1);
......@@ -47,6 +51,6 @@ t14 = t4.^2;
t15 = l_AC.*t10;
t16 = l_A_m3.*t10;
t17 = l_B_m2.*t10;
et1 = (I1.*t6)./2.0+(I4.*t6)./2.0+(I2.*t14)./2.0+(I3.*t14)./2.0+(m3.*((dx+dth1.*(t8+t16)+dth2.*t16).^2+(dy+dth1.*(t11+l_A_m3.*t12)+dth2.*l_A_m3.*t12).^2))./2.0+(m0.*(dx.^2+dy.^2))./2.0+(m1.*((dx+dth1.*l_O_m1.*t2).^2+(dy+dth1.*l_O_m1.*t3).^2))./2.0+(Ir.*(dth1+N.*dth2).^2)./2.0+(m4.*((dx+dth2.*t15+dth1.*(t7+t8+t15)).^2+(dy+dth1.*(t11+l_AC.*t12+l_C_m4.*t3)+dth2.*l_AC.*t12).^2))./2.0+(m2.*((dx+dth1.*(t9+t17)+dth2.*t17).^2+(dy+dth1.*(l_B_m2.*t12+l_OB.*t3)+dth2.*l_B_m2.*t12).^2))./2.0+g.*m1.*(y-l_O_m1.*t2)-g.*m4.*(t7+t8+t13+t15)+g.*m0.*y;
et2 = (Ir.*N.^2.*t6)./2.0-g.*m3.*(t8+t13+t16)-g.*m2.*(t9+t13+t17);
et1 = (I1.*t6)./2.0+(I4.*t6)./2.0+(I2.*t14)./2.0+(I3.*t14)./2.0+(m3.*((dx+dth1.*(t8+t16)+dth2.*t16).^2+(dy+dth1.*(t11+l_A_m3.*t12)+dth2.*l_A_m3.*t12).^2))./2.0+(m0.*(dx.^2+dy.^2))./2.0+(m1.*((dx+dth1.*l_O_m1.*t2).^2+(dy+dth1.*l_O_m1.*t3).^2))./2.0+(kappa1.*(th01-th1).^2)./2.0+(kappa2.*(th02-th2).^2)./2.0+(Ir.*(dth1+N.*dth2).^2)./2.0+(m4.*((dx+dth2.*t15+dth1.*(t7+t8+t15)).^2+(dy+dth1.*(t11+l_AC.*t12+l_C_m4.*t3)+dth2.*l_AC.*t12).^2))./2.0;
et2 = (m2.*((dx+dth1.*(t9+t17)+dth2.*t17).^2+(dy+dth1.*(l_B_m2.*t12+l_OB.*t3)+dth2.*l_B_m2.*t12).^2))./2.0+g.*m1.*(y-l_O_m1.*t2)-g.*m4.*(t7+t8+t13+t15)+g.*m0.*y+(Ir.*N.^2.*t6)./2.0-g.*m3.*(t8+t13+t16)-g.*m2.*(t9+t13+t17);
E = et1+et2;
......@@ -3,13 +3,13 @@ function dJ = jacobian_dot_foot(in1,in2)
% DJ = JACOBIAN_DOT_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
dth1 = in1(5,:);
dth2 = in1(6,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
l_AC = in2(22,:);
l_DE = in2(23,:);
l_OB = in2(21,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,11 +3,11 @@ function J = jacobian_foot(in1,in2)
% J = JACOBIAN_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
l_AC = in2(22,:);
l_DE = in2(23,:);
l_OB = in2(21,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,12 +3,12 @@ function keypoints = keypoints_leg(in1,in2)
% KEYPOINTS = KEYPOINTS_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:19
% 02-Dec-2021 21:48:15
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
l_AC = in2(22,:);
l_DE = in2(23,:);
l_OA = in2(20,:);
l_OB = in2(21,:);
th1 = in1(1,:);
th2 = in1(2,:);
x = in1(3,:);
......
......@@ -3,11 +3,11 @@ function rE = position_foot(in1,in2)
% RE = POSITION_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
l_AC = in2(22,:);
l_DE = in2(23,:);
l_OB = in2(21,:);
th1 = in1(1,:);
th2 = in1(2,:);
x = in1(3,:);
......
......@@ -3,15 +3,15 @@ function drE = velocity_foot(in1,in2)
% DRE = VELOCITY_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 23-Nov-2021 21:44:18
% 02-Dec-2021 21:48:14
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
l_AC = in2(22,:);
l_DE = in2(23,:);
l_OB = in2(21,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -7,6 +7,7 @@ syms m0 m1 m2 m3 m4 I1 I2 I3 I4 l_O_m1 l_B_m2 l_A_m3 l_C_m4 g real
syms l_OA l_OB l_AC l_DE real
syms tau1 tau2 Fx Fy real
syms Ir N real
syms th01 th02 kappa1 kappa2 real
% Group them
q = [th1 ; th2; x; y]; % generalized coordinates
......@@ -15,7 +16,7 @@ ddq = [ddth1;ddth2; ddx; ddy]; % second time derivatives
u = [tau1 ; tau2]; % controls
F = [Fx ; Fy];
p = [m0 m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]'; % parameters
p = [th01 th02 kappa1 kappa2 m0 m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]'; % parameters
% Generate Vectors and Derivativess
O = [x ; y ; 0];
......@@ -81,8 +82,12 @@ Vg2 = m2*g*dot(r_m2, -ihat);
Vg3 = m3*g*dot(r_m3, -ihat);
Vg4 = m4*g*dot(r_m4, -ihat);
Ve1 = 1/2*kappa1*(th1 - th01)^2;
Ve2 = 1/2*kappa2*(th2 - th02)^2;
T = simplify(T0 + T1 + T2 + T3 + T4 + T1r + T2r);
Vg = Vg0 + Vg1 + Vg2 + Vg3 + Vg4;
Vg = Vg0 + Vg1 + Vg2 + Vg3 + Vg4 + Ve1 +Ve2;
Q_tau1 = M2Q(tau1*khat,omega1*khat);
Q_tau2 = M2Q(tau2*khat,omega2*khat);
Q_tau2R= M2Q(-tau2*khat,omega1*khat);
......
......@@ -24,12 +24,18 @@ function p=get_parameters()
l_A_m3=0.0600; l_C_m4=0.065;
th01= -177.27*pi/180;
th02= 180*pi/180;
kappa1=0.00;
kappa2=0.00;
N = 18.75;
Ir = 0.0035/N^2;
g = 9.81/2;
%% Parameter vector
% p = [m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]';
p = [m0 m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]'; % parameters
p = [th01 th02 kappa1 kappa2 m0 m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]'; % parameters
end
\ No newline at end of file
function u=interpolate(ctrl_pt,t)
n = length(ctrl_pt)-1;
x = 0:(1/n):1;
u = interp1(x,ctrl_pt,t);
end
\ No newline at end of file
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
function [t_out, z_out, u_out,s_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
% ctrl1.T =[ctrl1.T ctrl1.T(1)];
% ctrl2.T =[ctrl2.T ctrl2.T(1)];
%% to remove
p_traj.omega = 30;
p_traj.omega = 3;
......@@ -18,9 +19,15 @@ function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
u_out = zeros(2,num_step);
z_out = zeros(8,num_step);
s_out = zeros(num_step,1);
z_out(:,1) = z0;
for i=1:num_step-1
rE = position_foot(z_out(:,i), p);
if rE(2) > ground.ground_height
s_out(i)=1;
end
[dz,u] = dynamics(tspan(i), z_out(:,i), p, ctrl1,ctrl2);
% dz = dynamics(tspan(i), z_out(:,i), p, p_traj);
......@@ -28,11 +35,10 @@ function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
% z_out(5:8,i+1) = joint_limit_constraint1(z_out(:,i+1),p);
z_out(:,i+1) = joint_limit_constraint(z_out(:,i+1),p);
z_out(5:8,i+1) = joint_limit_constraint1(z_out(:,i+1),p);
% z_out(:,i+1) = joint_limit_constraint(z_out(:,i+1),p);
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, ground);
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt;
......@@ -66,6 +72,10 @@ end
function tau = control_law_torque(t, z, p,ctrl1,ctrl2 )
tau1 = BezierCurve(ctrl1.T, t/ctrl1.tf);
tau2 = BezierCurve(ctrl2.T, t/ctrl2.tf);
tau1 = interpolate(ctrl1.T, t/ctrl1.tf);
tau2 = interpolate(ctrl2.T, t/ctrl2.tf);
tau =[tau1;tau2];
end
......@@ -193,27 +203,36 @@ end
%% joint Constraint
function z1 = joint_limit_constraint(z,p)
q2_min=(90-45)*pi/180;
q2_max=(90+55)*pi/180;
q1_min = (-90) * pi/ 180;
q1_max = (90) * pi/ 180;
q2_min=(90-50)*pi/180;
q2_max=(90+50)*pi/180;
q2_max=(90+55)*pi/180;
z1=z;
qdot=z(5:6);
% if z(2)<(90-45)*pi/180 || z(2)>(90+55)*pi/180
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if z(1)<q1_min || z(2)>q1_max
z1(5)=0;
end
if z(1)<q1_min
z1(1)=q1_min;
end
if z(1)>q1_max
z1(1)=q1_max;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if z(2)<q2_min || z(2)>q2_max
qdot=[0;0];
z1(6)=0;
end
if z(2)<q2_min
z1(2)=q2_min;
end
if z(2)>q2_max
z1(2)=q2_max;
end
% if z(2)<q2_min
% z1(2)=q2_min;
% end
% if z(2)>q2_max
% z1(2)=q2_max;
% end
z1(5:6)=qdot;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
......
function [t_out, z_out, u_out] =simulate_leg_casadi(z0,p,ground,ctrl1,ctrl2,ctrl3)
num_step=ctrl1.n+ctrl2.n+ctrl3.n;
% Declare casadi symbolic variables
t_out = casadi.MX(zeros(1,num_step));
z_out = casadi.MX(zeros(8,num_step));
u_out = casadi.MX(zeros(2,num_step-1));
% Time discretization
t_out(1) = 0;
dt1 = ctrl1.tf/(ctrl1.n-1);
dt2 = ctrl1.tf/(ctrl2.n-1);
dt3 = ctrl1.tf/(ctrl3.n-1);
i=2;
for j = 1:ctrl1.n-1
t_out(i) = t_out(i-1) + dt1;
i=i+1;
end
for j = 1:ctrl2.n
t_out(i) = t_out(i-1) + dt2;
i=i+1;
end
for j = 1:ctrl3.n
t_out(i) = t_out(i-1) + dt3;
i=i+1;
end
%% Perform Dynamic simulation
z_out(:,1) = z0;
for j=1:ctrl1.n-1
i=j;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl1);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt1;
z_out(5:8,i+1) = discrete_impact_contact_stance(z_out(:,i+1),p, ground);
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt1 ;
u_out(:,i)=u;
end
for j=1:ctrl2.n
i=j+ctrl1.n;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl2);