Запустите прямую кинематику на роботе с пятью панелями

Этот раздел демонстрирует, как выполнить исследования FK, чтобы вычислить рабочую область без сингулярности для пера, которое соответствует набору моторных углов. А именно, пример присваивает активные углы поворота шарнира робота как целевые переменные и координаты X и Y пера как выходные переменные. Пример также присваивает пассивные соединения и ориентацию пера как переменные исходного предположения, которые используются, чтобы помочь склонять решения к +- режим.

1. Чтобы выполнить исследования FK, сначала загрузите модель робота с пятью панелями в память и создайте KinematicsSolver объект (fk) для модели.

mdl = 'sm_five_bar_robot';
load_system(mdl);
fk = simscape.multibody.KinematicsSolver(mdl);

2. Чтобы задать и изучить отношения между пером, мировой системой координат, активными соединениями и пассивными соединениями робота, необходимо создать переменные системы координат, которые соответствуют настройке пера относительно мировой системы координат. Обратите внимание на то, что вы не должны создавать объединенные переменные, потому что они автоматически созданы, когда объект создается.

Добавьте группу поступательных и вращательных переменных системы координат для пера к fk. Задайте мировую систему координат как основу и систему координат Совета пера как последователь. Назовите группу переменной системы координат Pen. Объект имеет шесть переменных системы координат: три поступательных и три вращательных.

base = mdl + "/World Frame/W";
follower = mdl + "/Five Bar Robot/Pen/Pen/Tip";
addFrameVariables(fk,'Pen','Translation', base, follower);
addFrameVariables(fk,'Pen','Rotation', base, follower);

3. Присвойте основанные на положении объединенные переменные активных соединений (j5.Rz.q и j4.Rz.q) как цели. Чтобы найти идентификаторы основанных на положении объединенных переменных, используйте jointPositionVariables объектная функция.

jointPositionVariables(fk)

ans=5×4 table
       ID           JointType                                  BlockPath                               Unit 
    _________    ________________    ______________________________________________________________    _____

    "j1.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_c"                "deg"
    "j2.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_d"                "deg"
    "j3.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_e"                "deg"
    "j4.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_a/Active_Joint_a"    "deg"
    "j5.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_b/Active_Joint_b"    "deg"

targetIDs = ["j5.Rz.q";"j4.Rz.q"];
addTargetVariables(fk,targetIDs);

4. Присвойте эти переменные как выходные параметры:

Чтобы найти идентификаторы переменных системы координат, используйте frameVariables объектная функция.

frameVariables(fk)

ans=6×4 table
            ID                           Base                                      Follower                       Unit 
    ___________________    _________________________________    ______________________________________________    _____

    "Pen.Translation.x"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.y"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.z"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Rotation.x"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.y"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.z"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"

