Проверка на наличие экологических столкновений с манипуляторами

Сгенерируйте траекторию без столкновения в ограниченной рабочей области.

Задайте окружение столкновения

Создайте простое окружение с помощью примитивов столкновения. Этот пример создает сцену, где робот находится в рабочей области и должен перемещать объекты из одной таблицы в другую. Робот должен также избегать циклического светового фиксатора над рабочей областью. Моделируйте таблицы как два ящика и сферу и задайте их положение в мире. Более сложные окружения могут быть созданы с помощью 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);

Figure contains an axes. The axes contains 3 objects of type patch.

Добавить робота в SEnvironment

Загрузите модель манипулятора Kinova как rigidBodyTree объект с использованием loadrobot функция.

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

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

show(robot,homeConfiguration(robot),"Parent",ax);

Figure contains an axes. The axes contains 28 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.

Сгенерируйте траекторию и проверяйте на столкновения

Задайте начальное и конечное положение, используя векторы положения и ориентации, которые объединяются с помощью умножения матрицы преобразования.

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);

Figure contains an axes. The axes contains 78 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.

Используйте профиль трапеций скорости, чтобы сгенерировать гладкую траекторию от исходного положения до начального положения, а затем до конечного положения.

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);

Figure contains an axes. The axes contains 53 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.

Сгенерируйте траекторию без столкновения

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

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);

Figure contains an axes. The axes contains 53 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.

Сгенерируйте новую траекторию.

[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 contains an axes. The axes contains 28 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.

Постройте график положения соединений с течением времени.

figure
plot(t,q)
xlabel("Time")
ylabel("Joint Position")

Figure contains an axes. The axes contains 7 objects of type line.

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