Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

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)];

taskInittaskFinal の間に内挿して、タスク空間の中継の中間点を計算します。

[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 を使用してロボットの運動をシミュレートします。基準状態は各時点で変化するため、各時点で状態導関数への内挿軌跡入力を更新するためにラッパー関数が必要です。このため、例の補助関数が関数ハンドルとして 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 = wrapToPi(jointInit);
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
jointFinal = wrapToPi(jointFinal);

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

ctrlpoints = [jointInit',jointFinal'];
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');
    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');
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20)
    drawnow;
end