outputIDs = ["Pen.Translation.x";"Pen.Translation.y";"j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addOutputVariables(fk,outputIDs);

5. Присвойте объединенные переменные пассивных соединений (j1.Rz.q и j2.Rz.q) и вращательный z компонент пера (Pen.Rotation.z) как предположения.

guessIDs = ["j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addInitialGuessVariables(fk,guessIDs);

6. Изображение является эскизом робота с пятью панелями. Углы активных соединений ${\theta }_{\mathit{a}}$ и ${\theta }_{\mathit{b}}$, и пассивные соединения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$. Используйте вложенный for цикл, чтобы вычислить рабочую область пера для набора активных углов поворота шарнира, которые варьируются между [95, 150] и [30, 85] степени, соответственно. Гарантировать, что робот находится в +- режим, используйте [50 - 50 - 110] степени как начальные значения для переменных предположения (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z) и затем используйте последние значения переменных предположения, чтобы вести следующее решение FK.

guesses = [50 -50 -110]; % Initial guesses for theta_c, theta_d, and rotational z componenet of the pen
index = 1;

% Generate angles for theta_a and theta_b
theta_a = linspace(95,150,31);
theta_b = linspace(30,85,31);

% Allocate memory for output data
fk_data = zeros(length(theta_a)*length(theta_b),4);

% Calculate the robot's workspace for the given theta_a and theta_b
for i = 1:length(theta_a)
    for j = 1:length(theta_b)
        targets = [theta_b(j),theta_a(i)];
        [outputVec,statusFlag] = solve(fk,targets,guesses);  
        if statusFlag == 1 % Save a solution if its statusFlag is 1
            fk_data(index,1:4) = [outputVec(1),outputVec(2),outputVec(3),outputVec(4)];
            index = index + 1;
            guesses = [outputVec(3),outputVec(4),outputVec(5)]; % Update next guess with last solution
        end
    end
end

7. Постройте углы пассивных соединений, чтобы проверить, находится ли робот с пятью панелями в +- режим. Если робот находится в +- режим, значения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$ последовательно положительны и отрицательны, соответственно.

figure
plot(fk_data(1:index-1,3),'b','LineWidth',2); % Note that the total number of the solutions that satisfy statusFlag equals to index-1;
hold on
plot(fk_data(1:index-1,4),'k','LineWidth',2);
hold off


title('Passive Joint Angles')
xlabel('Index')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')

График показывает, что робот с пятью панелями находится в +- режим. Кроме того, вычисленная рабочая область без сингулярности потому что значения ${\theta }_{\mathit{c}}$ и ${\theta }_{\mathit{d}}$ вдали от нуля.

8. Отобразите данные о рабочей области на графике по изображению вида сверху робота с пятью панелями.

% Plot the top-view image of the five-bar robot
figure  
x = [-1 1];
y = [1 -1];
C = imread('top_view_five_bar_robot.png');
image(x,y,C)

hold on

% Plot the calculated workspace as blue dots
plot(fk_data(:,1),fk_data(:,2),'b.')
ylabel('y (m)')
xlabel('x (m)')
axis equal
axis([-1 1 -1 1]);
hold off

Запустите инверсную кинематику на роботе с пятью панелями

В этом разделе пример показывает, как выполнить исследования IK, чтобы вычислить моторные углы, которые соответствуют траектории исполнительного элемента конца. А именно, пример присваивает поступательные компоненты X и Y пера как целевые переменные и присваивает моторные углы как выходные переменные. Затем пример вычисляет моторные углы для траектории пера. Гарантировать робота находится в +- режим, используйте пассивные соединения и перьевую ориентацию как предположения, чтобы сместить решения IK. Обратите внимание на то, что необходимо запустить раздел FK прежде, чем запустить этот раздел.

1. Задайте круговую траекторию исполнительного элемента конца в ранее расчетной рабочей области. Сохраните координаты круга для исследований IK.

figure
% Plot the calculated workspace as blue dots on the top-view image of the five-bar robot
image(x,y,C)
hold on
plot(fk_data(:,1),fk_data(:,2),'b.')

% Specify the center and radius of the circle
center_x = 0; % m
center_y = 0.3; % m
radius = 0.15; % m
th = 0:pi/50:2*pi; % radians

% Save the x and y coordinates of the circle for the IK analyses
coordinates_x = radius * cos(th) + center_x;
coordinates_y = radius * sin(th) + center_y;

% Plot the circle
plot(coordinates_x,coordinates_y,'r','LineWidth',2)
axis equal
axis([-1 1 -1 1]);
ylabel('Length (m)')
xlabel('Width (m)')
hold off

2. Чтобы выполнить исследования IK, сначала создайте KinematicsSolver объект (ik) для модели.

ik = simscape.multibody.KinematicsSolver(mdl);

3. Чтобы сформулировать исследования IK, создайте переменные системы координат для настройки для пера относительно мировой системы координат и добавьте их в объект (ik). Можно снова использовать основу и переменные последователя из раздела FK.

addFrameVariables(ik, 'Pen','Translation', base, follower);
addFrameVariables(ik, 'Pen','Rotation', base, follower);

4. Присвойте переменные, которые соответствуют поступательным компонентам X и Y пера (Pen.Translation.x и Pen.Translation.y) как цели. Чтобы найти идентификаторы переменных системы координат, используйте frameVariables объектная функция.

frameVariables(ik)

ans=6×4 table
            ID                           Base                                      Follower                       Unit 
    ___________________    _________________________________    ______________________________________________    _____

    "Pen.Translation.x"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.y"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Translation.z"    "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "m"  
    "Pen.Rotation.x"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.y"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"
    "Pen.Rotation.z"       "sm_five_bar_robot/World Frame/W"    "sm_five_bar_robot/Five Bar Robot/Pen/Pen/Tip"    "deg"

targetIds = ["Pen.Translation.x";"Pen.Translation.y"];
addTargetVariables(ik,targetIds);

5. Присвойте эти переменные как выходные параметры:

Чтобы найти идентификаторы основанных на положении объединенных переменных, используйте jointPositionVariables объектная функция.

jointPositionVariables(ik)

ans=5×4 table
       ID           JointType                                  BlockPath                               Unit 
    _________    ________________    ______________________________________________________________    _____

    "j1.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_c"                "deg"
    "j2.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_d"                "deg"
    "j3.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Passive_Joint_e"                "deg"
    "j4.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_a/Active_Joint_a"    "deg"
    "j5.Rz.q"    "Revolute Joint"    "sm_five_bar_robot/Five Bar Robot/Step_Motor_b/Active_Joint_b"    "deg"

outputIds = ["j5.Rz.q";"j4.Rz.q";"j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addOutputVariables(ik,outputIds);

6. Используйте for цикл, чтобы реализовать исследования IK. А именно, используйте for цикл, чтобы присвоить координаты предопределенной траектории к перу и вычислить соответствующие моторные углы.

N = length(coordinates_x); % Number of points on trajectory
index_in_fk = 1;
fk_data = zeros(N,5); % Allocate memory for the variable

for ind = 1:N
    targets = [coordinates_x(ind), coordinates_y(ind)]; % Use the x and y coordinates of the predefined circle for the inverse kinematics problem
    [outputVec_ik,statusFlag_ik] = solve(ik,targets);
    if statusFlag_ik == 1 
        viewSolution(ik) % View the solution in the Kinematics Solver Viewer
        fk_data(index_in_fk,1:5) = outputVec_ik; % save the output data
        index_in_fk = index_in_fk+1;   
    else
        error('Did not hit targets');
    end
end
closeViewer(ik);

7. Постройте пассивные соединения робота, чтобы видеть, находится ли робот в +- режим.

% Plot the angles of the passive joints
figure
plot(fk_data(:,3),'b','LineWidth',2);
hold on
plot(fk_data(:,4),'k','LineWidth',2);
hold off
title('Passive Joint Angles')
xlabel('Point')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')

График показывает, что робот не находится в +- режим, потому что значения обоих пассивных соединений положительны.

Во время исследований IK Средство просмотра Решателя Кинематики открывает и показывает анимацию процесса. Анимация также показывает, что результат не желаем, потому что робот с пятью панелями не находится в +- режим. Изображение показывает снимок состояния анимации.

8. Чтобы помочь гарантировать, что все решения остаются дома + - режим, присвойте объединенные переменные пассивных соединений (j1.Rz.q и j2.Rz.q) и переменная вращательного z компонента пера (Pen.Rotation.z) как предполагает, чтобы сместить решения IK.

guessIds = ["j1.Rz.q";"j2.Rz.q";"Pen.Rotation.z"];
addInitialGuessVariables(ik,guessIds);

Используйте [50 - 50 - 110] степени как начальные значения для переменных предположения (j1.Rz.q, j2.Rz.q, и Pen.Rotation.z) и используйте последние значения переменных предположения, чтобы вести следующее решение IK.

guesses = [50 -50 -110]; % Assign initial guesses for passive joints and z rotation component of the pen
N = length(coordinates_x);
index_in_fk = 1;
fk_data = zeros(N,5); % Allocate memory for the variable
for ind = 1:N
    targets = [coordinates_x(ind), coordinates_y(ind)]; % Use the x and y coordinates of the prescribed circle for the inverse kinematics problem
    [outputVec_ik,statusFlag_ik] = solve(ik,targets, guesses);
    if statusFlag_ik == 1 
        viewSolution(ik) % View the solution in the Kinematics Solver Viewer
        fk_data(index_in_fk,1:5) = outputVec_ik; % Save the rotations of the motors
        index_in_fk=index_in_fk+1;
        guesses = [outputVec_ik(3) outputVec_ik(4) outputVec_ik(5)]; % Update next guess with last solution
    else
        error('Did not hit targets');
    end
end
closeViewer(ik);

График показывает, что пассивные углы поворота шарнира всегда хорошо вдали от нуля и всегда находятся в +- режим. Поэтому этот набор моторных углов соответствует в конец исполнительный элемент, перемещающийся в сглаженное движение вдоль круговой траектории, не проходя ни через какую кинематическую сингулярность.

% Plot the angles of the passive joints
figure
plot(fk_data(:,3),'b','LineWidth',2);
hold on
plot(fk_data(:,4),'k','LineWidth',2);
hold off
title('Passive Joint Angles')
xlabel('Index')
ylabel('Angle (Degrees)')
legend('Joint c','Joint d')