Проверяйте на экологические столкновения с манипуляторами
Сгенерируйте траекторию без коллизий в ограниченной рабочей области.
Задайте среду столкновения
Создайте простую среду с помощью примитивов столкновения. Этот пример создает сцену, где робот находится в рабочей области и должен переместить объекты от одной таблицы до другого. Робот должен также избежать кругового светильника выше рабочей области. Смоделируйте таблицы как два поля и сферу и задайте их положение в мире. Более сложные среды могут быть созданы с помощью 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);
Добавьте робота в SEnvironment
Загрузите модель манипулятора Киновой как 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аргументы значения имени. Сам столкновения проигнорированы, потому что пределы соединения модели робота предотвращают большинство сам столкновения. Проверка Exhausitive гарантирует, что функция вычисляет все разделительные расстояния и продолжает искать столкновения после обнаружения первого столкновения.
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);
Сгенерируйте траекторию без коллизий
Чтобы избежать этих столкновений, добавьте промежуточное звено 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(:,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")
