Документация

Задайте манипулятор

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

kin = loadrobot('kinovaJacoJ2S7S300');

Создайте среду

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

floor = collisionBox(1, 1, 0.01);
tabletop = collisionBox(0.4,1,0.02);
tabletop.Pose = trvec2tform([0.3,0,0.6]);
can = collisionCylinder(0.03,0.16);
can.Pose = trvec2tform([0.3,0.0,0.68]);

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

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

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

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

Можно задать и конкатенировать несколько 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,tabletop);
addFixedObstacle(sv,can);
addFixedObstacle(sv,floor);

% 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 conifiguration. 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 conifiguration (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.3;

% turn off GoalBias for now
planner.GoalBias = 0;
[pthObj,solnInfo] = plan(planner,initConfig,refGoalConfig)

pthObj = 
  navPath with properties:

    StateSpace: [1×1 ExampleHelperRigidBodyTreeStateSpace]
        States: [21×10 double]
     NumStates: 21

solnInfo = struct with fields:
      IsPathFound: 1
         ExitFlag: 1
         NumNodes: 42
    NumIterations: 186
         TreeData: [128×10 double]

Визуализируйте цепкое движение

Найденный путь сначала сглаживается через рекурсивную сокращающую угол стратегию [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,:));

% Render the environment
hold on
[~,pFloor] = show(floor,'Parent',ax);
[~,pTable] = show(tabletop,'Parent',ax);
[~,pCan] = show(can,'Parent',ax);
pFloor.LineStyle = 'none';
pTable.LineStyle = 'none';
pCan.LineStyle = 'none';
pTable.FaceColor = [71 161 214]/256;
pCan.FaceColor = 'r';

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

Ссылки

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

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

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