Планирование движения с RRT для Робота манипулятора

Планируйте захватывающее движение для Kinova Jaco Assistive Robotics Arm с помощью быстро исследующего алгоритма случайного дерева (RRT). Этот пример использует plannerRRTStar объект в состояниях выборки и планировать движение робота. Предоставленные в качестве примера помощники иллюстрируют, как задать пользовательские пространства состояний и валидацию состояния для приложений планирования движения.

Загрузите модель Kinova Jaco из библиотеки роботов. Эта конкретная модель включает в себя захват с тремя пальцами.

kin = loadrobot('kinovaJacoJ2S7S300');

Создайте окружение

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

floor = collisionBox(1, 1, 0.01);
tabletop1 = collisionBox(0.4,1,0.02);
tabletop1.Pose = trvec2tform([0.3,0,0.6]);
tabletop2 = collisionBox(0.6,0.2,0.02);
tabletop2.Pose = trvec2tform([-0.2,0.4,0.5]);
can = collisionCylinder(0.03,0.16);
can.Pose = trvec2tform([0.3,0.0,0.7]);

Настройте пространство состояний для манипулятора

Рука Кинова имеет десять степеней свободы (DoFs), причем последние три числа степеней свободы соответствуют пальцам. Используйте только первые семь чисел степеней свободы для планирования и держите пальцы в нулевом строении (открыто широко). Система координат ExampleHelperRigidBodyTreeStateSpace пространство состояний создается, чтобы представлять пространство строения (пространство соединений). ExampleHelperRigidBodyTreeStateSpace дискретизирует допустимые состояния для руки робота. The sampleUniform функция пространства состояний чередуется между следующими двумя стратегиями дискретизации с равной вероятностью:

  • Равномерно случайная выборка положения концевого эффектора в области цели рабочей области вокруг ссылки положения цели, затем сопоставьте его с пространством соединений через обратную кинематику. Пределы соединений соблюдаются.

  • Равномерно случайная выборка в пространстве соединений. Пределы соединений соблюдаются.

Первая стратегия дискретизации помогает направлять планировщик RRT к области цели в пространстве задач, чтобы RRT мог сходиться к решению быстрее, чем потеряться в семи пространстве соединений DoF.

Использование области цели рабочей области (WGR) вместо одиночного положения цели увеличивает вероятность нахождения решения путем смещения выборок в область цели. WGR задает непрерывность приемлемых конечных-эффекторных положений для определенных задач. Например, робот может приближаться с нескольких направлений, чтобы захватить чашку воды со стороны, пока он не сталкивается с окружением. Концепция WGR впервые предложена Дмитрием Беренсоном и др. [1] в 2009 году. Этот алгоритм позже превратился в области пространства задач [2]. РГР состоит из трех частей:

  • Twgr_0 - Ссылочное преобразование WGR в мировых координатах ({0})

  • Te_w - Преобразование смещения end-effector в {w} координатах, {w} взят из WGR

  • Bounds - матрица границ 6 на 2 в опорных координатах WGR. Первые три строки Bounds установите допустимое перемещение вдоль осей x, y и z (в метрах) соответственно, а последние три - допустимые повороты вокруг допустимых вращений вокруг осей x, y и z (в радианах). Обратите внимание, что углы Эйлера Roll-Pitch-Yaw (RPY) используются так, как они могут быть интуитивно заданы.

В одной задаче планирования можно определить и объединить несколько WGR. В этом примере разрешен только один WGR.

% Create state space and set workspace goal regions (WGRs)
ss = ExampleHelperRigidBodyTreeStateSpace(kin);
ss.EndEffector = 'j2s7s300_end_effector';

% Define the workspace goal region (WGR)
% This WGR tells the planner that the can shall be grasped from
% the side and the actual grasp height may wiggle at most 1 cm.

% This is the orientation offset between the end-effector in grasping pose and the can frame
R = [0 0 1; 1 0 0; 0 1 0]; 

Tw_0 = can.Pose;
Te_w = rotm2tform(R);
bounds = [0 0;       % x
          0 0;       % y
          0 0.01;    % z
          0 0;       % R
          0 0;       % P
         -pi pi];    % Y
