Задайте пространство состояний

Чтобы создать ограниченное пространство состояний, сгенерируйте класс exampleHelperConstrainedStateSpace это выводит из manipulatorStateSpace. Откройте этот пример, чтобы получить вспомогательный файл.

classdef exampleHelperConstrainedStateSpace < manipulatorStateSpace

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

Ограниченная область задана в конструкторе, использующем workspaceGoalRegion объект. Границами на xyz-позиционном является весь [-100,100] метры. Границы ориентации являются constrainted к любому вращению [-pi pi] об оси z с нулевым вращением в X и Y.

Создайте решатель инверсной кинематики (IK) и задайте любые параметры решателя. Этот решатель генерирует объединенные настройки робота для положений исполнительного элемента конца, произведенных в рабочей области.

        function obj = exampleHelperConstrainedStateSpace(rbt,endEffectorName,targetOrientation)
            %Constructor
            obj@manipulatorStateSpace(rbt);
            obj.EnableConstraint = true;
            obj.Region = workspaceGoalRegion(endEffectorName,'EndEffectorOffsetPose',targetOrientation);
            obj.Region.Bounds = [-100 100; -100 100; -100 100; -pi pi; 0 0; 0 0];
            
            % Store a reference of the manipulatorStateSpace/RigidBodyTree
            % in the Robot property. Note that the RigidBodyTree property
            % of the manipulatorStateSpace is read-only. Hence, accessing
            % it will involve creating a copy of the underlying handle,
            % which can be expensive.
            obj.Robot = obj.RigidBodyTree;
            
            % Configure the IK solver
            obj.IKSolver = inverseKinematics('RigidBodyTree', obj.Robot);
            obj.IKSolver.SolverParameters.AllowRandomRestart = false;
        end

Настройте метод интерполяции

Во время стадии планирования, interpolate функционируйте настройки подключений в дереве поиска. По умолчанию интерполяция неограничена. Настройте interpolate функция, чтобы гарантировать, что исполнительный элемент конца остается вертикальным. Замените interpolate метод на manipulatorStateSpace объект с этим кодом.

        function constrainedStates = interpolate(obj, state1, state2, ratios)
            constrainedStates = interpolate@manipulatorStateSpace(obj,state1,state2,ratios);
            if(obj.EnableConstraint)
                for i = 1:size(constrainedStates, 1)
                    constrainedStates(i,:) = constrainConfig(obj,constrainedStates(i,:));
                end
            end
        end

constraintConfig функционируйте заменяет неограниченные объединенные настройки на ограниченные настройки. Это находит, что исполнительный элемент конца позирует самый близкий к ограниченной области [1], и возвращает соответствующую настройку соединения робота. Учитывая положение исполнительного элемента конца ${\mathit{T}}_{\mathit{s}}^0$ соответствие config, функция вычисляет положение самое близкое положение в-конец-исполнительного-элемента в ограниченной области и возвращает соответствующую настройку соединения робота.

Чтобы найти самое близкое положение в системе координат, сначала найдите смещение ${\mathit{T}}_{\mathit{s}}^0$ из области путем вычисления ${\mathit{T}}_{{\mathit{s}}^{\prime }}^{\mathit{w}}={{\mathit{T}}_{\mathit{w}}^0}^{-1}{\mathit{T}}_{\mathit{s}}^0{{\mathit{T}}_{\mathit{e}}^{\mathit{w}}}^{-1}$ и преобразование ${\mathit{T}}_{{\mathit{s}}^{\prime }}^{\mathit{w}}$ к вектору положения близко к границам области. Учитывая границы, существует три possibililties для элемента в векторе положения и его соответствующих границах области:

