-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathequ.m
6 lines (5 loc) · 3.56 KB
/
equ.m
1
2
3
4
5
6
function F=equ(theta,x,y,z)
F(1) = -x + 69*cos(theta(1)*pi/180) - 69*sin(theta(1)*pi/180)*sin(theta(3)*pi/180) - (37429*sin(theta(4)*pi/180)*(sin(theta(1)*pi/180)*sin(theta(3)*pi/180) + cos(theta(1)*pi/180)*cos(theta(3)*pi/180)*sin(theta(2)*pi/180)))/100 - 10*sin(theta(5)*pi/180)*(cos(theta(3)*pi/180)*sin(theta(1)*pi/180) - cos(theta(1)*pi/180)*sin(theta(2)*pi/180)*sin(theta(3)*pi/180)) - (3683*sin(theta(6)*pi/180)*(sin(theta(5)*pi/180)*(cos(theta(3)*pi/180)*sin(theta(1)*pi/180) - cos(theta(1)*pi/180)*sin(theta(2)*pi/180)*sin(theta(3)*pi/180)) + cos(theta(5)*pi/180)*(cos(theta(4)*pi/180)*(sin(theta(1)*pi/180)*sin(theta(3)*pi/180) + cos(theta(1)*pi/180)*cos(theta(3)*pi/180)*sin(theta(2)*pi/180)) + cos(theta(1)*pi/180)*cos(theta(2)*pi/180)*sin(theta(4)*pi/180))))/10 - 10*cos(theta(5)*pi/180)*(cos(theta(4)*pi/180)*(sin(theta(1)*pi/180)*sin(theta(3)*pi/180) + cos(theta(1)*pi/180)*cos(theta(3)*pi/180)*sin(theta(2)*pi/180)) + cos(theta(1)*pi/180)*cos(theta(2)*pi/180)*sin(theta(4)*pi/180)) - (3683*cos(theta(6)*pi/180)*(sin(theta(4)*pi/180)*(sin(theta(1)*pi/180)*sin(theta(3)*pi/180) + cos(theta(1)*pi/180)*cos(theta(3)*pi/180)*sin(theta(2)*pi/180)) - cos(theta(1)*pi/180)*cos(theta(2)*pi/180)*cos(theta(4)*pi/180)))/10 + (7287*cos(theta(1)*pi/180)*cos(theta(2)*pi/180))/20 + (37429*cos(theta(1)*pi/180)*cos(theta(2)*pi/180)*cos(theta(4)*pi/180))/100 - 69*cos(theta(1)*pi/180)*cos(theta(3)*pi/180)*sin(theta(2)*pi/180);
F(2) = -y + 69*sin(theta(1)*pi/180) + (37429*sin(theta(4)*pi/180)*(cos(theta(1)*pi/180)*sin(theta(3)*pi/180) - cos(theta(3)*pi/180)*sin(theta(1)*pi/180)*sin(theta(2)*pi/180)))/100 + 10*sin(theta(5)*pi/180)*(cos(theta(1)*pi/180)*cos(theta(3)*pi/180) + sin(theta(1)*pi/180)*sin(theta(2)*pi/180)*sin(theta(3)*pi/180)) + (3683*sin(theta(6)*pi/180)*(sin(theta(5)*pi/180)*(cos(theta(1)*pi/180)*cos(theta(3)*pi/180) + sin(theta(1)*pi/180)*sin(theta(2)*pi/180)*sin(theta(3)*pi/180)) + cos(theta(5)*pi/180)*(cos(theta(4)*pi/180)*(cos(theta(1)*pi/180)*sin(theta(3)*pi/180) - cos(theta(3)*pi/180)*sin(theta(1)*pi/180)*sin(theta(2)*pi/180)) - cos(theta(2)*pi/180)*sin(theta(1)*pi/180)*sin(theta(4)*pi/180))))/10 + 10*cos(theta(5)*pi/180)*(cos(theta(4)*pi/180)*(cos(theta(1)*pi/180)*sin(theta(3)*pi/180) - cos(theta(3)*pi/180)*sin(theta(1)*pi/180)*sin(theta(2)*pi/180)) - cos(theta(2)*pi/180)*sin(theta(1)*pi/180)*sin(theta(4)*pi/180)) + (3683*cos(theta(6)*pi/180)*(sin(theta(4)*pi/180)*(cos(theta(1)*pi/180)*sin(theta(3)*pi/180) - cos(theta(3)*pi/180)*sin(theta(1)*pi/180)*sin(theta(2)*pi/180)) + cos(theta(2)*pi/180)*cos(theta(4)*pi/180)*sin(theta(1)*pi/180)))/10 + (7287*cos(theta(2)*pi/180)*sin(theta(1)*pi/180))/20 + 69*cos(theta(1)*pi/180)*sin(theta(3)*pi/180) + (37429*cos(theta(2)*pi/180)*cos(theta(4)*pi/180)*sin(theta(1)*pi/180))/100 - 69*cos(theta(3)*pi/180)*sin(theta(1)*pi/180)*sin(theta(2)*pi/180);
F(3) = -z + 10*cos(theta(5)*pi/180)*(sin(theta(2)*pi/180)*sin(theta(4)*pi/180) - cos(theta(2)*pi/180)*cos(theta(3)*pi/180)*cos(theta(4)*pi/180)) - (7287*sin(theta(2)*pi/180))/20 - (3683*cos(theta(6)*pi/180)*(cos(theta(4)*pi/180)*sin(theta(2)*pi/180) + cos(theta(2)*pi/180)*cos(theta(3)*pi/180)*sin(theta(4)*pi/180)))/10 + (3683*sin(theta(6)*pi/180)*(cos(theta(5)*pi/180)*(sin(theta(2)*pi/180)*sin(theta(4)*pi/180) - cos(theta(2)*pi/180)*cos(theta(3)*pi/180)*cos(theta(4)*pi/180)) + cos(theta(2)*pi/180)*sin(theta(3)*pi/180)*sin(theta(5)*pi/180)))/10 - 69*cos(theta(2)*pi/180)*cos(theta(3)*pi/180) - (37429*cos(theta(4)*pi/180)*sin(theta(2)*pi/180))/100 - (37429*cos(theta(2)*pi/180)*cos(theta(3)*pi/180)*sin(theta(4)*pi/180))/100 + 10*cos(theta(2)*pi/180)*sin(theta(3)*pi/180)*sin(theta(5)*pi/180) + 5407/20;
end