Управляйте PR2 движениях рук, используя действия ROS и обратную кинематику

Этот пример использует:

Открыть Live Script

В этом примере показано, как отправить команды роботам-манипуляторам в MATLAB ®. Команды положения соединения передаются через клиент действия ROS через сеть ROS. Этот пример также показывает, как вычислить положения соединений для желаемого положения концевого эффектора. Древо твёрдого тела задает геометрию робота и ограничения в соединениях, которые используются с обратной кинематикой, чтобы получить положения соединений робота. Затем можно отправить эти положения соединений как траекторию, чтобы заставить робота двигаться.

Подведение итогов PR2 симуляции Gazebo

Нерест PR2 в простое окружение (только с таблицей и коксовой банкой) в Gazebo Simulator. Следуйте шагам в запуске с Gazebo и моделируемым TurtleBot (ROS Toolbox), чтобы запустить Gazebo PR2 Simulator с рабочего стола виртуальной машины Ubuntu ®.

Подключение к сети ROS из MATLAB

®

В своем образце MATLAB на хост-компьютер запустите следующие команды, чтобы инициализировать глобальный узел ROS в MATLAB и соединиться с мастером ROS в виртуальной машине через его IP-адрес ipaddress. Замените '192.168.233.133' на IP-адрес вашей виртуальной машины. При необходимости укажите порт.

ipaddress = '192.168.116.161';
rosinit(ipaddress,11311);
Initializing global node /matlab_global_node_03004 with NodeURI http://192.168.116.1:64988/

Создайте клиенты Action для роботов Arms и отправьте команды

В этой задаче вы отправляете траектории соединений в PR2 руки. Руками можно управлять с помощью действий ROS. Траектории соединений задаются вручную для каждой руки и отправляются как отдельные сообщения о целях через отдельные клиенты действий.

Создайте простой клиент действия ROS, чтобы отправить сообщения о целях для перемещения правой руки робота. rosactionclient возвращаются (ROS Toolbox) объект и сообщение цели. Дождитесь подключения клиента к серверу действий ROS.

[rArm, rGoalMsg] = rosactionclient('r_arm_controller/joint_trajectory_action');
waitForServer(rArm);

Сообщение о цели является trajectory_msgs/JointTrajectoryPoint сообщение. Каждая точка траектории должна задавать положения и скорости соединений.

disp(rGoalMsg)
  ROS JointTrajectoryGoal message with properties:

    MessageType: 'pr2_controllers_msgs/JointTrajectoryGoal'
     Trajectory: [1×1 JointTrajectory]

  Use showdetails to show the contents of the message

disp(rGoalMsg.Trajectory)
  ROS JointTrajectory message with properties:

    MessageType: 'trajectory_msgs/JointTrajectory'
         Header: [1×1 Header]
         Points: [0×1 JointTrajectoryPoint]
     JointNames: {0×1 cell}

  Use showdetails to show the contents of the message

Установите имена соединений так, чтобы они совпадали с именами PR2 соединений роботов. Обратите внимание, что существует 7 соединений для управления. Чтобы найти соединения в PR2 правой руки, введите эту команду в терминале виртуальной машины:

rosparam get /r_arm_controller/joints 

rGoalMsg.Trajectory.JointNames = {'r_shoulder_pan_joint', ...
                                   'r_shoulder_lift_joint', ...
                                   'r_upper_arm_roll_joint', ...
                                   'r_elbow_flex_joint',...
                                   'r_forearm_roll_joint',...
                                   'r_wrist_flex_joint',...
                                   'r_wrist_roll_joint'};

Создайте уставки в траектории соединения рычагов путем создания ROS trajectory_msgs/JointTrajectoryPoint сообщения и определение положения и скорости всех 7 соединений в качестве вектора. Задайте время от начала как объект длительности ROS. 

% Point 1
tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint1.Positions = zeros(1,7);
tjPoint1.Velocities = zeros(1,7);
tjPoint1.TimeFromStart = rosduration(1.0);

% Point 2
tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];
tjPoint2.Velocities = zeros(1,7);
tjPoint2.TimeFromStart = rosduration(2.0);

Создайте объектный массив с точками на траектории и присвойте его целевому сообщению. Отправьте цель с помощью клиента действия. sendGoalAndWait (ROS Toolbox) функция будет блокировать выполнение, пока PR2 рука не завершит выполнение траектории

rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2];

sendGoalAndWait(rArm,rGoalMsg);

Создайте другой клиент действий, чтобы отправить команды в левую руку. Укажите имена соединений робота- PR2.

[lArm, lGoalMsg] = rosactionclient('l_arm_controller/joint_trajectory_action');
waitForServer(lArm);

