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

first commit

parent fbe0c4ba
function A = A_leg(in1,in2)
%A_LEG
% A = A_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:33
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = N.^2;
t6 = l_AC.^2;
t7 = l_A_m3.^2;
t8 = l_B_m2.^2;
t9 = l_O_m1.^2;
t10 = Ir.*N;
t20 = m1+m2+m3+m4;
t11 = l_C_m4.*t2;
t12 = l_OA.*t2;
t13 = l_OB.*t2;
t14 = cos(t4);
t15 = l_C_m4.*t3;
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;
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;
t40 = t34.*2.0;
t41 = t35.*2.0;
t42 = t36.*2.0;
t43 = m4.*t31;
t44 = m3.*t32;
t45 = m2.*t33;
t46 = m4.*t34;
t47 = m3.*t35;
t48 = m2.*t36;
t49 = t12+t32;
t50 = t13+t33;
t51 = t16+t35;
t52 = t17+t36;
t57 = t11+t12+t31;
t58 = t15+t16+t34;
t53 = t24+t38;
t54 = t25+t39;
t55 = t27+t41;
t56 = t28+t42;
t59 = t23+t24+t37;
t60 = t41.*t51;
t61 = t42.*t52;
t64 = t26+t27+t40;
t67 = t38.*t49;
t68 = t39.*t50;
t69 = t43+t44+t45;
t70 = t46+t47+t48;
t71 = t37.*t57;
t72 = t40.*t58;
t62 = (m3.*t53)./2.0;
t63 = (m2.*t54)./2.0;
t65 = (m3.*t55)./2.0;
t66 = (m2.*t56)./2.0;
t73 = (m4.*t64)./2.0;
t74 = (m4.*t59)./2.0;
t75 = t60+t67;
t76 = t61+t68;
t79 = t71+t72;
t77 = (m3.*t75)./2.0;
t78 = (m2.*t76)./2.0;
t80 = (m4.*t79)./2.0;
t81 = t21+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]);
function Corr_Joint_Sp = Corr_leg(in1,in2)
%CORR_LEG
% 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
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = sin(th2);
t5 = th1+th2;
t6 = dth1.^2;
t7 = dth2.^2;
t8 = l_AC.*l_C_m4.*m4;
t9 = l_AC.*l_OA.*m4;
t10 = l_A_m3.*l_OA.*m3;
t11 = l_B_m2.*l_OB.*m2;
t12 = cos(t5);
t13 = l_OA.*t3;
t14 = sin(t5);
t18 = t8+t9+t10+t11;
t15 = dth2.*l_AC.*t14.*2.0;
t16 = dth2.*l_A_m3.*t14.*2.0;
t17 = dth2.*l_B_m2.*t14.*2.0;
Corr_Joint_Sp = [-dth2.*t4.*t18.*(dth1.*2.0+dth2);t4.*t6.*t18;-dth1.*((m2.*(t17+dth1.*(l_B_m2.*t14+l_OB.*t3).*2.0))./2.0+(m3.*(t16+dth1.*(t13+l_A_m3.*t14).*2.0))./2.0+(m4.*(t15+dth1.*(t13+l_AC.*t14+l_C_m4.*t3).*2.0))./2.0+dth1.*l_O_m1.*m1.*t3)-dth2.*((m4.*(t15+dth1.*l_AC.*t14.*2.0))./2.0+(m3.*(t16+dth1.*l_A_m3.*t14.*2.0))./2.0+(m2.*(t17+dth1.*l_B_m2.*t14.*2.0))./2.0);l_AC.*m4.*t6.*t12+l_AC.*m4.*t7.*t12+l_A_m3.*m3.*t6.*t12+l_A_m3.*m3.*t7.*t12+l_B_m2.*m2.*t6.*t12+l_B_m2.*m2.*t7.*t12+l_C_m4.*m4.*t2.*t6+l_OA.*m3.*t2.*t6+l_OB.*m2.*t2.*t6+l_OA.*m4.*t2.*t6+l_O_m1.*m1.*t2.*t6+dth1.*dth2.*l_AC.*m4.*t12.*2.0+dth1.*dth2.*l_A_m3.*m3.*t12.*2.0+dth1.*dth2.*l_B_m2.*m2.*t12.*2.0];
function Grav_Joint_Sp = Grav_leg(in1,in2)
%GRAV_LEG
% 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
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,:);
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)];
function b = b_leg(in1,in2,in3)
%B_LEG
% B = B_LEG(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:33
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,:);
tau1 = in2(1,:);
tau2 = in2(2,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = l_C_m4.*t2;
t6 = l_OA.*t2;
t7 = l_OB.*t2;
t8 = cos(t4);
t9 = l_C_m4.*t3;
t10 = l_OA.*t3;
t11 = l_OB.*t3;
t12 = sin(t4);
t13 = dth1.*l_O_m1.*t2;
t14 = dth1.*l_O_m1.*t3;
t15 = l_AC.*t8;
t16 = l_A_m3.*t8;
t17 = l_B_m2.*t8;
t18 = l_AC.*t12;
t19 = l_A_m3.*t12;
t20 = l_B_m2.*t12;
t21 = dx+t13;
t28 = dy+t14;
t22 = dth1.*t15;
t23 = dth2.*t15;
t24 = dth1.*t16;
t25 = dth2.*t16;
t26 = dth1.*t17;
t27 = dth2.*t17;
t29 = dth1.*t18;
t30 = dth2.*t18;
t31 = dth1.*t19;
t32 = dth2.*t19;
t33 = dth1.*t20;
t34 = dth2.*t20;
t41 = t6+t16;
t42 = t7+t17;
t43 = t10+t19;
t44 = t11+t20;
t52 = t5+t6+t15;
t56 = t9+t10+t18;
t35 = t23.*2.0;
t36 = t25.*2.0;
t37 = t27.*2.0;
t38 = t30.*2.0;
t39 = t32.*2.0;
t40 = t34.*2.0;
t45 = dth1.*t41;
t46 = dth1.*t42;
t47 = dth1.*t43;
t48 = dth1.*t44;
t49 = t22+t23;
t50 = t24+t25;
t51 = t26+t27;
t53 = t29+t30;
t54 = t31+t32;
t55 = t33+t34;
t57 = dth1.*t56;
t58 = dth1.*t52;
t59 = t25+t45;
t60 = t27+t46;
t61 = t32+t47;
t62 = t34+t48;
t67 = t23+t58;
t68 = t30+t57;
t63 = dy+t61;
t64 = dy+t62;
t65 = dx+t59;
t66 = dx+t60;
t69 = dx+t67;
t70 = dy+t68;
t71 = t19.*t65.*2.0;
t72 = t20.*t66.*2.0;
t73 = t16.*t63.*2.0;
t74 = t17.*t64.*2.0;
t77 = t18.*t69.*2.0;
t78 = t15.*t70.*2.0;
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.*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);
function E = energy_leg(in1,in2)
%ENERGY_LEG
% E = ENERGY_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
I1 = in2(5,:);
I2 = in2(6,:);
I3 = in2(7,:);
I4 = in2(8,:);
Ir = in2(9,:);
N = in2(10,:);
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,:);
th1 = in1(1,:);
th2 = in1(2,:);
y = in1(4,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = dth1+dth2;
t5 = th1+th2;
t6 = dth1.^2;
t13 = -y;
t7 = l_C_m4.*t2;
t8 = l_OA.*t2;
t9 = l_OB.*t2;
t10 = cos(t5);
t11 = l_OA.*t3;
t12 = sin(t5);
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);
E = et1+et2;
function dJ = jacobian_dot_foot(in1,in2)
%JACOBIAN_DOT_FOOT
% DJ = JACOBIAN_DOT_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
dth1 = in1(5,:);
dth2 = in1(6,:);
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = cos(t4);
t6 = sin(t4);
t7 = dth2.*l_AC.*t5;
t8 = dth2.*l_AC.*t6;
t9 = -t8;
dJ = reshape([t9-dth1.*(l_AC.*t6+l_DE.*t3+l_OB.*t3),t7+dth1.*(l_AC.*t5+l_DE.*t2+l_OB.*t2),t9-dth1.*l_AC.*t6,t7+dth1.*l_AC.*t5,0.0,0.0,0.0,0.0],[2,4]);
function J = jacobian_foot(in1,in2)
%JACOBIAN_FOOT
% J = JACOBIAN_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = cos(t4);
t6 = sin(t4);
t7 = l_AC.*t5;
t8 = l_AC.*t6;
J = reshape([t7+l_DE.*t2+l_OB.*t2,t8+l_DE.*t3+l_OB.*t3,t7,t8,1.0,0.0,0.0,1.0],[2,4]);
function keypoints = keypoints_leg(in1,in2)
%KEYPOINTS_LEG
% KEYPOINTS = KEYPOINTS_LEG(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:35
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OA = in2(15,:);
l_OB = in2(16,:);
th1 = in1(1,:);
th2 = in1(2,:);
x = in1(3,:);
y = in1(4,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = l_OA.*t2;
t6 = l_OB.*t2;
t7 = cos(t4);
t8 = l_OA.*t3;
t9 = l_OB.*t3;
t10 = sin(t4);
t11 = l_AC.*t7;
t12 = l_AC.*t10;
t13 = -t5;
t14 = -t6;
t15 = -t11;
keypoints = reshape([t8+x,t13+y,t9+x,t14+y,t8+t12+x,t13+t15+y,t9+t12+x,t14+t15+y,t9+t12+x+l_DE.*t3,t14+t15+y-l_DE.*t2,x,y],[2,6]);
function rE = position_foot(in1,in2)
%POSITION_FOOT
% RE = POSITION_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
th1 = in1(1,:);
th2 = in1(2,:);
x = in1(3,:);
y = in1(4,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
rE = [x+l_DE.*t3+l_OB.*t3+l_AC.*sin(t4);y-l_DE.*t2-l_OB.*t2-l_AC.*cos(t4)];
function drE = velocity_foot(in1,in2)
%VELOCITY_FOOT
% DRE = VELOCITY_FOOT(IN1,IN2)
% This function was generated by the Symbolic Math Toolbox version 8.7.
% 05-Nov-2021 17:43:34
dth1 = in1(5,:);
dth2 = in1(6,:);
dx = in1(7,:);
dy = in1(8,:);
l_AC = in2(17,:);
l_DE = in2(18,:);
l_OB = in2(16,:);
th1 = in1(1,:);
th2 = in1(2,:);
t2 = cos(th1);
t3 = sin(th1);
t4 = th1+th2;
t5 = cos(t4);
t6 = sin(t4);
drE = [dx+dth1.*(l_AC.*t5+l_DE.*t2+l_OB.*t2)+dth2.*l_AC.*t5;dy+dth1.*(l_AC.*t6+l_DE.*t3+l_OB.*t3)+dth2.*l_AC.*t6];
clear
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 l_OA l_OB l_AC l_DE real
syms tau1 tau2 Fx Fy real
syms Ir N real
% Group them
q = [th1 ; th2; x; y]; % generalized coordinates
dq = [dth1 ; dth2; dx; dy]; % first time derivatives
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
% Generate Vectors and Derivativess
O = [x ; y ; 0];
ihat = [0; -1; 0];
jhat = [1; 0; 0];
xhat = [1; 0; 0];
yhat = [0; 1; 0];
khat = cross(ihat,jhat);
e1hat = cos(th1)*ihat + sin(th1)*jhat;
e2hat = cos(th1+th2)*ihat + sin(th1+th2)*jhat;
ddt = @(r) jacobian(r,[q;dq])*[dq;ddq]; % a handy anonymous function for taking time derivatives
rO=O;
rA = rO + l_OA * e1hat;
rB = rO + l_OB * e1hat;
rC = rA + l_AC * e2hat;
rD = rB + l_AC * e2hat;
rE = rD + l_DE * e1hat;
r_m1 = rO + l_O_m1 * e1hat;
r_m2 = rB + l_B_m2 * e2hat;
r_m3 = rA + l_A_m3 * e2hat;
r_m4 = rC + l_C_m4 * e1hat;
drO = ddt(O);
drA = ddt(rA);
drB = ddt(rB);
drC = ddt(rC);
drD = ddt(rD);
drE = ddt(rE);
dr_m1 = ddt(r_m1);
dr_m2 = ddt(r_m2);
dr_m3 = ddt(r_m3);
dr_m4 = ddt(r_m4);
% Calculate Kinetic Energy, Potential Energy, and Generalized Forces
F2Q = @(F,r) simplify(jacobian(r,q)'*(F)); % force contributions to generalized forces
M2Q = @(M,w) simplify(jacobian(w,dq)'*(M)); % moment contributions to generalized forces
omega1 = dth1;
omega2 = dth1 + dth2;
omega3 = dth1 + dth2;
omega4 = dth1;
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;
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;
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;
Q_tau1 = M2Q(tau1*khat,omega1*khat);
Q_tau2 = M2Q(tau2*khat,omega2*khat);
Q_tau2R= M2Q(-tau2*khat,omega1*khat);
Q_tau = Q_tau1+Q_tau2 + Q_tau2R;
Q = Q_tau;
% Assemble the array of cartesian coordinates of the key points
keypoints = [rA(1:2) rB(1:2) rC(1:2) rD(1:2) rE(1:2) rO(1:2)];
%% All the work is done! Just turn the crank...
% Derive Energy Function and Equations of Motion
E = T+Vg;
L = T-Vg;
eom = ddt(jacobian(L,dq).') - jacobian(L,q).' - Q;
% Rearrange Equations of Motion
A = jacobian(eom,ddq);
b = A*ddq - eom;
% Equations of motion are
% eom = A *ddq + (coriolis term) + (gravitational term) - Q = 0
Mass_Joint_Sp = A;
Grav_Joint_Sp = simplify(jacobian(Vg, q)');
Corr_Joint_Sp = simplify( eom + Q - Grav_Joint_Sp - A*ddq);