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

fmincon added variables

parent 9f1a190d
......@@ -3,25 +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.
% 05-Nov-2021 17:43:33
% 23-Nov-2021 21:44:16
I1 = in2(5,:);
I2 = in2(6,:);
I3 = in2(7,:);
I4 = in2(8,:);
Ir = in2(9,:);
N = in2(10,:);
l_AC = in2(17,:);
l_A_m3 = in2(13,:);
l_B_m2 = in2(12,:);
l_C_m4 = in2(14,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
l_O_m1 = in2(11,:);
m1 = in2(1,:);
m2 = in2(2,:);
m3 = in2(3,:);
m4 = in2(4,:);
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......@@ -33,7 +34,7 @@ t7 = l_A_m3.^2;
t8 = l_B_m2.^2;
t9 = l_O_m1.^2;
t10 = Ir.*N;
t20 = m1+m2+m3+m4;
t33 = m0+m1+m2+m3+m4;
t11 = l_C_m4.*t2;
t12 = l_OA.*t2;
t13 = l_OB.*t2;
......@@ -43,48 +44,48 @@ t16 = l_OA.*t3;
t17 = l_OB.*t3;
t18 = sin(t4);
t19 = l_O_m1.*m1.*t2;
t21 = l_O_m1.*m1.*t3;
t22 = Ir.*t5;
t23 = t11.*2.0;
t24 = t12.*2.0;
t25 = t13.*2.0;
t26 = t15.*2.0;
t27 = t16.*2.0;
t28 = t17.*2.0;
t29 = t14.^2;
t30 = t18.^2;
t31 = l_AC.*t14;
t32 = l_A_m3.*t14;
t33 = l_B_m2.*t14;
t20 = l_O_m1.*m1.*t3;
t21 = Ir.*t5;
t22 = t11.*2.0;
t23 = t12.*2.0;
t24 = t13.*2.0;
t25 = t15.*2.0;
t26 = t16.*2.0;
t27 = t17.*2.0;
t28 = t14.^2;
t29 = t18.^2;
t30 = l_AC.*t14;
t31 = l_A_m3.*t14;
t32 = l_B_m2.*t14;
t34 = l_AC.*t18;
t35 = l_A_m3.*t18;
t36 = l_B_m2.*t18;
t37 = t31.*2.0;
t38 = t32.*2.0;
t39 = t33.*2.0;
t37 = t30.*2.0;
t38 = t31.*2.0;
t39 = t32.*2.0;
t40 = t34.*2.0;
t41 = t35.*2.0;
t42 = t36.*2.0;
t43 = m4.*t31;
t44 = m3.*t32;
t45 = m2.*t33;
t43 = m4.*t30;
t44 = m3.*t31;
t45 = m2.*t32;
t46 = m4.*t34;
t47 = m3.*t35;
t48 = m2.*t36;
t49 = t12+t32;
t50 = t13+t33;
t49 = t12+t31;
t50 = t13+t32;
t51 = t16+t35;
t52 = t17+t36;
t57 = t11+t12+t31;
t57 = t11+t12+t30;
t58 = t15+t16+t34;
t53 = t24+t38;
t54 = t25+t39;
t55 = t27+t41;
t56 = t28+t42;
t59 = t23+t24+t37;
t53 = t23+t38;
t54 = t24+t39;
t55 = t26+t41;
t56 = t27+t42;
t59 = t22+t23+t37;
t60 = t41.*t51;
t61 = t42.*t52;
t64 = t26+t27+t40;
t64 = t25+t26+t40;
t67 = t38.*t49;
t68 = t39.*t50;
t69 = t43+t44+t45;
......@@ -103,7 +104,7 @@ t79 = t71+t72;
t77 = (m3.*t75)./2.0;
t78 = (m2.*t76)./2.0;
t80 = (m4.*t79)./2.0;
t81 = t21+t65+t66+t73;
t81 = t20+t65+t66+t73;
t82 = t19+t62+t63+t74;
t83 = I2+I3+t10+t77+t78+t80;
A = reshape([I1+I2+I3+I4+Ir+t22+(m1.*(t2.^2.*t9.*2.0+t3.^2.*t9.*2.0))./2.0+(m3.*(t49.^2.*2.0+t51.^2.*2.0))./2.0+(m2.*(t50.^2.*2.0+t52.^2.*2.0))./2.0+(m4.*(t57.^2.*2.0+t58.^2.*2.0))./2.0,t83,t82,t81,t83,I2+I3+t22+(m4.*(t6.*t29.*2.0+t6.*t30.*2.0))./2.0+(m3.*(t7.*t29.*2.0+t7.*t30.*2.0))./2.0+(m2.*(t8.*t29.*2.0+t8.*t30.*2.0))./2.0,t69,t70,t82,t69,t20,0.0,t81,t70,0.0,t20],[4,4]);
A = reshape([I1+I2+I3+I4+Ir+t21+(m1.*(t2.^2.*t9.*2.0+t3.^2.*t9.*2.0))./2.0+(m3.*(t49.^2.*2.0+t51.^2.*2.0))./2.0+(m2.*(t50.^2.*2.0+t52.^2.*2.0))./2.0+(m4.*(t57.^2.*2.0+t58.^2.*2.0))./2.0,t83,t82,t81,t83,I2+I3+t21+(m4.*(t6.*t28.*2.0+t6.*t29.*2.0))./2.0+(m3.*(t7.*t28.*2.0+t7.*t29.*2.0))./2.0+(m2.*(t8.*t28.*2.0+t8.*t29.*2.0))./2.0,t69,t70,t82,t69,t33,0.0,t81,t70,0.0,t33],[4,4]);
......@@ -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.
% 05-Nov-2021 17:43:35
% 23-Nov-2021 21:44:19
dth1 = in1(5,:);
dth2 = in1(6,:);
l_AC = in2(17,:);
l_A_m3 = in2(13,:);
l_B_m2 = in2(12,:);
l_C_m4 = in2(14,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
l_O_m1 = in2(11,:);
m1 = in2(1,:);
m2 = in2(2,:);
m3 = in2(3,:);
m4 = in2(4,:);
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,24 +3,25 @@ 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.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
g = in2(19,:);
l_AC = in2(17,:);
l_A_m3 = in2(13,:);
l_B_m2 = in2(12,:);
l_C_m4 = in2(14,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
l_O_m1 = in2(11,:);
m1 = in2(1,:);
m2 = in2(2,:);
m3 = in2(3,:);
m4 = in2(4,:);
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,:);
th1 = in1(1,:);
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.*(m1+m2+m3+m4)];
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)];
......@@ -3,24 +3,25 @@ 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.
% 05-Nov-2021 17:43:33
% 23-Nov-2021 21:44:17
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
g = in3(19,:);
l_AC = in3(17,:);
l_A_m3 = in3(13,:);
l_B_m2 = in3(12,:);
l_C_m4 = in3(14,:);
l_OA = in3(15,:);
l_OB = in3(16,:);
l_O_m1 = in3(11,:);
m1 = in3(1,:);
m2 = in3(2,:);
m3 = in3(3,:);
m4 = in3(4,:);
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,:);
tau1 = in2(1,:);
tau2 = in2(2,:);
th1 = in1(1,:);
......@@ -106,5 +107,5 @@ 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.*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)];
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);
......@@ -3,30 +3,31 @@ function E = energy_leg(in1,in2)
% E = ENERGY_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
I1 = in2(5,:);
I2 = in2(6,:);
I3 = in2(7,:);
I4 = in2(8,:);
Ir = in2(9,:);
N = in2(10,:);
I1 = in2(6,:);
I2 = in2(7,:);
I3 = in2(8,:);
I4 = in2(9,:);
Ir = in2(10,:);
N = in2(11,:);
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
g = in2(19,:);
l_AC = in2(17,:);
l_A_m3 = in2(13,:);
l_B_m2 = in2(12,:);
l_C_m4 = in2(14,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
l_O_m1 = in2(11,:);
m1 = in2(1,:);
m2 = in2(2,:);
m3 = in2(3,:);
m4 = in2(4,:);
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
y = in1(4,:);
......@@ -46,6 +47,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+(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)+(Ir.*N.^2.*t6)./2.0;
et2 = -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+(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);
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.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
dth1 = in1(5,:);
dth2 = in1(6,:);
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
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.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
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.
% 05-Nov-2021 17:43:35
% 23-Nov-2021 21:44:19
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OA = in2(16,:);
l_OB = in2(17,:);
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.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
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.
% 05-Nov-2021 17:43:34
% 23-Nov-2021 21:44:18
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
l_AC = in2(18,:);
l_DE = in2(19,:);
l_OB = in2(17,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
......
......@@ -3,7 +3,7 @@ name = 'leg';
% Define variables for time, generalized coordinates + derivatives, controls, and parameters
syms t th1 th2 dth1 dth2 ddth1 ddth2 x y dx dy ddx ddy real
syms m1 m2 m3 m4 I1 I2 I3 I4 l_O_m1 l_B_m2 l_A_m3 l_C_m4 g real
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
......@@ -15,7 +15,7 @@ ddq = [ddth1;ddth2; ddx; ddy]; % second time derivatives
u = [tau1 ; tau2]; % controls
F = [Fx ; Fy];
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]'; % parameters
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
% Generate Vectors and Derivativess
O = [x ; y ; 0];
......@@ -66,6 +66,7 @@ omega2 = dth1 + dth2;
omega3 = dth1 + dth2;
omega4 = dth1;
T0 = (1/2)*m0 * dot(drO,drO);
T1 = (1/2)*m1 * dot(dr_m1,dr_m1) + (1/2) * I1 * omega1^2;
T2 = (1/2)*m2 * dot(dr_m2,dr_m2) + (1/2) * I2 * omega2^2;
T3 = (1/2)*m3 * dot(dr_m3,dr_m3) + (1/2) * I3 * omega3^2;
......@@ -73,13 +74,15 @@ T4 = (1/2)*m4 * dot(dr_m4,dr_m4) + (1/2) * I4 * omega4^2;
T1r = (1/2)*Ir*(N*dth1)^2;
T2r = (1/2)*Ir*(dth1 + N*dth2)^2;
Vg0 = m0*g*dot(rO, -ihat);
Vg1 = m1*g*dot(r_m1, -ihat);
Vg2 = m2*g*dot(r_m2, -ihat);
Vg3 = m3*g*dot(r_m3, -ihat);
Vg4 = m4*g*dot(r_m4, -ihat);
T = simplify(T1 + T2 + T3 + T4 + T1r + T2r);
Vg = Vg1 + Vg2 + Vg3 + Vg4;
T = simplify(T0 + T1 + T2 + T3 + T4 + T1r + T2r);
Vg = Vg0 + Vg1 + Vg2 + Vg3 + Vg4;
Q_tau1 = M2Q(tau1*khat,omega1*khat);
Q_tau2 = M2Q(tau2*khat,omega2*khat);
Q_tau2R= M2Q(-tau2*khat,omega1*khat);
......@@ -125,7 +128,7 @@ J = J(1:2,:);
dJ = dJ(1:2,:);
directory = '../AutoDerived/';
directory = './AutoDerived/';
matlabFunction(A,'file',[directory 'A_' name],'vars',{z p});
matlabFunction(b,'file',[directory 'b_' name],'vars',{z u p});
matlabFunction(E,'file',[directory 'energy_' name],'vars',{z p});
......
......@@ -5,17 +5,31 @@ function p=get_parameters()
I1 = 25.1 * 10^-6; I2 = 53.5 * 10^-6;
I3 = 9.25 * 10^-6; I4 = 22.176 * 10^-6;
% 127mm
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;
%% Definte fixed paramters
m0=0.214;
% m0=0;
m1 =.2607; m2 =.036;
m3 = .009; m4 = .027;
l_OA=0.03673; l_OB=.06773;
l_AB =l_OB-l_OA;
l_AC =.096; l_DE=0.127-l_AB;
l_O_m1=0.051; l_B_m2=0.0345;
l_A_m3=0.0600; l_C_m4=0.065;
N = 18.75;
Ir = 0.0035/N^2;
g = 9.81;
g = 9.81/4;
%% 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 = [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
end
\ No newline at end of file
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
%% to remove
p_traj.omega = 30;
p_traj.omega = 3;
p_traj.x_0 = 0;
p_traj.y_0 = -.125-0.05;
p_traj.r = 0.025*1.5;
%% Perform Dynamic simulation
dt = 0.001;
num_step = floor(tf/dt);
tspan = linspace(0, tf, num_step);
t_out=tspan;
u_out = zeros(2,num_step);
z_out = zeros(8,num_step);
z_out(:,1) = z0;
for i=1:num_step-1
[dz,u] = dynamics(tspan(i), z_out(:,i), p, ctrl1,ctrl2);
% dz = dynamics(tspan(i), z_out(:,i), p, p_traj);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
z_out(5:6,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;
u_out(:,i+1)=u;
end
%% Compute foot position over time
rE = zeros(2,length(tspan));
vE = zeros(2,length(tspan));
for i = 1:length(tspan)
rE(:,i) = position_foot(z_out(:,i),p);
vE(:,i) = velocity_foot(z_out(:,i),p);
end
% Target traj
%TH = 0:.1:2*pi;
%plot( p_traj.x_0 + p_traj.r * cos(TH), ...
%p_traj.y_0 + p_traj.r * sin(TH),'k--');
end
%Torque Control Law
function tau = control_law_torque(t, z, p,ctrl1,ctrl2 )
tau1 = BezierCurve(ctrl1.T, t/ctrl1.tf);
tau2 = BezierCurve(ctrl2.T, t/ctrl2.tf);
tau =[tau1;tau2];
end
%Impedance Control Law
function tau = control_law_Impedance(t, z, p, p_traj)