KINOVA Gen3 マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行

この例では、初期姿勢から目的のエンドエフェクタ姿勢に移動するために、内挿されたジョイント軌跡を生成してシミュレートする方法を説明します。軌跡のタイミングは、アームツール端 (EOAT) の望ましいおよその速度に基づきます。

KINOVA Gen3 剛体ツリー (RBT) ロボットモデルを読み込みます。

robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

現在のロボットのジョイントコンフィギュレーションを設定します。

currentRobotJConfig = homeConfiguration(robot);

ジョイントの数とエンドエフェクタ RBT の座標系を取得します。

numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

軌跡のタイムステップと、目的のツールのおよその速度を指定します。

timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s

エンドエフェクタの初期姿勢と最終姿勢を設定します。

jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);

taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

タスク空間の軌跡の生成

タスク空間の軌跡のウェイポイントを内挿により計算します。

まず、ツールの移動距離を計算します。

distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

次に、移動距離と目的のツール速度に基づいて軌跡の時間を定義します。

initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];

taskInit と taskFinal の間に内挿して、タスク空間の中継のウェイポイントを計算します。

[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);

タスク空間の運動の制御

ジョイントに PD 制御を適用するために、タスク空間の運動モデルを作成します。taskSpaceMotionModel オブジェクトは、タスク空間の比例-微分制御下にある剛体ツリーモデルの運動をモデル化します。

tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

方向の比例ゲインと微分ゲインを 0 に設定します。これにより、制御される動作は単に基準位置を追従します。

tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

初期状態 (ジョイントの位置と速度) を定義します。

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

ode15s を使用してロボットの運動をシミュレートします。この問題では、閉ループシステムはスティッフです。つまり、問題中のいずれかの部分でスケールに差があります。その結果、積分器は非常に小さなステップを取らなければならなくなることがあり、また ode45 などのノンスティッフ ODE のソルバーで同じ問題を解決する場合は非常に長い時間がかかります。詳細については、ドキュメンテーションのODE ソルバーの選択およびスティッフ ODE の求解を参照してください。

基準状態は各時点で変化するため、各時点で状態導関数への内挿軌跡入力を更新するためにラッパー関数が必要です。このため、例の補助関数が関数ハンドルとして ODE ソルバーに渡されます。結果のマニピュレーター状態は stateTask に出力されます。

[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

ジョイント空間の軌跡の生成

ロボットの逆運動学オブジェクトを作成します。

ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];

逆運動学を使用して、初期および目的のジョイントコンフィギュレーションを計算します。値をパイにラップし、内挿の領域を最小限にします。

initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);

既定では、IK の解はジョイント制限を考慮します。ただし、連続ジョイント (範囲が無限の回転ジョイント) の場合、結果の値は不必要に大きくなる可能性があります。[-pi, pi] にラップすることで、最終的な軌跡の距離を確実に最小限にできます。Gen3 の非連続ジョイントでは既にこの区間内に範囲が設定されているため、ジョイント値を pi にラップするだけで十分です。連続ジョイントは区間 [-pi, pi] にマップされ、他の値は同じままとなります。

wrappedJointFinal = wrapToPi(jointFinal);

3 次多項関数を使用してこれらの間に内挿し、等間隔のジョイントコンフィギュレーションの配列を生成します。B スプラインを使用して滑らかな軌跡を生成します。

ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

ジョイント空間の軌跡の制御

ジョイントに PD 制御を適用するために、ジョイント空間の運動モデルを作成します。jointSpaceMotionModel オブジェクトは、剛体ツリーモデルの運動をモデル化し、指定されたジョイント位置に比例-微分制御を使用します。

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

初期状態 (ジョイントの位置と速度) を設定します。

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

ode15s を使用してロボットの運動をシミュレートします。ここでも、基準入力を各時点で更新するため、例の補助関数を ODE ソルバーへの関数ハンドル入力として使用します。ジョイント空間の状態が stateJoint に出力されます。

[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

ロボットの軌跡の可視化と比較

ロボットの初期コンフィギュレーションを表示します。

show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
hold on
axis([-1 1 -1 1 -0.1 1.5]);

タスク空間の軌跡を可視化します。stateTask 状態を反復し、現在の時間に基づいて内挿します。

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tTask,stateTask(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20);
    drawnow;
end

ジョイント空間の軌跡を可視化します。jointTask 状態を反復し、現在の時間に基づいて内挿します。

% Return to initial configuration
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');

for i=1:length(trajTimes)
    % Current time 
    tNow= trajTimes(i);
    % Interpolate simulated joint positions to get configuration at current time
    configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow);
    poseNow = getTransform(robot,configNow,endEffector);
    show(robot,configNow,'PreservePlot',false,'Frames','off');
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20);
    drawnow;
end

% Add a legend and title
legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'});
title('Manipulator Trajectories')