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