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

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

Задайте среду

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

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

exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

Добавьте робота манипулятора

Добавьте манипулятор Киновой в среду в начале координат. Загрузите предоставленную модель робота. Визуализируйте препятствия и покажите робота на том же рисунке.

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,homeConfiguration(robot),"Parent",ax);

Смоделируйте манипулятор как массив объектов столкновения

Создайте массив объектов столкновения от rigidBodyTree объект. Этот подход использует помощника в качестве примера, exampleHelperManipCollisionsFromVisuals, это извлекает сетки, сначала визуальные в каждом rigidBody объект. Для обзора других подходов относитесь, чтобы Создать Объекты Столкновения для Проверки Столкновения Манипулятора.

% Generate an array of collision objects from the visuals of the associated tree
collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

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

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

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

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

%Initialize outputs
isCollision = false(length(q),1); % Check whether each pose is in collision
selfCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision

for i = 1:length(q)
    [isCollision(i),selfCollisionPairIdx{i},worldCollisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

isTrajectoryInCollision = logical
   1

Смотрите проблемные случаи

Путем осмотра столкновений существует 2 появления столкновений. Визуализируйте эти настройки, чтобы заняться расследованиями далее.

problemIdx1 = find(isCollision,1);
problemIdx2 = find(isCollision,1,"last");

% Identify the problem rigid bodies
problemBodies1 = [selfCollisionPairIdx{problemIdx1} worldCollisionPairIdx{problemIdx1}*[1 0]'];
problemBodies2 = [selfCollisionPairIdx{problemIdx2} worldCollisionPairIdx{problemIdx2}*[1 0]'];

% Visualize the environment
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

% Add the robots & highlight the problem bodies
show(robot,q(:,problemIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,problemBodies1,ax);
show(robot,q(:,problemIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,problemBodies2,ax);

Сгенерируйте Траекторию Без коллизий с помощью Промежуточного Waypoints

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

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(:,problemIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,problemIdx2));

% 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
isCollision = false(length(q),1); % Check whether each pose is in collision
collisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
for i = 1:length(q)
    [isCollision(i),collisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

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

Документация