rigidBodyTree
ツリー構造のロボットを作成
説明
rigidBodyTree は、剛体とジョイントとの接続の表現です。このクラスを使用して、MATLAB® でロボット マニピュレーター モデルを構築します。Unified Robot Description Format (URDF) を使用して指定されたロボット モデルがある場合、importrobot を使用してロボット モデルをインポートします。
剛体ツリー モデルは、rigidBody オブジェクトとしての剛体から構成されます。各剛体には、その剛体が親ボディを基準とした可動域を定義する rigidBodyJoint オブジェクトが関連付けられています。setFixedTransform を使用して、ジョイントの座標系と隣接ボディの 1 つの座標系の固定変換を定義します。RigidBodyTree クラスのメソッドを使用して、モデルに対して剛体を追加、置換、または削除できます。
ロボット ダイナミクスの計算も可能です。ロボット モデルの各 rigidBody について、Mass、CenterOfMass および Inertia の各プロパティを指定します。順ダイナミクスと逆ダイナミクスを外力ありまたはなしで計算して、与えられたロボットのジョイント運動とジョイント入力に対するダイナミクスの量を計算できます。ダイナミクス関連の関数を使用するには、DataFormat プロパティを "row" または "column" に設定します。
また、特定の剛体ツリー モデルに対して、ロボティクスの逆運動学アルゴリズムを使用して目標となるエンドエフェクタ位置のジョイント角度を計算するために、ロボット モデルを使用することもできます。inverseKinematics または generalizedInverseKinematics を使用するときには、剛体ツリー モデルを指定します。
show メソッドは、ボディ メッシュの可視化をサポートします。メッシュは .stl ファイルとして指定され、addVisual を使用して個々の剛体に追加できます。また、関数 importrobot は、ご使用の URDF ロボット モデルで指定されているすべてのアクセス可能な .stl ファイルを既定で読み込みます。
作成
プロパティ
オブジェクト関数
例
剛体ツリーへの剛体とジョイントの接続
剛体および対応するジョイントを剛体ツリーに追加します。それぞれの rigidBody オブジェクトは rigidBodyJoint オブジェクトを含み、addBody を使用して rigidBodyTree に追加しなければなりません。
剛体ツリーを作成します。
rbtree = rigidBodyTree;
固有の名前をもつ剛体を作成します。
body1 = rigidBody('b1');回転ジョイントを作成します。既定で、rigidBody オブジェクトには固定ジョイントが付属します。body1.Joint プロパティに新しい rigidBodyJoint オブジェクトを割り当てることにより、ジョイントを置き換えます。
jnt1 = rigidBodyJoint('jnt1','revolute'); body1.Joint = jnt1;
剛体をツリーに追加します。剛体を接続するボディ名を指定します。最初のボディなので、ツリーのベース名を使用します。
basename = rbtree.BaseName; addBody(rbtree,body1,basename)
ツリーに対して showdetails を使用して、剛体とジョイントが正しく追加されたことを確認します。
showdetails(rbtree)
-------------------- Robot: (1 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 b1 jnt1 revolute base(0) --------------------
Denavit-Hartenberg パラメーターを使用したマニピュレーター ロボットの構築
Puma560® ロボットの Denavit-Hartenberg (DH) パラメーターを使用して、ロボットを構築します。各剛体は 1 つずつ追加され、子から親への変換がジョイント オブジェクトによって指定されます。
DH パラメーターは、各剛体がどのように親に接続されているかを基準として、ロボットのジオメトリを定義します。わかりやすくするために、Puma560 ロボットのパラメーターを行列で設定します [1]。Puma ロボットは連続チェーンのマニピュレーターです。DH パラメーターは、前のジョイント接続に対応する行列内の前の行との相対値です。
dhparams = [0 pi/2 0 0;
0.4318 0 0 0
0.0203 -pi/2 0.15005 0;
0 pi/2 0.4318 0;
0 -pi/2 0 0;
0 0 0 0];剛体ツリー オブジェクトを作成して、ロボットを構築します。
robot = rigidBodyTree;
最初の剛体を作成し、ロボットに追加します。剛体を追加するには次を行います。
rigidBodyオブジェクトを作成し、一意の名前を付けます。rigidBodyJointオブジェクトを作成し、一意の名前を付けます。setFixedTransformを使用して、ボディからボディへの変換を DH パラメーターを使用して指定します。DH パラメーターの最後の要素であるthetaは無視されます。これは、角度がジョイント位置に依存するためです。addBodyを呼び出して、最初のボディのジョイントをロボットの base 座標系に接続します。
body1 = rigidBody('body1'); jnt1 = rigidBodyJoint('jnt1','revolute'); setFixedTransform(jnt1,dhparams(1,:),'dh'); body1.Joint = jnt1; addBody(robot,body1,'base')
その他の剛体を作成し、ロボットに追加します。addBody を呼び出して接続するときには、前述のボディ名を指定します。各固定変換は、前のジョイント座標系と相対的です。
body2 = rigidBody('body2'); jnt2 = rigidBodyJoint('jnt2','revolute'); body3 = rigidBody('body3'); jnt3 = rigidBodyJoint('jnt3','revolute'); body4 = rigidBody('body4'); jnt4 = rigidBodyJoint('jnt4','revolute'); body5 = rigidBody('body5'); jnt5 = rigidBodyJoint('jnt5','revolute'); body6 = rigidBody('body6'); jnt6 = rigidBodyJoint('jnt6','revolute'); setFixedTransform(jnt2,dhparams(2,:),'dh'); setFixedTransform(jnt3,dhparams(3,:),'dh'); setFixedTransform(jnt4,dhparams(4,:),'dh'); setFixedTransform(jnt5,dhparams(5,:),'dh'); setFixedTransform(jnt6,dhparams(6,:),'dh'); body2.Joint = jnt2; body3.Joint = jnt3; body4.Joint = jnt4; body5.Joint = jnt5; body6.Joint = jnt6; addBody(robot,body2,'body1') addBody(robot,body3,'body2') addBody(robot,body4,'body3') addBody(robot,body5,'body4') addBody(robot,body6,'body5')
関数 showdetails または関数 show を使用して、ロボットが正しく構築されていることを確認します。showdetails は、すべてのボディを MATLAB® コマンド ウィンドウにリストします。show は、指定されたコンフィギュレーション (既定はホーム) でロボットを表示します。axis の呼び出しは、軸の範囲を変更し、軸ラベルを非表示にします。
showdetails(robot)
-------------------- Robot: (6 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 body1 jnt1 revolute base(0) body2(2) 2 body2 jnt2 revolute body1(1) body3(3) 3 body3 jnt3 revolute body2(2) body4(4) 4 body4 jnt4 revolute body3(3) body5(5) 5 body5 jnt5 revolute body4(4) body6(6) 6 body6 jnt6 revolute body5(5) --------------------
show(robot);
axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
axis off
[1] Corke, P. I., and B. Armstrong-Helouvry. "A Search for Consensus among Model Parameters Reported for the PUMA 560 Robot." Proceedings of the 1994 IEEE International Conference on Robotics and Automation, IEEE Computer.Soc. Press, 1994, pp. 1608–13. DOI.org (Crossref), doi:10.1109/ROBOT.1994.351360.
ロボットの剛体ツリー モデルの変更
既存の rigidBodyTree オブジェクトに変更を加えます。剛体ツリーのジョイント、ボディ、およびサブツリーを置換できます。
Robotics System Toolbox™ loadrobotから ABB IRB-120T マニピュレーターを読み込みます。rigidBodyTree オブジェクトとして指定します。
manipulator = loadrobot("abbIrb120T");ロボットを with show で表示し、showdetails. を使用してロボットの詳細を読み取ります。
show(manipulator);
showdetails(manipulator)
-------------------- Robot: (8 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 base base_link-base fixed base_link(0) 2 link_1 joint_1 revolute base_link(0) link_2(3) 3 link_2 joint_2 revolute link_1(2) link_3(4) 4 link_3 joint_3 revolute link_2(3) link_4(5) 5 link_4 joint_4 revolute link_3(4) link_5(6) 6 link_5 joint_5 revolute link_4(5) link_6(7) 7 link_6 joint_6 revolute link_5(6) tool0(8) 8 tool0 joint6-tool0 fixed link_6(7) --------------------
プロパティを検査する特定のボディを取得します。link_3 ボディの唯一の子は link_4 ボディです。特定のボディをコピーすることもできます。
body3 = getBody(manipulator,"link_3");
childBody = body3.Children{1}childBody =
rigidBody with properties:
Name: 'link_4'
Joint: [1x1 rigidBodyJoint]
Mass: 1.3280
CenterOfMass: [0.2247 1.5000e-04 4.1000e-04]
Inertia: [0.0028 0.0711 0.0723 1.3052e-05 -1.3878e-04 -6.6037e-05]
Parent: [1x1 rigidBody]
Children: {[1x1 rigidBody]}
Visuals: {'Mesh Filename link_4.stl'}
Collisions: {'Mesh Filename link_4.stl'}
body3Copy = copy(body3);
link_3 ボディのジョイントを置き換えます。下流のボディ ジオメトリへの影響を確実に防ぐために、新しい Joint オブジェクトを作成し、replaceJoint を使用しなければなりません。必要に応じて、既定の単位行列を使用する代わりに、setFixedTransform を呼び出してボディ間の変換を定義します。
newJoint = rigidBodyJoint("prismatic"); replaceJoint(manipulator,"link_3",newJoint); showdetails(manipulator)
-------------------- Robot: (8 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 base base_link-base fixed base_link(0) 2 link_1 joint_1 revolute base_link(0) link_2(3) 3 link_2 joint_2 revolute link_1(2) link_3(4) 4 link_3 prismatic fixed link_2(3) link_4(5) 5 link_4 joint_4 revolute link_3(4) link_5(6) 6 link_5 joint_5 revolute link_4(5) link_6(7) 7 link_6 joint_6 revolute link_5(6) tool0(8) 8 tool0 joint6-tool0 fixed link_6(7) --------------------
removeBody を使用してボディ全体を削除し、結果のサブツリーを取得します。削除されたボディはサブツリーに含まれます。
subtree = removeBody(manipulator,"link_4")subtree =
rigidBodyTree with properties:
NumBodies: 4
Bodies: {[1x1 rigidBody] [1x1 rigidBody] [1x1 rigidBody] [1x1 rigidBody]}
Base: [1x1 rigidBody]
BodyNames: {'link_4' 'link_5' 'link_6' 'tool0'}
BaseName: 'link_3'
Gravity: [0 0 0]
DataFormat: 'struct'
show(subtree);
変更した link_3 ボディを削除します。コピーされた元の link_3 ボディを link_2 ボディに追加し、次に返されたサブツリーも追加します。ロボット モデルはそのままにします。showdetails で詳細な比較を確認します。
removeBody(manipulator,"link_3"); addBody(manipulator,body3Copy,"link_2") addSubtree(manipulator,"link_3",subtree) showdetails(manipulator)
-------------------- Robot: (8 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 base base_link-base fixed base_link(0) 2 link_1 joint_1 revolute base_link(0) link_2(3) 3 link_2 joint_2 revolute link_1(2) link_3(4) 4 link_3 joint_3 revolute link_2(3) link_4(5) 5 link_4 joint_4 revolute link_3(4) link_5(6) 6 link_5 joint_5 revolute link_4(5) link_6(7) 7 link_6 joint_6 revolute link_5(6) tool0(8) 8 tool0 joint6-tool0 fixed link_6(7) --------------------
剛体ツリーへのダイナミクス プロパティの指定
ダイナミクス関数を使用してジョイント トルクとジョイント加速度を計算するには、rigidBodyTreeオブジェクトとrigidBodyにダイナミクス プロパティを指定します。
剛体ツリー モデルを作成します。接続する 2 つの剛体を作成します。
robot = rigidBodyTree('DataFormat','row'); body1 = rigidBody('body1'); body2 = rigidBody('body2');
ボディに接続するジョイントを指定します。body2 から body1 への固定変換を設定します。変換は x 方向に 1 m です。
joint1 = rigidBodyJoint('joint1','revolute'); joint2 = rigidBodyJoint('joint2'); setFixedTransform(joint2,trvec2tform([1 0 0])) body1.Joint = joint1; body2.Joint = joint2;
2 つのボディのダイナミクス プロパティを指定します。ボディをロボット モデルに追加します。この例では、ロッド (body1) と、接続された球体 (body2) の基本値が与えられています。
body1.Mass = 2; body1.CenterOfMass = [0.5 0 0]; body1.Inertia = [0.001 0.67 0.67 0 0 0]; body2.Mass = 1; body2.CenterOfMass = [0 0 0]; body2.Inertia = 0.0001*[4 4 4 0 0 0]; addBody(robot,body1,'base'); addBody(robot,body2,'body1');
ロボット全体の重心位置を計算します。位置をロボットにプロットします。ビューを xy 平面に移動します。
comPos = centerOfMass(robot); show(robot); hold on plot(comPos(1),comPos(2),'or') view(2)
2 番目のボディの質量を変更します。重心の変化に注目してください。
body2.Mass = 20; replaceBody(robot,'body2',body2) comPos2 = centerOfMass(robot); plot(comPos2(1),comPos2(2),'*g') hold off
剛体ツリー モデルにかかる外力に起因する順ダイナミクスの計算
与えられたロボット コンフィギュレーションに外力と重力がかかった結果のジョイント加速度を計算します。ロボット全体に対して重力が指定されている状態で、レンチが特定のボディに適用されています。
Robotics System Toolbox™ loadrobotから KUKA iiwa 14 ロボット モデルを読み込みます。ロボットは、rigidBodyTree オブジェクトとして指定されています。
データ形式を "row" に設定します。すべてのダイナミクス計算について、データ形式は "row" または "column" でなければなりません。
重力を設定します。既定では、重力はゼロと仮定されます。
kukaIIWA14 = loadrobot("kukaIiwa14",DataFormat="row",Gravity=[0 0 -9.81]);
kukaIIWA14 ロボットのホーム コンフィギュレーションを取得します。
q = homeConfiguration(kukaIIWA14);
ロボットにかかる外力を表すレンチ ベクトルを指定します。関数 externalForce を使用して外力行列を生成します。ロボット モデル、レンチがかかるエンドエフェクタ、レンチ ベクトル、および現在のロボット コンフィギュレーションを指定します。wrench は "iiwa_link_ee_kuka" ボディ座標系を基準に指定されるため、ロボット コンフィギュレーション q を指定する必要があります。
wrench = [0 0 0.5 0 0 0.3];
fext = externalForce(kukaIIWA14,"iiwa_link_ee_kuka",wrench,q);kukaIIWA14 がホーム コンフィギュレーションにあるときにエンドエフェクタ "iiwa_link_ee_kuka" に外力を適用した状態で、重力による結果のジョイント加速度を計算します。ジョイント速度とジョイント トルクはゼロと仮定されます (空のベクトル [] として入力)。
qddot = forwardDynamics(kukaIIWA14,q,[],[],fext)
qddot = 1×7
-0.0023 -0.0112 0.0036 -0.0212 0.0067 -0.0075 499.9920
静的ジョイント コンフィギュレーションからの逆ダイナミクスの計算
関数 inverseDynamics を使用して、特定のロボット コンフィギュレーションを静的に保持するために必要なジョイント トルクを計算します。また、その他の構文を使用して、ジョイント速度、ジョイント加速度、および外力を指定できます。
Robotics System Toolbox™ loadrobotから Omron eCobra-600 を読み込みます。rigidBodyTree オブジェクトとして指定されます。重力のプロパティを設定し、データ形式が "row" に設定されていることを確認します。すべてのダイナミクス計算について、データ形式は "row" または "column" でなければなりません。
robot = loadrobot("omronEcobra600", DataFormat="row", Gravity=[0 0 -9.81]);
robot のランダムなコンフィギュレーションを生成します。
q = randomConfiguration(robot);
robot が静的にそのコンフィギュレーションを保持するために必要なジョイント トルクを計算します。
tau = inverseDynamics(robot,q)
tau = 1×4
0.0000 0.0000 -19.6200 0
外力に対抗するジョイント トルクの計算
関数 externalForce を使用して、剛体ツリー モデルに適用する力行列を生成します。力行列は、ロボットのジョイントごとに 6 要素のレンチを適用するための 1 つの行がある、m 行 6 列のベクトルです。関数 externalForce を使用し、エンドエフェクタを指定して、レンチを行列の正しい行に適切に割り当てます。複数の力行列を一緒に追加して、複数の力を 1 つのロボットに適用できます。
これらの外力に対抗するジョイント トルクを計算するには、関数 inverseDynamics を使用します。
Robotics System Toolbox™ loadrobotから Universal Robots UR5e を読み込みます。rigidBodyTree オブジェクトとして指定されます。重力を更新し、データ形式を "row" に設定します。すべてのダイナミクス計算について、データ形式は "row" または "column" でなければなりません。
manipulator = loadrobot("universalUR5e", DataFormat="row", Gravity=[0 0 -9.81]); showdetails(manipulator)
-------------------- Robot: (10 bodies) Idx Body Name Joint Name Joint Type Parent Name(Idx) Children Name(s) --- --------- ---------- ---------- ---------------- ---------------- 1 base base_link-base_fixed_joint fixed base_link(0) 2 base_link_inertia base_link-base_link_inertia fixed base_link(0) shoulder_link(3) 3 shoulder_link shoulder_pan_joint revolute base_link_inertia(2) upper_arm_link(4) 4 upper_arm_link shoulder_lift_joint revolute shoulder_link(3) forearm_link(5) 5 forearm_link elbow_joint revolute upper_arm_link(4) wrist_1_link(6) 6 wrist_1_link wrist_1_joint revolute forearm_link(5) wrist_2_link(7) 7 wrist_2_link wrist_2_joint revolute wrist_1_link(6) wrist_3_link(8) 8 wrist_3_link wrist_3_joint revolute wrist_2_link(7) flange(9) 9 flange wrist_3-flange fixed wrist_3_link(8) tool0(10) 10 tool0 flange-tool0 fixed flange(9) --------------------
manipulator のホーム コンフィギュレーションを取得します。
q = homeConfiguration(manipulator);
shoulder_link に外力を設定します。入力レンチ ベクトルは、base 座標系で表現されます。
fext1 = externalForce(manipulator,"shoulder_link",[0 0 0.0 0.1 0 0]);エンドエフェクタ tool0 に外力を設定します。入力レンチ ベクトルは、tool0 座標系で表現されます。
fext2 = externalForce(manipulator,"tool0",[0 0 0.0 0.1 0 0],q);外力と釣り合うために必要なジョイント トルクを計算します。力を合わせるには、力行列を足し合わせます。ジョイント速度とジョイント加速度はゼロと仮定されます ([] として入力)。
tau = inverseDynamics(manipulator,q,[],[],fext1+fext2)
tau = 1×6
-0.0233 -52.4189 -14.4896 -0.0100 0.0100 -0.0000
ビジュアル ジオメトリを使用したロボット モデルの表示
ロボットのビジュアル ジオメトリを記述するために、Unified Robot Description format (URDF) ファイルと関連付けられた .stl ファイルをもつロボットをインポートできます。各剛体には、個別のビジュアル ジオメトリが指定されています。関数 importrobot は、URDF ファイルを解析してロボット モデルとビジュアル ジオメトリを取得します。この関数は、ロボットのビジュアル ジオメトリと衝突ジオメトリが同じであると想定し、対応するボディの衝突ジオメトリとしてビジュアル ジオメトリを割り当てます。
関数 show を使用して、Figure 内でロボット モデルのビジュアル ジオメトリと衝突ジオメトリを表示します。その後、コンポーネントをクリックして検査したり、右クリックして表示状態を切り替えたりして、モデルを操作できます。
ロボット モデルを URDF ファイルとしてインポートします。.stl ファイルの場所が、この URDF 内で正しく指定されていなければなりません。他の .stl ファイルを個別の剛体に追加する方法の詳細については、addVisualを参照してください。
robot = importrobot('iiwa14.urdf');関連付けられたビジュアル モデルを使用してロボットを可視化します。ボディまたは座標系を検査するには、それらをクリックします。各ビジュアル ジオメトリの表示状態を切り替えるには、ボディを右クリックします。
show(robot,Visuals="on",Collisions="off");
関連付けられた衝突ジオメトリを使用してロボットを可視化します。ボディまたは座標系を検査するには、それらをクリックします。各衝突ジオメトリの表示状態を切り替えるには、ボディを右クリックします。
show(robot,Visuals="off",Collisions="on");
詳細
ダイナミクス プロパティ
ロボット ダイナミクスを操作するときは、rigidBody オブジェクトの以下のプロパティを使用してマニピュレーター ロボットの個々のボディの情報を指定します。
Mass— キログラム単位の剛体の質量。CenterOfMass— 剛体の重心位置。[x y z]の形式のベクトルとして指定します。このベクトルは、ボディ座標系を基準とした剛体の重心位置をメートル単位で記述します。オブジェクト関数centerOfMassは、ロボットの重心を計算する際にこれらの剛体プロパティ値を使用します。Inertia— 剛体の慣性。[Ixx Iyy Izz Iyz Ixz Ixy]の形式のベクトルとして指定します。ベクトルは、キログラム平方メートル単位でボディ座標系を基準としています。慣性テンソルは、次の形式の正定値行列です。
Inertiaベクトルの最初の 3 要素は、慣性テンソルの対角要素である慣性モーメントです。最後の 3 要素は、慣性テンソルの非対角要素である慣性乗積です。
マニピュレーター ロボット モデル全体に関連する情報については、以下の rigidBodyTree オブジェクト プロパティを指定します。
Gravity— ロボットにかかる重力加速度。[x y z]ベクトルとして指定します (m/s2)。既定では重力加速度はありません。DataFormat— 運動学およびダイナミクス関数の入出力データ形式。"struct"、"row"、または"column"として指定します。
参照
[1] Craig, John J. Introduction to Robotics: Mechanics and Control. Reading, MA: Addison-Wesley, 1989.
[2] Siciliano, Bruno, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo. Robotics: Modelling, Planning and Control. London: Springer, 2009.
拡張機能
バージョン履歴
R2016b で導入
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)