Если получившееся самое близкое ограниченное положение найдено в системе координат, преобразуйте это в положение в мировых координатах.

        function constrainedConfig = constrainConfig(obj, config)
            %constrainConfig Constraint joint configuration to the region
            %   The function finds the joint configuration corresponding to
            %   the end-effector pose closest to the constrained region.
            wgr = obj.Region;
            T0_s = obj.Robot.getTransform(config, wgr.EndEffectorName);
            T0_w = wgr.ReferencePose;
            Tw_e = wgr.EndEffectorOffsetPose;
            Tw_sPrime = T0_w \ T0_s / Tw_e;
            dw = convertTransformToPoseVector(obj, Tw_sPrime);
            bounds = wgr.Bounds;
            
            % Find the pose vector closeset to the bounds of the region.
            for dofIdx = 1:6
                if(dw(dofIdx) < bounds(dofIdx, 1))
                    dw(dofIdx) = bounds(dofIdx, 1);
                elseif(dw(dofIdx) > bounds(dofIdx, 2))
                    dw(dofIdx) = bounds(dofIdx, 2);
                end
            end
            
            % Convert the pose vector in the region's reference frame to a
            % homogeneous transform.
            constrainedPose = obj.convertPoseVectorToTransform(dw);
            
            % Convert this pose to world coordinates, and find the
            % corresponding joint configuration.
            constrainedPose = T0_w * constrainedPose * Tw_e;
            constrainedConfig = obj.IKSolver(obj.Region.EndEffectorName, ...
                constrainedPose,  ...
                ones(1, 6), ...
                config);
        end

Создайте пространство состояний робота

Теперь, когда вы имеете, устанавливают ваше ограниченное пространство состояний, загружают робота в качестве примера и среду с помощью exampleHelperEndEffectorConstraintedEnvironment функция. Выход kinova модель робота дерева твердого тела Генерала KINOVA™ 3 и env массив ячеек объектов тела столкновения в мире робота.

[kinova,env] = exampleHelperEndEffectorConstrainedEnvironment;

Задайте имя исполнительного элемента конца и предназначайтесь для ориентации. Сохраните объединенные значения положения для открытия и закрытия механизма захвата.

endEffectorName = "EndEffector_Link";
targetOrientation = eul2tform([0 pi 0]);
openPostion = 0.06;
closedPosition = 0.04;

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

ss = exampleHelperConstrainedStateSpace(kinova, endEffectorName, targetOrientation);

Задайте границы состояния на положении механизма захвата, таким образом, это остается открытым.

ss.StateBounds(end-1:end,:) = repmat([openPostion, openPostion], 2, 1);

Визуализируйте ограниченный workspaceGoalRegion область.

figure;
show(ss.Region)

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

Создайте manipulatorCollisionBodyValidator объект от пространства состояний и добавляет среду объектов столкновения. Чтобы улучшать производительность, проигнорируйте сам столкновения при проверке пространства состояний. Модель робота использует объединенные пределы, чтобы гарантировать, что сам столкновения не произойдут.

sv = manipulatorCollisionBodyValidator(ss,"Environment",env);
sv.IgnoreSelfCollision = true;

Создайте планировщика

Этот пример использует plannerBiRRT объект, который является двунаправленным вариантом алгоритма RRT с включенной эвристикой "подключения". Для разреженной среды этот планировщик находит решение в меньшем количестве итераций по сравнению с другими основанными на RRT планировщиками. В качестве альтернативы для более коротких путей с обрезанными ребрами, рассмотрите plannerRRTStar объект.

Робот запускается в объединенной настройке, которая удовлетворяет ограничению. В качестве альтернативы interpolate функция ограниченного пространства состояний может использоваться, чтобы ограничить настройку запуска. Это гарантирует, что все состояния в получившемся плане ограничиваются. Установите seed случайных чисел для повторяемых результатов.

rng(20);
planner = plannerBiRRT(ss, sv);
planner.MaxConnectionDistance = 0.5;
planner.EnableConnectHeuristic = true;
startConfig = [0.1196 -0.045 -0.1303 1.6528 -0.0019 1.5032 0.0083, openPostion, openPostion];
figure("Visible","on");
show(kinova, startConfig, ...
    "Visuals", "off", ...
    "Collisions", "on", ...
    "PreservePlot", true);

Задайте область схватывания

Затем задайте цепкую область с помощью workspaceGoalRegion объект, который представляет цилиндр, чтобы выбрать в среде (env{3}). Сгенерируйте объединенную настройку для произведенного положения исполнительного элемента конца. Необходимо гарантировать, что настройка, переданная планировщику, без коллизий.

graspingRegion = workspaceGoalRegion(endEffectorName);

% Attach the reference frame to the pose of the cylinder object.
graspingRegion.ReferencePose = env{3}.Pose;

% Define the offset of the end-effector relative to the target orientation.
graspingRegion.EndEffectorOffsetPose = trvec2tform([0, 0, 0.07]) * targetOrientation;

% Generate goal joint configuration of the robot given a sampled end-effector pose in
% the region.
goalConfig = jointConfigurationGiven(ss,sample(graspingRegion));
goalConfig(end-1:end) = openPostion;

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

