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