Создайте траекторию без столкновений в рабочей области с ограничениями.
Создание простой среды с использованием примитивов коллизий. В этом примере создается сцена, в которой робот находится в рабочей области и должен перемещать объекты из одной таблицы в другую. Робот также должен избегать кругового светильника над рабочим пространством. Смоделировать таблицы как два поля и сферу и указать их позу в мире. Более сложные среды можно создать с помощью 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 nad Exhaustiveаргументы «имя-значение». Самостоятельные столкновения игнорируются, поскольку пределы соединения модели робота предотвращают большинство самостоятельных столкновений. Проверка выдоха обеспечивает вычисление всех расстояний разделения и продолжение поиска столкновений после обнаружения первого столкновения.
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")
