Дискретизация положений концевого эффектора в лабораторной системе координат
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"); 
Определите область цели
Создайте область цели рабочей области, используя имя тела робота с конечным эффектором.
Определите параметры области цели для вашей рабочей области. Область цели включает положение ссылки, границы 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)
Выборочные положения
Равномерная выборка положений в области таблицы с помощью 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
disp(string(validPoses'))
"3" "5" "7" "10"
Визуализация допустимого положения сингла
Постройте график всех допустимых положений как преобразований. Окончательное допустимое строение от проверки конфликтов все еще отображается на рисунке.
translations = tform2trvec(poses(:,:,validPoses));
rotations = tform2quat(poses(:,:,validPoses));
plotTransforms(translations,rotations,"FrameSize",0.1)
Показать допустимое строение из списка. Измените индекс в validPoses посмотреть на разные положения. Функции hold off чтобы остановить сохранение элементов рисунка. Чтобы вручную просмотреть положения и строения, закомментируйте конечную линию при запуске.
poseIndex = validPoses(1);
show(robot,config{poseIndex},"PreservePlot",false,"Collisions","on","Visuals","off");
hold off