sample

Дискретизация положений концевого эффектора в лабораторной системе координат

Описание

eePose = sample(goalRegion) дискретизирует конечное-эффекторное положение в мировой системе координат как однородную матрицу преобразования.

Функция возвращает положение, равномерно выбранное в свойстве Bounds относительно опорной системы координат, и применяет следующие преобразования, основанные на свойствах ReferencePose и EndEffectorOffsetPose:

tSample; % Pose sampled within Bounds
Tw0 = goalRegion.ReferencePose;
TeW = goalRegion.EndEffectorOffsetPose;
eePose = Tw0 * tSample * TeW; % tSample is a pose within the bounds.

пример

eePose = sample(goalRegion,numSamples) Выборки несколько положений на основе входа numSamples. Функция возвращает положения end-effector как трехмерный массив однородных преобразований.

Примеры

свернуть все

Дискретизируйте различные положения в границах области цели рабочей области для руки манипулятора. Некоторые положения концевого эффекта могут быть нежелательными из-за расположения тел рук и препятствий в сцене. The workspaceGoalRegion объект задает границы XYZ-положения и ориентации ZYX Euler эффектора конца робота. The sample функция объекта равномерно дискретизирует случайные положения в границах. Найдите строения, которые достигают этих конечных эффекторных положений и определяют лучшее с помощью визуализации.

Загрузка существующей модели робота в rigidBodyTree объект.

robot = loadrobot("kinovaGen3","DataFormat","row");
show(robot,"Collisions","on","Visuals","off");

Добавьте банку как collisionCylinder объект для руки робота.

can = collisionCylinder(0.05, 0.1); 
can.Pose = trvec2tform([0.2, 0.3, 0.5]); 

addCollision(robot.Bodies{end},"cylinder", [0.05, 0.1], trvec2tform([0, 0, 0.02])); 

Цель этого примера - поместить эту банку на таблицу с другими банками. Добавьте таблицу и другие банки в окружение путем создания массива ячеек с объектами столкновения. Показать всю env массив ячеек.

table = collisionBox(0.7, 0.5, 0.04); 
table.Pose = trvec2tform([0, 0.5, 0.43]); 
env = {can, copy(can), copy(can), table}; 
env{2}.Pose = trvec2tform([-0.1, 0.3, 0.5]); 
env{3}.Pose = trvec2tform([-0.1, 0.5, 0.5]); 

hold on 
for i = 1: length(env) 
    show(env{i}) 
end
show(robot,homeConfiguration(robot),"Collisions","on","Visuals","off"); 

Figure contains an axes. The axes contains 55 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, base_link_coll_mesh, EndEffector_Link_coll_mesh.

Определите область цели

Создайте область цели рабочей области, используя имя тела робота с конечным эффектором.

Определите параметры области цели для вашей рабочей области. Область цели включает положение ссылки, границы XYZ-положения и пределы ориентации углов Эйлера ZYX. Этот пример задает границы XYZ в размерности таблицы и фиксирует поворот в небольшой области значений осей Y и X.

tableRegion = workspaceGoalRegion("EndEffector_Link",...
    "ReferencePose",table.Pose);
tableRegion.EndEffectorOffsetPose(1:3,1:3) = eul2rotm([0, 0, pi]); 
tableRegion.EndEffectorOffsetPose(3, end) = 0.1; 

tableRegion.Bounds = ... 
    [-table.X/2, table.X/2; % X Bounds
    -table.Y/2, table.Y/2;  % Y Bounds
    0.04, 0.10;             % Z Bounds
    -pi, pi;                % Rotation about Z-axis
    -0.01, 0.01;            % Y-Axis
    -0.01, 0.01;];          % X-Axis

show(tableRegion);
view(165,50)
camzoom(3.5)

Figure contains an axes. The axes contains 65 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, base_link_coll_mesh, EndEffector_Link_coll_mesh.

Выборочные положения

Равномерная выборка положений в области таблицы с помощью sample функция объекта. В этом примере установите rng seed, чтобы получить повторяемые результаты. Создайте векторы для хранения допустимых и недопустимых положений.

rng(0)
poses = sample(tableRegion,10);
validPoses = [];
invalidPoses = [];

Проверка на наличие коллизий

Чтобы найти строения для этих положений, создайте решатель обратной кинематики (IK).

ik = inverseKinematics('RigidBodyTree',robot);
config = cell(10);

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

for i = 1:length(poses)
    % Solve for robot configuraiton using IK.
    config{i} = ik("EndEffector_Link",poses(:,:,i),ones(6,1),homeConfiguration(robot));
    % Check for collisions.
    isColliding = checkCollision(robot,config{i},env);
    
    if ~isColliding % If not in collision, show robot configuration and save valid pose.
        show(robot,config{i},"PreservePlot",false,"Collisions","on","Visuals","off");
        drawnow
        validPoses = [validPoses; i];
    else
        invalidPoses = [invalidPoses; i];
    end
    
end

Figure contains an axes. The axes contains 40 objects of type line, patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

disp(string(validPoses'))
    "3"    "5"    "7"    "10"

Визуализация допустимого положения сингла

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

translations = tform2trvec(poses(:,:,validPoses));
rotations = tform2quat(poses(:,:,validPoses));

plotTransforms(translations,rotations,"FrameSize",0.1)

Figure contains an axes. The axes contains 56 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

Показать допустимое строение из списка. Измените индекс в validPoses посмотреть на разные положения. Функции hold off чтобы остановить сохранение элементов рисунка. Чтобы вручную просмотреть положения и строения, закомментируйте конечную линию при запуске.

poseIndex = validPoses(1);
show(robot,config{poseIndex},"PreservePlot",false,"Collisions","on","Visuals","off");
hold off

Figure contains an axes. The axes contains 56 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, EndEffector_Link_coll_mesh, base_link_coll_mesh.

Входные параметры

свернуть все

Область цели рабочей области, заданная как workspaceGoalRegion объект.

Количество выборок, заданное в виде положительного целого числа

Выходные аргументы

свернуть все

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

Функция возвращает положение, равномерно выбранное в свойстве Bounds относительно опорной системы координат, и применяет следующие преобразования, основанные на свойствах ReferencePose и EndEffectorOffsetPose:

tSample = rand(6,2);
Tw0 = goalRegion.ReferencePose;
TeW = goalRegion.EndEffectorOffsetPose;
eePose = Tw0 * tSample * TeW;

Типы данных: double

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

.
Введенный в R2021a
Для просмотра документации необходимо авторизоваться на сайте