robotics section 4 inverse kinematics matlab practice

题目

problem

代码

%% 输入齐次变量矩阵TH0
GD = [1 0 0 9
      0 1 0 0
      0 0 1 0
      0 0 0 1];
% GD = [0.5   -0.866 0 7.5373
%       0.866  0.6   0 3.9266
%       0      0     1 0
%       0      0     0 1];
% GD = [ 0 1 0 -3
%       -1 0 0  2
%        0 0 1  0
%        0 0 0  1];
% GD = [ 0.866 0.5   0 -3.1245
%       -0.5   0.866 0  9.1674
%        0     0     1  0
%        0     0     0  1];
%% 定义符号变量
syms L1 L2 L3 theta1 theta2 theta3 x y c1 s1;
%% 正向运动学
L(1) = Link([theta1 0  0  0 0],'modified');
L(2) = Link([theta2 0  4  0 0],'modified');
L(3) = Link([theta3 0  3  0 0],'modified');
L(4) = Link([0      0  2  0 1],'modified');%工具坐标{T}
%% 初始化变量
L1=4;L2=3;L3=2;
%% TH3矩阵
TH3 = [1 0 0 2
       0 1 0 0
       0 0 1 0
       0 0 0 1];
% Check input size
if ~isequal(size(GD), [4,4])
    msg ='Wrong input format, must be a 4x4 matrix';
    error(msg);
end
gd = GD*(TH3^-1);
%% DH parameters
%一般需要输入D-H参数，本题无变量，故省略
%% Calculating theta2
x = gd(1,4);
y = gd(2,4);
c1 = gd(1,1);
s1 = gd(2,1);
c2 = (x^2 + y^2 - 4^2 - 3^2)/(2*4*3);
%Check whether the target exceeds the limit
if c2<-1||c2>1
    msg ='the target exceeds the limit';
    error(msg);
end
s2P =  sqrt(1-c2^2);
s2N = -sqrt(1-c2^2);
theta2P = atan2(s2P,c2);
theta2N = atan2(s2N,c2);
%% Calculating theta1
k1P = 4 + 3 * cos(theta2P);
k2P = 4 * sin(theta2P);
theta1P = atan2(y,x) - atan2(k2P,k1P);
%
k1N = 4 + (3 * cos(theta2N));
k2N = 4 * sin(theta2N);
theta1N = atan2(y,x) - atan2(k2N,k1N);
% Calculating theta3
theta3P = atan2(s1,c1) - theta2P - theta1P;
theta3N = atan2(s1,c1) - theta2N - theta1N;
%% Output of result in the form of 'deg'
K1P = theta1P*180/pi;
K2P = theta2P*180/pi;
K3P = theta3P*180/pi;
K1N = theta1N*180/pi;
K2N = theta2N*180/pi;
K3N = theta3N*180/pi;
result1 = [K1P,K2P,K3P,0]
result2 = [K1N,K2N,K3N,0]
%% Recheck
digits(4);%设置只保留小数点后4位
T1 = L(1).A(theta1P)*L(2).A(theta2P)*L(3).A(theta3P)*L(4).A(0)%此时为分数
T2 = L(1).A(theta1N)*L(2).A(theta2N)*L(3).A(theta3N)*L(4).A(0)
A1 = vpa(T1)%变换为小数输出
A2 = vpa(T2)

结果

result

result

result

result