lGoalMsg.Trajectory.JointNames = {'l_shoulder_pan_joint', ...
                                   'l_shoulder_lift_joint', ...
                                   'l_upper_arm_roll_joint', ...
                                   'l_elbow_flex_joint',...
                                   'l_forearm_roll_joint',...
                                   'l_wrist_flex_joint',...
                                   'l_wrist_roll_joint'};

Установите точку траектории для левого рычага. Присвойте его целевому сообщению и отправьте цель.

tjPoint3 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint3.Positions = [1.0 0.2 -0.1 -1.2 1.5 -0.3 0.5];
tjPoint3.Velocities = zeros(1,7);
tjPoint3.TimeFromStart = rosduration(2.0);

lGoalMsg.Trajectory.Points = tjPoint3;

sendGoalAndWait(lArm,lGoalMsg);

Вычислите обратную кинематику для положения End-Effector

В этом разделе вы вычисляете траекторию для соединений на основе требуемых положений концевого эффектора (PR2 правого захвата). The inverseKinematics класс вычисляет все необходимые положения соединений, которые могут быть отправлены в качестве сообщения цели траектории через клиент действий. A rigidBodyTree объект используется для определения параметров робота, генерации строений и визуализации робота в MATLAB ®.

Выполните следующие шаги:

  • Получите текущее состояние робота- PR2 из сети ROS и присвойте его rigidBodyTree объект для работы с роботом в MATLAB ®.

  • Задайте конечное положение цели эффектора.

  • Визуализируйте строение робота, используя эти положения соединений, чтобы гарантировать правильное решение.

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

  • Отправьте траекторию положений соединений на сервер действий ROS, чтобы выполнить команду фактического робота- PR2.

Создайте древовидное твердое тело в MATLAB

®

Загрузите PR2 робота как rigidBodyTree объект. Этот объект определяет все кинематические параметры (включая пределы соединений) робота.

pr2 = exampleHelperWGPR2Kinect;

Получите текущее состояние робота

Создайте абонента, чтобы получить совместные состояния от робота. 

jointSub = rossubscriber('joint_states');

Получите текущее сообщение о совместном состоянии.

jntState = receive(jointSub);

Присвойте положения соединений из сообщения о состояниях соединений полям struct строения, которую pr2 объект понимает.

jntPos = exampleHelperJointMsgToStruct(pr2,jntState);

Визуализация текущего строения робота

Использовать show визуализировать робота с заданным вектором положения соединения. Это должно совпадать с текущим состоянием робота. 

show(pr2,jntPos)

ans = 
  Axes (Primary) with properties:

             XLim: [-2 2]
             YLim: [-2 2]
           XScale: 'linear'
           YScale: 'linear'
    GridLineStyle: '-'
         Position: [0.1300 0.1100 0.7750 0.8150]
            Units: 'normalized'

  Show all properties

Создайте inverseKinematics объект из pr2 объект робота. Цель обратной кинематики состоит в том, чтобы вычислить углы поворота шарнира для PR2 рычага, который помещает захват (то есть эффектор конца) в желаемое положение. Последовательность положений энд-эффектора в течение периода времени называется траекторией.

В этом примере мы будем управлять только руками робота. Поэтому мы устанавливаем жесткие пределы на туловище во время планирования.

torsoJoint = pr2.getBody('torso_lift_link').Joint;
idx = strcmp({jntPos.JointName}, torsoJoint.Name);
torsoJoint.HomePosition = jntPos(idx).JointPosition;
torsoJoint.PositionLimits = jntPos(idx).JointPosition + [-1e-3,1e-3];

Создайте inverseKinematics объект.

ik = inverseKinematics('RigidBodyTree', pr2);

Отключите случайный перезапуск, чтобы гарантировать согласованность решений IK.

ik.SolverParameters.AllowRandomRestart = false;

Задайте веса для допусков для каждого компонента положения цели. 

