syms q1 q2 q3 q4 q5 q6 q7 q8 q9
syms dq1 dq2 dq3 dq4 dq5 dq6 dq7 dq8 dq9
eqn1 = q1 + q2 - q7 == 0;
eqn2 = -q2 + q3 + q8 == 0;
eqn3 = -q1 -q3 + q9 == 0;
eqn4 = -q4 + q5 + q7 == 0;
eqn5 = -q5 + q6 - q8 == 0;
[M,~] = equationsToMatrix([eqn1,eqn2,eqn3,eqn4,eqn5,eqn6])
M =

eqn1a = dq1 + dq2 - dq7 == 0;
eqn2a = -dq2 + dq3 + dq8 == 0;
eqn3a = -dq1 -dq3 + dq9 == 0;
eqn4a = -dq4 + dq5 + dq7 == 0;
eqn5a = -dq5 + dq6 - dq8 == 0;
eqn6a = dq4 -dq6 - dq9 == 0;
eqn7 = J*dq1 + K*q1^2 == p3 - p1;
eqn8 = J*dq2 + K*q2^2 == p2 - p1;
eqn9 = J*dq3 + K*q3^2 == p3 - p2;
eqn10 = J*dq4 + K*q4^2 == p4 - p6;
eqn11 = J*dq5 + K*q5^2 == p5 - p4;
eqn12 = J*dq6 + K*q6^2 == p6 - p5;
eqn13 = J*dq7 + K*q7^2 == p1 - p4;
eqn14 = J*dq8 + K*q8^2 == p5 - p2;
eqn15 = J*dq9 + K*q9^2 == p6 - p3;
s1 = solve([eqn1,eqn2,eqn4,eqn5,eqn6],[q1,q2,q4,q5,q9])
s1 =
q1: q7 - q3 - q8
q2: q3 + q8
q4: q6 + q7 - q8
q5: q6 - q8
q9: q7 - q8
s1a = solve([eqn1a,eqn2a,eqn4a,eqn5a,eqn6a],[dq1,dq2,dq4,dq5,dq9])
s1a =
dq1: dq7 - dq3 - dq8
dq2: dq3 + dq8
dq4: dq6 + dq7 - dq8
dq5: dq6 - dq8
dq9: dq7 - dq8
subs([eqn1,eqn2,eqn3,eqn4,eqn5,eqn6],[q1,q2,q4,q5,q9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9])
ans = 
subs([eqn1a,eqn2a,eqn3a,eqn4a,eqn5a,eqn6a],[dq1,dq2,dq4,dq5,dq9],[s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
ans = 
eqn7 = subs(eqn7,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn7 = 
eqn8 = subs(eqn8,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn8 = 
eqn9 = subs(eqn9,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn9 = 
eqn10 = subs(eqn10,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn10 = 
eqn11 = subs(eqn11,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn11 = 
eqn12 = subs(eqn12,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn12 = 
eqn13 = subs(eqn13,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn13 = 
eqn14 = subs(eqn14,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn14 = 
eqn15 = subs(eqn15,[q1,q2,q4,q5,q9,dq1,dq2,dq4,dq5,dq9],[s1.q1,s1.q2,s1.q4,s1.q5,s1.q9,s1a.dq1,s1a.dq2,s1a.dq4,s1a.dq5,s1a.dq9])
eqn15 = 
solve([eqn7,eqn8,eqn9,eqn10,eqn11,eqn12,eqn13,eqn14,eqn15],[dq3,dq6,dq7,dq8,p1,p2,p3,p4,p5])