主要内容

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

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

ラ@ @ブスクリプトを開く

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

PR2の凉亭シミュレ，ションの準備

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

Vmデスクトップから凉亭PR2模拟器デスクトップショートカットを起動することにより,露台シミュレーターのシンプルな環境(テーブルと飲料缶のみが存在)にPR2を発生させます。このプロセスの詳細にいては，凉亭およびシミュレ，トされた乌龟机器人の入門(ROS工具箱)を参照してください。

Matlab®からrosネットワクへの接続

ホストコンピューター上のMATLABインスタンスで,次のコマンドを実行してMATLAB内のROSグローバルノードを初期化し,IPアドレスipaddressを使用してバーチャルマシン内のROSマスターに接続します。192.168.233.133を,使用しているバーチャルマシンのIPアドレスに置き換えます。必要に応じてポ，トを指定します。

ipaddress =“192.168.116.161”；rosinit (ipaddress, 11311);
使用NodeURI http://192.168.116.1:64988/初始化全局节点/matlab_global_node_03004

ロボットアム用アクションクラアントの作成とコマンドの送信

このタスクでは，ジョントの軌跡をpr2のアムに送信します。アムはrosアクションによって制御できます。ジョイントの軌跡はアームごとに手動で指定され,個別のアクションクライアント経由で個別のゴールメッセージとして送信されます。

ロボットの右アームを動かすゴールメッセージを送信する,ROSのシンプルなアクションクライアントを作成します。rosactionclient(ROS工具箱)オブジェクトとゴ，ルメッセ，ジが返されます。Rosアクションサバにクラアントが接続するまで待機します。

