-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathh_b_func.m
6 lines (5 loc) · 3.68 KB
/
h_b_func.m
1
2
3
4
5
function [h] = h_b_func(q1, q2, q3, dq1, dq2, dq3, ddq1, ddq2, ddq3)
h = [[ ddq1*sin(2*q2 + 2*q3) + dq1*cos(2*q2 + 2*q3)*(2*dq2 + 2*dq3), - ddq2*sin(q2 + q3) - ddq3*sin(q2 + q3) - dq2*cos(q2 + q3)*(dq2 + dq3) - dq3*cos(q2 + q3)*(dq2 + dq3), - ddq1*sin(2*q2) - 2*dq1*dq2*cos(2*q2), dq2*sin(q2 + q3)*(dq2 + dq3) - ddq3*cos(q2 + q3) - ddq2*cos(q2 + q3) + dq3*sin(q2 + q3)*(dq2 + dq3), (73*ddq1)/200 + (73*ddq1*cos(2*q2 + 2*q3))/200 - (279*ddq1*sin(q3))/1000 + (279*ddq1*sin(2*q2 + q3))/1000 - (73*dq1*sin(2*q2 + 2*q3)*(2*dq2 + 2*dq3))/200 + (279*dq1*cos(2*q2 + q3)*(2*dq2 + dq3))/1000 - (279*dq1*dq3*cos(q3))/1000, ddq1/2 - (ddq1*cos(2*q2 + 2*q3))/2 + (dq1*sin(2*q2 + 2*q3)*(2*dq2 + 2*dq3))/2, ddq1/2 - (ddq1*cos(2*q2))/2 + dq1*dq2*sin(2*q2), cos(q2)*dq2^2 + ddq2*sin(q2), 0, (279*ddq1*cos(2*q2 + q3))/1000 - (279*ddq1*cos(q3))/1000 - (73*ddq1*sin(2*q2 + 2*q3))/200 + (279*dq1*dq3*sin(q3))/1000 - (73*dq1*cos(2*q2 + 2*q3)*(2*dq2 + 2*dq3))/200 - (279*dq1*sin(2*q2 + q3)*(2*dq2 + dq3))/1000, sin(q2)*dq2^2 - ddq2*cos(q2), dq1, 0, 0, 0, ddq1/2 + (ddq1*cos(2*q2 + 2*q3))/2 - (dq1*sin(2*q2 + 2*q3)*(2*dq2 + 2*dq3))/2, 0, 0, 1, (279*ddq1)/1000 - (279*ddq1*cos(2*q2))/1000 + (279*dq1*dq2*sin(2*q2))/500, (279*ddq1*sin(2*q2))/1000 + (279*dq1*dq2*cos(2*q2))/500, 0, 0, sign(dq1)];...
[ -dq1^2*cos(2*q2 + 2*q3), dq1*dq2*cos(q2 + q3) - dq1*cos(q2 + q3)*(dq2 + dq3) - ddq1*sin(q2 + q3) + dq1*dq3*cos(q2 + q3), dq1^2*cos(2*q2), dq1*sin(q2 + q3)*(dq2 + dq3) - ddq1*cos(q2 + q3) - dq1*dq2*sin(q2 + q3) - dq1*dq3*sin(q2 + q3), (73*ddq2)/100 + (73*ddq3)/100 + (981*cos(q2 + q3))/1000 - (279*dq1^2*cos(2*q2 + q3))/1000 + (73*dq1^2*sin(2*q2 + 2*q3))/200 - (279*ddq2*sin(q3))/500 - (279*ddq3*sin(q3))/1000 - (279*dq3^2*cos(q3))/1000 - (279*dq2*dq3*cos(q3))/500, -(dq1^2*sin(2*q2 + 2*q3))/2, -(dq1^2*sin(2*q2))/2, ddq1*sin(q2), ddq2 + ddq3, (279*dq3^2*sin(q3))/1000 - (981*sin(q2 + q3))/1000 + (279*dq1^2*sin(2*q2 + q3))/1000 + (73*dq1^2*cos(2*q2 + 2*q3))/200 - (279*ddq2*cos(q3))/500 - (279*ddq3*cos(q3))/1000 + (279*dq2*dq3*sin(q3))/500, -ddq1*cos(q2), 0, 0, ddq2, dq2, (dq1^2*sin(2*q2 + 2*q3))/2, 0, 1, 0, (279*ddq2)/500 + (981*sin(q2))/1000 - (279*dq1^2*sin(2*q2))/1000, (981*cos(q2))/1000 - (279*dq1^2*cos(2*q2))/1000, 0, sign(dq2), 0];...
[ -dq1^2*cos(2*q2 + 2*q3), dq1*dq2*cos(q2 + q3) - dq1*cos(q2 + q3)*(dq2 + dq3) - ddq1*sin(q2 + q3) + dq1*dq3*cos(q2 + q3), 0, dq1*sin(q2 + q3)*(dq2 + dq3) - ddq1*cos(q2 + q3) - dq1*dq2*sin(q2 + q3) - dq1*dq3*sin(q2 + q3), (73*ddq2)/100 + (73*ddq3)/100 + (981*cos(q2 + q3))/1000 - (279*dq1^2*cos(2*q2 + q3))/2000 + (73*dq1^2*sin(2*q2 + 2*q3))/200 - (279*ddq2*sin(q3))/1000 + (279*dq1^2*cos(q3))/2000 + (279*dq2^2*cos(q3))/1000, -(dq1^2*sin(2*q2 + 2*q3))/2, 0, 0, ddq2 + ddq3, (279*dq1^2*sin(2*q2 + q3))/2000 - (279*dq1^2*sin(q3))/2000 - (279*dq2^2*sin(q3))/1000 - (981*sin(q2 + q3))/1000 + (73*dq1^2*cos(2*q2 + 2*q3))/200 - (279*ddq2*cos(q3))/1000, 0, 0, dq3, 0, 0, (dq1^2*sin(2*q2 + 2*q3))/2, 1, 0, 0, 0, 0, sign(dq3), 0, 0]];
end