Сгенерируйте траекторию без столкновения в ограниченной рабочей области.
Создайте простое окружение с помощью примитивов столкновения. Этот пример создает сцену, где робот находится в рабочей области и должен перемещать объекты из одной таблицы в другую. Робот должен также избегать циклического светового фиксатора над рабочей областью. Моделируйте таблицы как два ящика и сферу и задайте их положение в мире. Более сложные окружения могут быть созданы с помощью collisionMesh объекты.
% Create two platforms platform1 = collisionBox(0.5,0.5,0.25); platform1.Pose = trvec2tform([-0.5 0.4 0.2]); platform2 = collisionBox(0.5,0.5,0.25); platform2.Pose = trvec2tform([0.5 0.2 0.2]); % Add a light fixture, modeled as a sphere lightFixture = collisionSphere(0.1); lightFixture.Pose = trvec2tform([.2 0 1]); % Store in a cell array for collision-checking worldCollisionArray = {platform1 platform2 lightFixture};
Визуализируйте окружение с помощью вспомогательной функции, которая итерация через массив столкновений.
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
Загрузите модель манипулятора Kinova как rigidBodyTree объект с использованием loadrobot функция.
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
Показать робота в окружение с помощью тех же осей, что и объекты столкновения. Основа робота закреплена за источником мира.
show(robot,homeConfiguration(robot),"Parent",ax);
Задайте начальное и конечное положение, используя векторы положения и ориентации, которые объединяются с помощью умножения матрицы преобразования.
startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]); endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);
Использование inverseKinematics для решения положения соединений на основе требуемых положений. Выполните проверку вручную, чтобы убедиться, что строения действительны.
% Use a fixed random seed to ensure repeatable results rng(0); ik = inverseKinematics("RigidBodyTree",robot); weights = ones(1,6); startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration); endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration); % Show initial and final positions show(robot,startConfig); show(robot,endConfig);
Используйте профиль трапеций скорости, чтобы сгенерировать гладкую траекторию от исходного положения до начального положения, а затем до конечного положения.
q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);
Проверьте наличие столкновений с препятствиями в окружении с помощью checkCollision функция. Включите IgnoreSelfCollision над Exhaustiveаргументы имя-значение. Столкновения между собой игнорируются, потому что пределы модели робота предотвращают большинство столкновений между собой. Исчерпывающая проверка гарантирует, что функция вычисляет все расстояния разделения и продолжает поиск столкновений после обнаружения первого столкновения.
The sepDist выход хранит расстояния между телами робота и объектами столкновения мира в виде матрицы. Каждая строка соответствует определенному объекту мирового столкновения. Каждый столбец соответствует телу робота. Значения NaN указать столкновение. Сохраните индексы столкновения как массив ячеек.
% Initialize outputs inCollision = false(length(q), 1); % Check whether each pose is in collision worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision for i = 1:length(q) [inCollision(i),sepDist] = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on","Exhaustive","on"); [bodyIdx,worldCollisionObjIdx] = find(isnan(sepDist)); % Find collision pairs worldCollidingPairs = [bodyIdx,worldCollisionObjIdx]; worldCollisionPairIdx{i} = worldCollidingPairs; end isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
1
На последнем этапе обнаруживаются два столкновения. Визуализируйте эти строения, чтобы продолжить исследование. Используйте exampleHelperHighlightCollisionBodies функция для подсветки тел на основе индексов. Можно увидеть, что столкновение происходит в сфере и таблице.
collidingIdx1 = find(inCollision,1); collidingIdx2 = find(inCollision,1,"last"); % Identify the colliding rigid bodies. collidingBodies1 = worldCollisionPairIdx{collidingIdx1}*[1 0]'; collidingBodies2 = worldCollisionPairIdx{collidingIdx2}*[1 0]'; % Visualize the environment. ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); % Add the robotconfigurations & highlight the colliding bodies. show(robot,q(:,collidingIdx1),"Parent",ax,"PreservePlot",false); exampleHelperHighlightCollisionBodies(robot,collidingBodies1 + 1,ax); show(robot,q(:,collidingIdx2),"Parent"',ax); exampleHelperHighlightCollisionBodies(robot,collidingBodies2 + 1,ax);
Чтобы избежать этих столкновений, добавьте промежуточные путевые точки, чтобы убедиться, что робот перемещается вокруг препятствия.
intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % Out and around the sphere intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % Come in from above intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,collidingIdx1)); intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,collidingIdx2)); % Show the new intermediate poses ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false); show(robot,intermediateConfig2,"Parent",ax);
Сгенерируйте новую траекторию.
[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);Проверьте, что он свободен от столкновения.
%Initialize outputs inCollision = false(length(q),1); % Check whether each pose is in collision for i = 1:length(q) inCollision(i) = checkCollision(robot,q(:,i),worldCollisionArray,"IgnoreSelfCollision","on"); end isTrajectoryInCollision = any(inCollision)
isTrajectoryInCollision = logical
0
Анимируйте результат.
% Plot the environment ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); % Visualize the robot in its home configuration show(robot,startConfig,"Parent",ax2); % Update the axis size axis equal % Loop through the other positions for i = 1:length(q) show(robot,q(:,i),"Parent",ax2,"PreservePlot",false); % Update the figure drawnow end
Постройте график положения соединений с течением времени.
figure plot(t,q) xlabel("Time") ylabel("Joint Position")
