Образцы концевых эффекторных поз в мировой рамке
eePose = sample(goalRegion)
Функция возвращает позу, равномерно дискретизированную в пределах свойства Bounds относительно опорного кадра, и применяет следующие преобразования на основе свойств ReferurePose и EndEffsecterOffsetPose:
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. Функция возвращает конечный эффектор, представляющий собой 3-D массив однородных преобразований.
Выполните выборку различных поз в пределах области цели рабочего пространства для манипулятора. Некоторые концевые эффекторные позы могут быть нежелательными из-за расположения тел рук и препятствий в сцене. workspaceGoalRegion объект определяет границы XYZ-положения и ориентации Euler ZYX концевого эффектора робота. 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 затравка для получения повторяющихся результатов. Создайте векторы для хранения действительных и недопустимых поз.
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