Загрузите модель робота

Используйте loadrobot функционируйте, чтобы получить доступ к предопределенным моделям робота. Этот пример использует робота Quanser QArm™, и объединенные настройки заданы как векторы-строки.

robot = loadrobot("quanserQArm",DataFormat="row");
figure(Visible="on")
show(robot);
xlim([-0.5 0.5])
ylim([-0.5 0.5])
zlim([-0.25 0.75])
hold on

Сконфигурируйте пространство состояний и валидацию состояния

Создайте пространство состояний и утвердите блок проверки допустимости из модели робота.

ss = manipulatorStateSpace(robot);
sv = manipulatorCollisionBodyValidator(ss);

Установите расстояние валидации до 0.05, который основан на расстоянии, нормальном между двумя состояниями. Вы можете сконфигурировать блок проверки допустимости, чтобы проигнорировать сам столкновения, чтобы улучшить скорость валидации, но должны рассмотреть, имеет ли ваша модель робота соответствующий объединенный предельный набор настроек, чтобы гарантировать, что это не сталкивается с собой.

sv.ValidationDistance = 0.05;
sv.IgnoreSelfCollision = true;

Поместите объекты столкновения в среду робота. Установите Environment свойство объекта блока проверки допустимости столкновения использование массива ячеек объектов.

box = collisionBox(0.1,0.1,0.5); % XYZ Lengths
box.Pose = trvec2tform([0.2 0.2 0.5]); % XYZ Position
sphere = collisionSphere(0.25); % Radius
sphere.Pose = trvec2tform([-0.2 -0.2 0.5]); % XYZ Position
env = {box sphere};
sv.Environment = env;

Визуализируйте среду.

for i = 1:length(env)
    show(env{i})
end
view(60,10)

Запланируйте путь

Начните с домашней настройки как первая точка на пути.

rng(0); % Repeatable results
start = homeConfiguration(robot);
path = start;
idx = 1;

Запланируйте путь с помощью этих шагов в цикле:

for i = 2:25
    goal = sampleGaussian(ss,start,0.25*ones(4,1));
    validState = isStateValid(sv,goal);
    
    if validState % If state is valid, check motion between states.
        [validMotion,~] = isMotionValid(sv,path(idx,:),goal);

        if validMotion % If motion is valid, add to path.
            path = [path; goal];
            idx = idx + 1;
        end
    end
end

Визуализируйте путь

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

Чтобы визуализировать путь исполнительного элемента конца в 3-D, получите преобразование относительно основной мировой системы координат в каждой точке. Сохраните точки как xyz вектор сдвига. Постройте путь исполнительного элемента конца.

eePose = nan(3,size(path,1));

for i = 1:size(path,1)
    show(robot,path(i,:),PreservePlot=false);
    eePos(i,:) = tform2trvec(getTransform(robot,path(i,:),"END-EFFECTOR")); % XYZ translation vector
    plot3(eePos(:,1),eePos(:,2),eePos(:,3),"-b",LineWidth=2)
    drawnow
end