最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

ジョイント トルク コマンドによる LBR マニピュレーター動作の制御

目的とするジョイント コンフィギュレーションの一連の中間点とトルク制御マニピュレーターがあるとして、この例では関数 inverseDynamics を使用して、計算トルク コントローラーを実装する方法を説明します。このコントローラーにより、ロボットは滑らかな軌跡に沿って、指定されたコンフィギュレーションの中間点を追従できるようになります。

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

Gazebo シミュレーターで LBR ロボットを発生させます。Gazebo およびシミュレートされた TurtleBot の入門の手順に従って、Ubuntu® 仮想マシンのデスクトップから Gazebo LBR Simulator を起動します。

Gazebo LBR Simulator は、既定の位置、速度、または安全コントローラーをもたないベアボーンの KUKA Light Weight Robot (LBR) マニピュレーターを提供します。ロボットを動かす唯一の手段はジョイント トルクです。シミュレーションが実行を開始すると、ジョイント トルクの入力がないため、LBR のアームは地面に落ちます。

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

ホスト コンピューター上の MATLAB インスタンスで、以下のコマンドを実行して MATLAB 内の ROS グローバル ノードを初期化し、IP アドレスを使って仮想マシン (Gazebo を実行中) 内の ROS マスターに接続します。ipaddress を仮想マシンの IP アドレスに置き換えます。

ipaddress = '192.168.203.129';
rosinit(ipaddress);
Initializing global node /matlab_global_node_46761 with NodeURI http://192.168.203.1:49420/

URDF からの LBR の RigidBodyTree オブジェクトの作成

lbr = importrobot('iiwa14.urdf');
lbr.DataFormat = 'row';

Gazebo と同じになるように重力を設定します。

lbr.Gravity = [0 0 -9.80];

ホーム コンフィギュレーションを MATLAB Figure に表示します。

show(lbr);
view([150 12]);
axis([-0.6 0.6 -0.6 0.6 0 1.35]);
camva(9);
daspect([1 1 1]);

目的とする動作のジョイント トルクによる軌跡の事前計算

ジョイント コンフィギュレーションの中間点を読み込みます。これにより、目的とするロボット動作の主要な座標系が得られます。

load lbr_waypoints.mat

cdt は計画された制御のステップサイズです。これを使用して、軌跡を評価する必要のある一連の時点を入力し、ベクトル tt に保存します。

cdt = 0.001; 
tt = 0:cdt:5;

各ジョイントについて、目的とする動作の軌跡を生成します。exampleHelperJointTrajectoryGeneration は、指定された時間とジョイント コンフィギュレーションの中間点からジョイントの軌跡を生成します。

中間点が違反しない限りは内挿されたジョイント位置がジョイント制限に違反しないように、軌跡は pchip を使用して生成されます。

[qDesired, qdotDesired, qddotDesired, tt] = exampleHelperJointTrajectoryGeneration(tWaypoints, qWaypoints, tt);

inverseDynamics を使用して、理論上で目的の動作を実現するフィードフォワード トルクを事前計算します (外乱がなく、どのような誤差もないと仮定)。次の for ループの実行には時間がかかります。高速化するには、inverseDynamics の生成コードの使用を検討してください。実行方法の詳細については、前節を参照してください。

n = size(qDesired,1);
tauFeedForward = zeros(n,7);
for i = 1:n
    tauFeedForward(i,:) = inverseDynamics(lbr, qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:));
end

カスタマイズされたトピックを使った Gazebo との通信チャネルの確立

Gazebo は /gazebo/get_joint_properties/gazebo/apply_joint_effort の 2 つの ROS サービスを提供しており、ジョイントの状態の取得とジョイント トルクの設定に使用できます。ただし、トルク制御ループを閉じるには、これらのサービスは低速すぎます。このため、カスタマイズされた Gazebo プラグインを使用します。これにより、標準の ROS トピック (パブリッシャーおよびサブスクライバー) を使って、Gazebo におけるジョイントの状態やトルクをはるかに高速で読み取り、書き込むことができます。カスタマイズされた Gazebo プラグインは既に Gazebo LBR Simulator と共に準備されています。

[jointTauPub, jtMsg] = rospublisher('/iiwa_matlab_plugin/iiwa_matlab_joint_effort');
jointStateSub = rossubscriber('/iiwa_matlab_plugin/iiwa_matlab_joint_state');

Gazebo で LBR をホーム コンフィギュレーションにリセット

Gazebo 提供のサービスを使用して、ロボットをホーム コンフィギュレーションにリセットします。MATLAB で ROS サービスを扱う方法の詳細については、ROS サービスの呼び出しと提供を参照してください。

mdlConfigClient = rossvcclient('gazebo/set_model_configuration');

必要なサービス メッセージを作成します。これには、Gazebo に送信するジョイント名および対応するジョイント位置が含まれます。このメッセージを使用してサービスを呼び出します。

msg = rosmessage(mdlConfigClient);
msg.ModelName = 'mw_iiwa';
msg.UrdfParamName = 'robot_description';
msg.JointNames = {'mw_iiwa_joint_1', 'mw_iiwa_joint_2', 'mw_iiwa_joint_3',...
                  'mw_iiwa_joint_4', 'mw_iiwa_joint_5', 'mw_iiwa_joint_6', 'mw_iiwa_joint_7'};
