Движение, планирующее с RRT манипулятор робота

Запланируйте цепкое движение Кинову Джако Ассистив Роботикс Арм, использующую алгоритм быстро исследующего случайного дерева (RRT). Этот пример использует plannerRRTStar возразите, чтобы произвести состояния и запланировать движение робота. Если помощники в качестве примера иллюстрируют, как задать пользовательские пространства состояний и валидацию состояния для приложений планирования движения.

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

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]);

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

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

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

  • Однородно случайная выборка на объединенном пробеле. Уважают объединенные пределы.

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

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

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

  • Te_w - Смещение исполнительного элемента конца преобразовывает в {w}, координаты, {w} производится от WGR

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

Можно задать и конкатенировать несколько WGRs в одной проблеме планирования. В этом примере позволен только один 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 объект с индивидуально настраиваемым пространством состояний и объекты блока проверки допустимости состояния. Задайте запуск и целевые настройки при помощи inverseKinematics решить для configrations на основе положения исполнительного элемента конца. Задайте 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 свойство к истине.

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] Д. Беренсон, С. Сриниваса, Д. Фергюсон, A. Оправа и Дж. Каффнер, "Манипуляция, Планирующая с областями Цели Рабочей области", в Продолжениях Международной конференции IEEE по вопросам Робототехники и Автоматизации, 2009, pp.1397-1403

[2] Д. Беренсон, С. Сриниваса и Дж. Каффнер, "области пробела задачи: среда для ограниченного положением планирования манипуляции", международный журнал исследования робототехники, издания 30, № 12 (2011): 1435-1460

[3] П. Чен и И. Хван, "SANDROS: динамический алгоритм поиска графика для планирования движения", транзакция IEEE на робототехнике и автоматизации, издании 14 № 3 (1998): 390-403

Для просмотра документации необходимо авторизоваться на сайте