[rArm, rGoalMsg] = rosactionclient(“r_arm_controller / joint_trajectory_action”）;waitForServer (rArm);

ゴ，ルメッセ，ジはtrajectory_msgs / JointTrajectoryPointメッセ，ジです。各軌跡点では，ジョ。

disp (rGoalMsg)
ROS JointTrajectory message with properties: MessageType: 'pr2_controllers_msgs/JointTrajectory ' Trajectory: [1×1 JointTrajectory]使用showdetails显示消息的内容
disp (rGoalMsg.Trajectory)
带有属性的ROS JointTrajectory消息:MessageType: 'trajectory - msgs/JointTrajectory' Header: [1×1 Header] Points: [0×1 JointTrajectory - point] JointNames: {0×1 cell}使用showdetails显示消息的内容

Pr2ロボットのジョ▪▪ント名と一致するジョ▪▪ント名を設定します。制御するジョesc escントが7個ある点に注意してください。PR2の右アームにあるジョイントを調べるには,バーチャルマシンのターミナルで次のコマンドを入力します。

Rosparam get /r_arm_controller/关节
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の持续时间オブジェクトとして指定します。

%点1tjPoint1 = rosmessage(“trajectory_msgs / JointTrajectoryPoint”）;tjPoint1。位置= 0 (1,7);tjPoint1。速度= 0 (1,7);tjPoint1。TimeFromStart = rosduration(1.0);%点2tjPoint2 = rosmessage(“trajectory_msgs / JointTrajectoryPoint”）;tjPoint2。位置= [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];tjPoint2。速度= 0 (1,7);tjPoint2。TimeFromStart = rosduration(2.0);

軌跡内の点をもオブジェクト配列を作成し，ゴルメッセジに割り当てます。アクションクラ▪▪アントを使用してゴ▪▪ルを送信します。関数sendGoalAndWait(ROS工具箱)は，pr2のア，ムが軌跡の実行を終了するまで，実行をブロックします。

rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2];sendGoalAndWait (rArm rGoalMsg);

左アムにコマンドを送信する別のアクションクラアントを作成します。Pr2ロボットのジョeprント名を指定します。

[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。位置= [1.0 0.2 -0.1 -1.2 1.5 -0.3 0.5];tjPoint3。速度= 0 (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);

現在のロボットコンフィギュレ，ションの可視化

显示を使用して，与えられたジョ▪▪▪ント位置ベクトルをも▪▪▪ロボットを可視化します。これは，ロボットの現在の状態と一致します。

显示(pr2 jntPos)

ans =轴(主要)与属性:XLim: [-2 2] YLim: [-2 2] XScale: '线性' YScale: '线性' GridLineStyle: '-'位置:[0.1300 0.1100 0.7750 0.8150]单位:'归一化'显示所有属性

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 =逆运动学(“RigidBodyTree”pr2);

ランダムリスタトを無効にしてikの解の整合性を確保します。

ik.SolverParameters.AllowRandomRestart = false;

ゴ，ル姿勢の各コンポ，ネントの許容誤差に重みを指定します。

权重= [0.25 0.25 0.25 1 1 1];initialGuess = jntPos;%当前JNT pos作为初始猜测

タスクに関連するエンドエフェクタの姿勢を指定します。この例で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,08])*eul2tform([pi/8,0，-pi]);

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

  • グリッパ，を開く

  • ロボットが握る動作を無理なく開始できるように，エンドエフェクタを現在の姿勢からT1に動かす

  • エンドエフェクタをT1からTGraspに動かす(握る準備をする)

  • グリッパ，を閉じて缶を握る

  • エンドエフェクタをTGraspからT2に動かす(缶を空中に持上げる)

  • エンドエフェクタをT2からT3に動かす(缶を水平移動する)

  • エンドエフェクタをT3からTReleaseに動かす(缶をテ，ブル上に降ろし，離す準備をする)

  • グリッパ，を開いて缶を離す

  • エンドエフェクタをTReleaseからT4に動かす(ア，ムを引き戻す)

TGrasp = TCanInitial*TGraspToCan;%抓罐时所需的末端执行器姿态T1 = TGrasp*trvec2tform([0.，0，-0.1]);T2 = TGrasp*trvec2tform([0,0，-0.2]);T3 = TCanFinal*TGraspToCan*trvec2tform([0,0，-0.2]);TCanFinal*TGraspToCan;释放易拉罐时所需的末端执行器姿态T4 = trvec2tform([-0.1,0.05,0]);

実際の動作は一連のnumWaypoints個のジョイント位置で指定され,シンプルなROSアクションクライアントを通してロボットに送信されます。これらのコンフィギュレ，ションはinverseKinematicsオブジェクトを使用して選択され，エンドエフェクタの姿勢が初期姿勢から最終姿勢まで内挿されます。

動作の実行

動作のシ，ケンスを指定します。

motionTask = {“发布”， T1, TGrasp，“把握”， T2, T3，释放，“发布”, T4};

motionTaskで指定されている各タスクを1ず。指定された補助関数を使用してコマンドを送信します。

为i = 1: length(motionTask)如果比较字符串(motionTask {},“把握”) exampleHelperSendPR2GripperCommand (“对”, 0.0, 1000,假);继续结束如果比较字符串(motionTask {},“发布”) exampleHelperSendPR2GripperCommand (“对”、0.1、1、真实);继续结束Tf = motionTask{i};获取当前关节状态jntState = receive(jointSub);jntPos = exampleHelperJointMsgToStruct(pr2, jntState);T0 = getTransform(pr2, jntPos, endEffectorName);在关键路径点之间进行插值numWaypoints = 10;[s, sd, sdd, tvec] = trapveltraj([0 1]， numWaypoints，“AccelTime”, 0.4);%相对缓慢的坡道到最高速度TWaypoints = transformtraj(T0, Tf， [0 1]， tvec，“时间尺度”[s;sd;sdd]);%末端执行器姿态路径点jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);%关节位置航路点rArmJointNames = rGoalMsg.Trajectory.JointNames;rArmJntPosWaypoints = 0 (numWaypoints, nummel (rArmJointNames));使用IK计算每个末端执行器姿态路点的关节位置为k = 1:numWaypoints jntPos = ik(endEffectorName, TWaypoints(:，:，k)， weights, initialGuess);jntPosWaypoints(k，:) = jntPos;initialGuess = jntPos;从jntPos中提取右臂关节位置rArmJointPos = 0(大小(rArmJointNames));为n = 1:length(rArmJointNames) rn = rArmJointNames{n};idx = strcmp({jntPos。JointName}, rn);rArmJointPos(n) = jntPos(idx).JointPosition;结束rArmJntPosWaypoints(k，:) = rArmJointPos';结束%每个路点对应的时间点timePoints = linspace(0,3,numWaypoints);数值估计关节速度轨迹h = diff(时间点);H = H (1);jntTrajectoryPoints = arrayfun(@(~) rosmessage(“trajectory_msgs / JointTrajectoryPoint”), 0 (numWaypoints));[~， rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);为m = 1:numWaypoints jntTrajectoryPoints(m)。位置= rArmJntPosWaypoints(m，:);jntTrajectoryPoints (m)。速度= rArmJntVelWaypoints(m，:);jntTrajectoryPoints (m)。TimeFromStart = rosduration(timePoints(m));结束在MATLAB中可视化机器人运动和末端执行器轨迹(R)持有在为j = 1:numWaypoints show(pr2, jntPosWaypoints(j，:))，“PreservePlot”、假);exampleHelperShowEndEffectorPos (TWaypoints (:,:, j));drawnow;暂停(0.1);结束将右臂轨迹发送给机器人rGoalMsg.Trajectory.Points = jnttrajectory . points;sendGoalAndWait (rArm rGoalMsg);结束

最後に

ア，ムを開始位置に戻します。

exampleHelperSendPR2GripperCommand (“r”rGoalMsg.Trajectory.Points = tjPoint2;sendGoal (rArm rGoalMsg);

rosshutdownを呼び出してrosネットワ，クをシャットダウンし，ロボットから切断します。

rosshutdown
使用NodeURI http://192.168.116.1:64988/关闭全局节点/matlab_global_node_03004