Вызовите plan функционируйте, чтобы запланировать путь между запуском и целевыми настройками. Время стадия планирования. Интерполируйте путь к 50 точкам.

tic;
pickPath = plan(planner,startConfig,goalConfig);
tOut = toc;
interpolate(pickPath,50);
fprintf(newline);
fprintf("Planning finished in %d s\n", tOut);

Planning finished in 1.485242e+00 s

Визуализируйте запланированный путь. Робот перемещается в контейнер, чтобы захватить его.

hold on;
ylim([-1 0.5])
zlim([-0.25 1])
for i  = 1:length(env)
    show(env{i})
end
for i = 1:size(pickPath.States, 1)
    show(kinova, pickPath.States(i, :), ...
        "Visuals", "on", ...
        "FastUpdate",true, ...
        "Frames", "off", ...
        "PreservePlot",false);
    drawnow;
end
exampleHelperConstrainedRobotMoveGripper(kinova,...
    pickPath.States(end,:),openPostion,closedPosition);
hold off;

Воссоздайте планировщика

Присоедините геометрию столкновения в-конец-исполнительного-элемента банки и удалите банку из среды. Затем с модифицированным роботом создайте экземпляр планирования с новой моделью робота. После планирования вы могли unceck ss.EnableContraint и повторно выполненный этот скрипт, чтобы видеть, что путь без constrainted заканчивает исполнительный элемент.

zOffset = [0 0 (env{3}.Length)/2];
container = env{3};
addCollision(kinova.getBody(endEffectorName),env{3}, (env{3}.Pose) \ trvec2tform(zOffset));
env(3) = [];
ss = exampleHelperConstrainedStateSpace(kinova,endEffectorName,targetOrientation);

% Keep the end-effector closed during planning
ss.StateBounds(end-1:end,:) = repmat([0.04 0.04],2,1);
sv = manipulatorCollisionBodyValidator(ss, ...
    "Environment",env,"ValidationDistance",0.2);
planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 1;
planner.EnableConnectHeuristic = true;

ss.EnableConstraint = true;

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

Затем запланируйте от последней настройки в предыдущем пути к новой целевой объединенной настройке. Целевая настройка помещает банку в противоположную таблицу в среде.

% The last state of the of the previous plan
startConfig = pickPath.States(end,:);
startConfig(end-1:end) = 0.04;
goalConfig = [1.1998 0.6044 0.0091 1.4219 -0.0058 1.1154 0.0076 0.04 0.04];
rng(10);
tic
placePath = plan(planner,startConfig,goalConfig);
tOut = toc;
fprintf("Planning finished in %0.2d s\n",tOut);

Planning finished in 3.37e+00 s

interpolate(placePath,50);

Визуализируйте путь, который помещает контейнер. Заметьте, что банка остается вертикальной в пути.

show(kinova, startConfig, ...
    "FastUpdate", false, ...
    "PreservePlot", false);
view([90,0,15])
ylim([-1 0.5])
zlim([-0.25 1])
hold on;
for i  = 1:length(env)
    show(env{i})
end
containerPose = hgtransform();
containerPose.Matrix = container.Pose;
[ax, p] = show(container);
p.FaceColor = [0 0 1];
p.Parent = containerPose;
for i = 1:size(placePath.States, 1)
    cPose = getTransform(kinova,placePath.States(i, :),endEffectorName);
    containerPose.Matrix = cPose/(container.Pose)*trvec2tform(zOffset);
    show(kinova,placePath.States(i,:), ...
        "FastUpdate",true, ...
        "PreservePlot",false, ...
        "Frames", "off");
    drawnow;
end
exampleHelperConstrainedRobotMoveGripper(kinova,...
    placePath.States(end,:),closedPosition,openPostion);
hold off;

Следующие шаги

Чтобы видеть неограниченный путь, снимите флажок с ss.EnableConstraint флажок и повторно выполненный скрипт. Наклоны чашки способом, которые могли легко пролить содержимое. Вы могли также рассмотреть заменяющий plannerBiRRT объект с другим основанным на выборке планировщиком как plannerRRTStar оценивать их эффективность.

Ссылки

[1] Д. Беренсон, С. Сриниваса, Д. Фергюсон, A. Оправа и Дж. Каффнер, "Манипуляция, Планирующая с областями Цели Рабочей области", в Продолжениях Международной конференции IEEE по вопросам Робототехники и Автоматизации, 2009, pp.1397-1403