Надеюсь, у тебя все хорошо.
Я проверяю вывод моей прямой кинематики с помощью обратной кинематики, и результаты не такие, как хотелось бы. Поскольку вывод моей обратной кинематики не совпадает с выводом прямой кинематики.
Параметры D-H манипулятора задаются как:
Ссылка: альфа, а, тета, д
Ссылка 1: -90 0 theta1 * d1
Ссылка 2: 0 a2 theta2 * 0
Ссылка 3: 0 a3 theta3 * 0
Используемые функции:
Обратная кинематика
function q = inv_kinematics(ph)
%input in radians: [ph1 ph2 ph3] = [0.1 0.2 0.4]
l1 = 0.05;
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
ph1 = ph(1);
ph2 = ph(2);
ph3 = ph(3);
r=sqrt(ph1^2+(ph3-d1)^2);
alpha=acos((r^2+l2^2-l3^2)/(2*r*l2))
q = zeros(3,1);
q1=atan2(ph2,ph1);
q2=atan2(ph1,ph3-d1)-alpha;
q3=atan2(ph1-l2*sin(q2),ph3-d1-l2*cos(q2))-q2;
q=[q1;q2;q3];
%output: q = [1.107 -0.265 1.314]
end
Прямая кинематика:
function ph = forward_kinematics(q)
%input: [q1 q2 q3] = [1.107 -0.265 1.314]
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
q1 = q(1);
q2 = q(2);
q3 = q(3);
ph = zeros(3,1);
ph1 = l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3);
ph2 = l2*sin(q1)*cos(q2)+l3*sin(q1)*cos(q2+q3);
ph3 = d1-l2*sin(q2)-l3*sin(q2+q3);
ph=[ph1;ph2;ph3];
%output: p = [0.1655 0.3308 -0.07005]
end