weights = [0.25 0.25 0.25 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess

Задайте положения end-effector, связанные с задачей. В этом примере PR2 доберемся до баллончика на таблице, схватите баллончик, переместите его в другое место и снова установите его вниз. Мы будем использовать inverseKinematics объект для планирования движений, которые плавно переходят от одного концевого эффекторного положения к другому.

Задайте имя конечного эффектора.

endEffectorName = 'r_gripper_tool_frame';

Задайте начальное (текущее) положение банки и желаемое окончательное положение.

TCanInitial = trvec2tform([0.7, 0.0, 0.55]);
TCanFinal = trvec2tform([0.6, -0.5, 0.55]);

Задайте требуемое относительное преобразование между end-effector и банкой при захвате.

TGraspToCan = trvec2tform([0,0,0.08])*eul2tform([pi/8,0,-pi]);

Задайте ключевые точки пути нужной Декартовой траектории. Банка должна двигаться по этой траектории. Траектория может быть разделена следующим образом:

  • Откройте захват

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

  • Переместите конечный эффектор из T1 на TGrasp (готов к захвату)

  • Закройте захват и схватите банку

  • Переместите конечный эффектор из TGrasp на T2 (лифт банка в воздухе)

  • Переместите конечный эффектор из T2 на T3 (перемещение может нивелироваться)

  • Переместите конечный эффектор из T3 на TRelease (нижняя баллончик к поверхности стола и готов к освобождению)

  • Откройте захват и отпустите банку

  • Переместите конечный эффектор из TRelease на T4 (отведите руку)

TGrasp = TCanInitial*TGraspToCan; % The desired end-effector pose when grasping the can
T1 = TGrasp*trvec2tform([0.,0,-0.1]);
T2 = TGrasp*trvec2tform([0,0,-0.2]);
T3 = TCanFinal*TGraspToCan*trvec2tform([0,0,-0.2]);
TRelease = TCanFinal*TGraspToCan; % The desired end-effector pose when releasing the can
T4 = TRelease*trvec2tform([-0.1,0.05,0]);

Фактическое движение будет задано как numWaypoints положения соединений в последовательности и отправка роботу через клиент простого действия ROS. Эти строения будут выбраны с помощью inverseKinematics объект таким образом, чтобы положение концевого эффектора интерполировалось из начального положения в окончательное положение.

Выполните Движение

Задайте последовательность движений.

motionTask = {'Release', T1, TGrasp, 'Grasp', T2, T3, TRelease, 'Release', T4};

Выполните каждую задачу, указанную в motionTask один за другим. Отправляйте команды с помощью заданных вспомогательных функций.

for i = 1: length(motionTask)
    
    if strcmp(motionTask{i}, 'Grasp')
        exampleHelperSendPR2GripperCommand('right',0.0,1000,false); 
        continue
    end
    
    if strcmp(motionTask{i}, 'Release')
        exampleHelperSendPR2GripperCommand('right',0.1,-1,true);
        continue
    end  
    
    Tf = motionTask{i};
    % Get current joint state
    jntState = receive(jointSub);
    jntPos = exampleHelperJointMsgToStruct(pr2, jntState);
    
    T0 = getTransform(pr2, jntPos, endEffectorName);  
    
    % Interpolating between key waypoints
    numWaypoints = 10;
    [s, sd, sdd, tvec] = trapveltraj([0 1], numWaypoints, 'AccelTime', 0.4); % Relatively slow ramp-up to top speed
    TWaypoints = transformtraj(T0, Tf, [0 1], tvec, 'TimeScaling', [s; sd; sdd]); % end-effector pose waypoints
    jntPosWaypoints = repmat(initialGuess, numWaypoints, 1); % joint position waypoints
    
    rArmJointNames = rGoalMsg.Trajectory.JointNames;
    rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
    
    % Calculate joint position for each end-effector pose waypoint using IK
    for k = 1:numWaypoints
        jntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
        jntPosWaypoints(k, :) = jntPos;
        initialGuess = jntPos;
        
        % Extract right arm joint positions from jntPos
        rArmJointPos = zeros(size(rArmJointNames));
        for n = 1:length(rArmJointNames)
            rn = rArmJointNames{n};
            idx = strcmp({jntPos.JointName}, rn);
            rArmJointPos(n) = jntPos(idx).JointPosition;
        end  
        rArmJntPosWaypoints(k,:) = rArmJointPos'; 
    end
    
    % Time points corresponding to each waypoint
    timePoints = linspace(0,3,numWaypoints);
        
    % Estimate joint velocity trajectory numerically
    h = diff(timePoints); h = h(1);
    jntTrajectoryPoints = arrayfun(@(~) rosmessage('trajectory_msgs/JointTrajectoryPoint'), zeros(1,numWaypoints)); 
    [~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);
    for m = 1:numWaypoints
        jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
        jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
        jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
    end
    
    % Visualize robot motion and end-effector trajectory in MATLAB(R)
    hold on
    for j = 1:numWaypoints
        show(pr2, jntPosWaypoints(j,:),'PreservePlot', false);
        exampleHelperShowEndEffectorPos(TWaypoints(:,:,j));
        drawnow;
        pause(0.1);
    end
    
    % Send the right arm trajectory to the robot
    rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
    sendGoalAndWait(rArm, rGoalMsg);

end

Перенос вверх

Переместите руку назад в исходное положение.

exampleHelperSendPR2GripperCommand('r',0.0,-1)
rGoalMsg.Trajectory.Points = tjPoint2;
sendGoal(rArm, rGoalMsg);

Функции rosshutdown для завершения работы сети ROS и отсоединения от робота.

rosshutdown
Shutting down global node /matlab_global_node_03004 with NodeURI http://192.168.116.1:64988/

Документация по Robotics System Toolbox

Поддержка

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