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

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

В этом примере показано, как отправить команды роботам-манипуляторам в 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/

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