msg.JointPositions = homeConfiguration(lbr);

call(mdlConfigClient, msg)
ans = 
  ROS SetModelConfigurationResponse message with properties:

      MessageType: 'gazebo_msgs/SetModelConfigurationResponse'
          Success: 1
    StatusMessage: 'SetModelConfiguration: success'

  Use showdetails to show the contents of the message

計算トルク制御

PD ゲインを指定します。

weights = [0.3,0.8,0.6, 0.6,0.3,0.2,0.1];
Kp = 100*weights;
Kd = 2* weights;

once = 1;

データ ログの準備をします。

feedForwardTorque = zeros(n, 7);
pdTorque = zeros(n, 7);
timePoints = zeros(n,1);
Q = zeros(n,7);
QDesired = zeros(n,7);

計算トルク制御は、以下の for ループで実装されます。MATLAB が新規のジョイント状態を Gazebo から受信すると、事前生成された tauFeedForward がただちに検索されて、タイム スタンプに対応するフィードフォワード トルクが求められます。また、ジョイントの位置と速度の誤差を補正するための PD トルクも計算されます [1]。

Gazebo の既定の設定では、/iiwa_matlab_plugin/iiwa_matlab_joint_state トピックは通常のリアル タイム係数 0.6 を使用して約 1 kHz (Gazebo のシミュレーション時間) で更新されます。そして、以下のトルク制御ループは通常、約 200 Hz (Gazebo のシミュレーション時間) で実行できます。

for i = 1:n
    % Get joint state from Gazebo.
    jsMsg = receive(jointStateSub);
    data = jsMsg.Data;
    
    % Parse the received message. 
    % The Data in jsMsg is a 1-by-15 vector.
    % 1:7  - joint positions
    % 8:14 - joint velocities
    % 15   - time (Gazebo sim time) when the joint state is updated
    q = double(data(1:7))';
    qdot = double(data(8:14))';
    t = double(data(15));
    
    % Set the start time.
    if once
        tStart = t;
        once = 0;
    end
    
    % Find the corresponding index h in tauFeedForward vector for joint 
    % state time stamp t.
    h = ceil((t - tStart + 1e-8)/cdt);
    if h>n
        break
    end
    
    % Log joint positions data.
    Q(i,:) = q';
    QDesired(i,:) = qDesired(h,:);
    
    % Inquire feed-forward torque at the time when the joint state is
    % updated (Gazebo sim time).
    tau1 = tauFeedForward(h,:);
    % Log feed-forward torque.
    feedForwardTorque(i,:) = tau1;
    
    % Compute PD compensation torque based on joint position and velocity
    % errors.
    tau2 = Kp.*(qDesired(h,:) - q) + Kd.*(qdotDesired(h,:) - qdot);
    % Log PD torque.
    pdTorque(i,:) = tau2';
    
    % Combine the two torques.
    tau = tau1 + tau2;
    
    % Log the time.
    timePoints(i) = t-tStart;
    
    % Send torque to Gazebo.
    jtMsg.Data = tau;
    send(jointTauPub,jtMsg);    
end

ジョイント トルクが送信された LBR ロボットは、軌跡に追従します。次の図は、軌跡全体でロボットの複数のスナップショットを重ね合わせたものです。

結果の検査

実際のジョイントのトルクと位置を目的の値と並べてプロットし、検査します。フィードフォワード トルクがあると、PD トルクが 0 付近で振動する点に注意してください。

exampleHelperLBRPlot(i-1, timePoints, feedForwardTorque, pdTorque, Q, QDesired )

ROS ネットワークをシャットダウンします。

ロボットから切断して ROS ネットワークをシャットダウンします。

rosshutdown
Shutting down global node /matlab_global_node_46761 with NodeURI http://192.168.203.1:49420/

逆ダイナミクス向けのコード生成

ループ内のトルク計算を高速化するには、関数 inverseDynamics のコードを生成します。

invDyn という関数を作成します。exampleHelperMwIiwa14 は codegen 対応の関数であり、importrobot('iiwa14.urdf')robotics.RigidBodyTree により返されるものと同じ オブジェクトを再作成します。

    function tau = invDyn( q, qdot, qddot )
        %#codegen
        persistent robot 
        if isempty(robot) 
            robot = exampleHelperMwIiwa14; 
        end 
        tau = robot.inverseDynamics(q, qdot, qddot);  
    end 

続いて、次の codegen コマンドを使用します。

    codegen invDyn.m -args {zeros(1,7), zeros(1,7), zeros(1,7)}

最後に、生成された invDyn_mex ファイルを使って、for ループ内の inverseDynamics の呼び出し

    tauFeedForward(i,:) = inverseDynamics(lbr, qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:));

を、次に置き換えることができます。

    tauFeedForward(i,:) = invDyn_mex(qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:));

参考

参考文献

[1] B. Sicilano, L. Sciavicco, L. Villani, G. Oriolo, "Robotics: Modelling, Planning and Control", Springer, 2009