ROS アクションと逆運動学を使用した PR2 のアーム動作の制御

この例では次を使用します。

この例では、MATLAB® でロボットマニピュレーターにコマンドを送信する方法を説明します。ジョイント位置のコマンドは、ROS ネットワークで ROS アクションクライアントを介して送信されます。この例ではまた、目的のエンドエフェクタ位置を得るためにジョイント位置を計算する方法も説明します。剛体ツリーはロボットの形状とジョイントの拘束を定義します。これを逆運動学と共に使用してロボットのジョイント位置を見出します。これにより、これらのジョイント位置を軌跡として送信して、ロボットに移動するよう指示できます。

PR2 の Gazebo シミュレーションの準備

この例では、こちらからダウンロード可能な、ROS Melodic が含まれる Ubuntu® バーチャルマシン (VM) を使用します。この例は、サポートされているのが ROS Melodic までである ROS パッケージに依存しているため、ROS Noetic をサポートしていません。

VM デスクトップから Gazebo PR2 Simulator デスクトップショートカットを起動することにより、Gazebo シミュレーターのシンプルな環境 (テーブルと飲料缶のみが存在) に PR2 を発生させます。このプロセスの詳細については、Gazebo およびシミュレートされた TurtleBot の入門 (ROS Toolbox)を参照してください。

MATLAB® から ROS ネットワークへの接続

ホストコンピューター上の MATLAB インスタンスで、次のコマンドを実行して MATLAB 内の ROS グローバルノードを初期化し、IP アドレス ipaddress を使用してバーチャルマシン内の ROS マスターに接続します。'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 の duration オブジェクトとして指定します。

% 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 クラスは、必要なすべてのジョイント位置を計算します。これは、アクションクライアントを介して軌跡のゴールメッセージとして送信できます。rigidBodyTreeオブジェクトは、MATLAB® でのロボットパラメーターの定義、コンフィギュレーションの生成、およびロボットの可視化に使用されます。

以下の手順を実行します。

MATLAB® でロボットを操作するために、ROS ネットワークから PR2 ロボットの現在の状態を取得し、rigidBodyTree オブジェクトに割り当てます。
エンドエフェクタのゴール姿勢を定義します。
これらのジョイント位置を使用してロボットのコンフィギュレーションを可視化し、解が適切であることを確認します。
逆運動学を使用して、エンドエフェクタのゴール姿勢からジョイント位置を計算します。
ジョイント位置の軌跡を ROS アクションサーバーに送信して、実際の PR2 ロボットに命令します。

MATLAB® での剛体ツリーの作成

PR2 ロボットを rigidBodyTree オブジェクトとして読み込みます。このオブジェクトは、ロボットのすべての運動学的パラメーター (ジョイント制限を含む) を定義します。

pr2 = exampleHelperWGPR2Kinect;

現在のロボットの状態の取得

ロボットからジョイントの状態を取得するサブスクライバーを作成します。

jointSub = rossubscriber('joint_states');

現在のジョイントの状態のメッセージを取得します。

jntState = receive(jointSub);

ジョイントの状態のメッセージから、ジョイントの位置を pr2 オブジェクトが理解するコンフィギュレーションの struct のフィールドに割り当てます。

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

pr2 ロボットオブジェクトから inverseKinematics オブジェクトを作成します。逆運動学でのゴールは、グリッパー (つまりエンドエフェクタ) を目的の姿勢に配置する、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

タスクに関連するエンドエフェクタの姿勢を指定します。この例で 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]);

目的の直交座標軌跡における主要なウェイポイントを定義します。缶はこの軌跡に沿って移動します。軌跡は以下のように分割できます。

グリッパーを開く
ロボットが握る動作を無理なく開始できるように、エンドエフェクタを現在の姿勢から 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 で指定されている各タスクを 1 つずつ実行します。指定された補助関数を使用してコマンドを送信します。

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/