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