setWorkspaceGoalRegion(ss,Tw_0,Te_w,bounds);

Настройка средства проверки состояния

Настраиваемый валидатор состояния, ExampleHelperValidatorRigidBodyTree, обеспечивает проверку столкновения твёрдого тела между роботом и окружением. Этот валидатор проверяет выбранные строения, и планировщик должен отменить недопустимые состояния.

sv = ExampleHelperValidatorRigidBodyTree(ss);

% Add obstacles in the environment
addFixedObstacle(sv,tabletop1, 'tabletop1', [71 161 214]/256);
addFixedObstacle(sv,tabletop2, 'tabletop2', [71 161 214]/256);
addFixedObstacle(sv,can, 'can', 'r');
addFixedObstacle(sv,floor, 'floor', [1,0.5,0]);

% Skip collision checking for certain bodies for performance
skipCollisionCheck(sv,'root'); % root will never touch any obstacles
skipCollisionCheck(sv,'j2s7s300_link_base'); % base will never touch any obstacles
skipCollisionCheck(sv,'j2s7s300_end_effector'); % this is a virtual frame

% Set the validation distance
sv.ValidationDistance = 0.01;

Планируйте движение захвата

Используйте plannerRRT объект с настроенными объектами состояния space и state validator. Задайте начальные и целевые строения при помощи inverseKinematics решить для конфиграций на основе положения end-effector. Задайте GoalReachedFcn использование exampleHelperIsStateInWorkspaceGoalRegion, который проверяет, достигает ли путь области цели.

% Set random seeds for repeatable results
rng(0,'twister') % 0

% Compute the reference goal configuration. Note this is applicable only when goal bias is larger than 0. 
Te_0ref = Tw_0*Te_w; % Reference end-effector pose in world coordinates, derived from WGR
ik = inverseKinematics('RigidBodyTree',kin);
refGoalConfig = ik(ss.EndEffector,Te_0ref,ones(1,6),homeConfiguration(ss.RigidBodyTree));

% Compute the initial configuration (end-effector is initially under the table)
T = Te_0ref;
T(1,4) = 0.3;
T(2,4) = 0.0;
T(3,4) = 0.4;
initConfig = ik(ss.EndEffector,T,ones(1,6),homeConfiguration(ss.RigidBodyTree));


% Create the planner from previously created state space and state validator
planner = plannerRRT(ss,sv);

% If a node in the tree falls in the WGR, a path is considered found.
planner.GoalReachedFcn = @exampleHelperIsStateInWorkspaceGoalRegion;

% Set the max connection distance.
planner.MaxConnectionDistance = 0.5;

% With WGR, there is no need to specify a particular goal configuration (use
% initConfig to hold the place).
% As a result, one can set GoalBias to zero.
planner.GoalBias = 0;
[pthObj,solnInfo] = plan(planner,initConfig, initConfig);

Визуализация движения Грасса

Найденный путь сначала сглаживается через рекурсивную стратегию вырезывания углов [3], прежде чем движение будет анимировано.

% Smooth the path.
interpolate(pthObj,100);
newPathObj = exampleHelperPathSmoothing(pthObj,sv);
interpolate(newPathObj,200);

figure
states = newPathObj.States;

% Draw the robot.
ax = show(kin,states(1,:));
zlim(ax, [-0.03, 1.4])
xlim(ax, [-1, 1])
ylim(ax, [-1, 1])


% Render the environment.
hold on
showObstacles(sv, ax);
view(146, 33)
camzoom(1.5)

% Show the motion.
for i = 2:length(states)
    show(kin,states(i,:),'PreservePlot',false,'Frames','off','Parent',ax);
    drawnow
end


q = states(i,:);

% Grab the can.
q = exampleHelperEndEffectorGrab(sv,'can',q, ax);

Установите целевой psotion для банки на другой столешнице.

targetPos = [-0.15,0.35,0.51];
exampleHelperDrawHorizontalCircle(targetPos,0.02,'y',ax);

