-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCalPos.m
37 lines (36 loc) · 1.35 KB
/
CalPos.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
function [value] = CalPos(forward,a)
%% æ ¹æ®å‰å‘è¿åŠ¨è§£è®¡ç®—工具çƒåæ ‡ç³»åœ¨åŸºåæ ‡ç³»ä¸çš„ä½ç½®å’Œå§¿æ€RPYè§?
% 输出为６*１的相对于基åæ ‡ç³»åæ ‡çŸ©é˜µx,y,z,alpha,beta,gamma
if a==0
syms site_x site_y site_z pose_1 pose_2 pose_3;
site_x = forward(1,4);
site_y = forward(2,4);
site_z = forward(3,4);
pose_1 = atan(forward(2,1)/forward(1,1));
pose_2 = -atan(forward(3,1)/sqrt(forward(1,1)^2+forward(2,1)^2));
pose_3 = atan(forward(3,2)/forward(3,3));
else
site_x = forward(1,4);
site_y = forward(2,4);
site_z = forward(3,4);
% pose_1 = atan(-forward(1,3)/forward(2,3));
% pose_2 = atan((forward(1,3)*sin(pose_1)-forward(2,3)...
% *cos(pose_1))/forward(3,3));
% pose_3 = atan(-(forward(1,2)*cos(pose_1)+forward(2,2)...
% *sin(pose_1))/(forward(1,1)*cos(pose_1)+forward(2,1)...
% *sin(pose_1)));
pose_2 = -atan(forward(3,1)/sqrt(forward(1,1)^2+forward(2,1)^2));
if abs(pose_2+pi/2) <= 0.000001
pose_2 = -pi/2;
pose_1 = 0;
pose_3 = -atan(forward(1,2)/forward(2,2));
elseif abs(pose_2-pi/2) <= 0.000001
pose_2 = pi/2;
pose_1 = 0;
pose_3 = atan(forward(1,2)/forward(2,2));
else
pose_1 = atan(forward(2,1)/forward(1,1));
pose_3 = atan(forward(3,2)/forward(3,3));
end
end
value = [site_x site_y site_z pose_1 pose_2 pose_3];