Управляйте перемещениями руки PR2 Используя действия ROS и инверсную кинематику

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

Скрипт Open Live Script

В этом примере показано, как отправить команды в автоматизированные манипуляторы в MATLAB®. Объединенные команды положения отправляются через клиент действия ROS по сети ROS. Этот пример также показывает, как вычислить объединенные положения для желаемого положения исполнительного элемента конца. Дерево твердого тела задает геометрию робота и объединенные ограничения, который используется с инверсной кинематикой, чтобы получить положения соединения робота. Можно затем отправить эти объединенные позиции траектории, чтобы управлять, чтобы робот переместился.

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

Породите PR2 в простой среде (только с таблицей, и кокс может) в Средстве моделирования Gazebo. Выполните шаги в Начало работы с 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/

Клиенты действия по созданию для манипуляторов и отправляют команды

В этой задаче вы отправляете объединенные траектории в руки 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);

Вычислите инверсную кинематику для положения исполнительного элемента конца

В этом разделе вы вычисляете траекторию для соединений на основе желаемого исполнительного элемента конца (механизм захвата права PR2) положения. 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, которая помещает механизм захвата (i.e. исполнительный элемент конца) в желаемом положении. Последовательность положений исполнительного элемента конца в течение времени называется траекторией.

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

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

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

Задайте имя исполнительного элемента конца.

endEffectorName = 'r_gripper_tool_frame';

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

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

Задайте желаемое относительное преобразование между исполнительным элементом конца и банкой при схватывании.

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

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

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

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

  • Переместите исполнительный элемент конца от T1 к TGrasp (готовый схватить)

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

  • Переместите исполнительный элемент конца от TGrasp к T2 (лифт может в воздухе),

  • Переместите исполнительный элемент конца от T2 к T3 (перемещение может levelly),

  • Переместите исполнительный элемент конца от 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

Поддержка

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