Figure contains an axes. The axes contains 28 objects of type patch, line. These objects represent world, root, j2s7s300_link_base, j2s7s300_link_1, j2s7s300_link_2, j2s7s300_link_3, j2s7s300_link_4, j2s7s300_link_5, j2s7s300_link_6, j2s7s300_link_7, j2s7s300_end_effector, j2s7s300_link_finger_1, j2s7s300_link_finger_tip_1, j2s7s300_link_finger_2, j2s7s300_link_finger_tip_2, j2s7s300_link_finger_3, j2s7s300_link_finger_tip_3, workpiece, root_mesh, j2s7s300_link_base_mesh, j2s7s300_link_1_mesh, j2s7s300_link_2_mesh, j2s7s300_link_3_mesh, j2s7s300_link_4_mesh, j2s7s300_link_5_mesh, j2s7s300_link_6_mesh, j2s7s300_link_7_mesh, j2s7s300_end_effector_mesh, j2s7s300_link_finger_1_mesh, j2s7s300_link_finger_tip_1_mesh, j2s7s300_link_finger_2_mesh, j2s7s300_link_finger_tip_2_mesh, j2s7s300_link_finger_3_mesh, j2s7s300_link_finger_tip_3_mesh, workpiece_mesh.

Планируйте Движение Перемещения

Во время движения, держите цилиндр может выровняться в любое время, чтобы избежать разлива. Задайте дополнительное ограничение для промежуточных строений манипулятора для планировщика RRT. Включите ограничение путем установки UseConstrainedSampling свойство true.

Tw_0 = trvec2tform(targetPos+[0,0,0.08]); 
Te_w = rotm2tform(R);
bounds =  [0 0;       % x
           0 0;       % y
           0 0;       % z
           0 0;       % R
           0 0;       % P
          -pi pi];    % Y
setWorkspaceGoalRegion(ss,Tw_0,Te_w,bounds);
ss.UseConstrainedSampling = true;
planner.MaxConnectionDistance = 0.05;
[pthObj2,~] = plan(planner,q,q);

Визуализируйте движение.

states = pthObj2.States;

view(ax, 152,45)
for i = 2:length(states)
    show(kin,states(i,:),'PreservePlot',false,'Frames','off','Parent',ax);
    drawnow
end
q = states(i,:);

% let go of the can
q = exampleHelperEndEffectorRelease(sv,q,ax);

Figure contains an axes. The axes contains 51 objects of type patch, line. These objects represent world, root, j2s7s300_link_base, j2s7s300_link_1, j2s7s300_link_2, j2s7s300_link_3, j2s7s300_link_4, j2s7s300_link_5, j2s7s300_link_6, j2s7s300_link_7, j2s7s300_end_effector, j2s7s300_link_finger_1, j2s7s300_link_finger_tip_1, j2s7s300_link_finger_2, j2s7s300_link_finger_tip_2, j2s7s300_link_finger_3, j2s7s300_link_finger_tip_3, workpiece, root_mesh, j2s7s300_link_base_mesh, j2s7s300_link_1_mesh, j2s7s300_link_2_mesh, j2s7s300_link_3_mesh, j2s7s300_link_4_mesh, j2s7s300_link_5_mesh, j2s7s300_link_6_mesh, j2s7s300_link_7_mesh, j2s7s300_end_effector_mesh, j2s7s300_link_finger_1_mesh, j2s7s300_link_finger_tip_1_mesh, j2s7s300_link_finger_2_mesh, j2s7s300_link_finger_tip_2_mesh, j2s7s300_link_finger_3_mesh, j2s7s300_link_finger_tip_3_mesh.

Ссылки

[1] D. Berenson, S. Srinivasa, D. Ferguson, A. Collet, and J. Kuffner, «Manipulation Planning with Workspace Goal Областей», в трудах Международной конференции IEEE по робототехнике и автоматизации, 2009, стр. 1397-1403

[2] D. Berenson, S. Srinivasa, and J. Kuffner, «Task Space Regions: A Framework for Pose-Constructed Manipulation Planning», International Journal of Robotics Research, Vol. 30, № 12 (2011): 1435-1460

[3] P. Chen, and Y. Hwang, «SANDROS: A Dynamic Graph Search Algorithm for Motion Planning», Транзакция IEEE по робототехнике и автоматизации, том 14 № 3 (1998): 390-403