В этом примере показано, как запланировать цепкое движение Кинову Джако Асситив Роботикс Арм, использующую алгоритм быстро исследующего случайного дерева (RRT). Этот пример использует plannerRRTStar возразите, чтобы произвести состояния и запланировать движение робота. Если помощники в качестве примера иллюстрируют, как задать пользовательские пространства состояний и валидацию состояния для приложений планирования движения.
Загрузите модель Киновой Джако от библиотеки робота. Эта конкретная модель включает механизм захвата с тремя пальцами.
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 состоит из трех частей:
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,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