このページの翻訳は最新ではありません。ここをクリックして,英語の最新版を参照してください。
KINOVA Gen3マニピュレーターを使用したタスク空間およびジョイント空間の軌跡の計画と実行
この例では,初期姿勢から目的のエンドエフェクタ姿勢に移動するために,内挿されたジョイント軌跡を生成してシミュレートする方法を説明します。軌跡のタイミングは,アームツール端(EOAT)の望ましいおよその速度に基づきます。
KINOVA Gen3剛体ツリー(RBT)ロボットモデルを読み込みます。
机器人= loadrobot (“kinovaGen3”,“DataFormat”,“行”,“重力”[0 0 -9.81]);
現在のロボットのジョイントコンフィギュレーションを設定します。
currentRobotJConfig = homeConfiguration(机器人);
ジョイントの数とエンドエフェクタRBTの座標系を取得します。
numJoints =元素个数(currentRobotJConfig);endEffector =“EndEffector_Link”；
軌跡のタイムステップと,目的のツールのおよその速度を指定します。
步伐= 0.1;%秒toolSpeed = 0.1;% m / s
エンドエフェクタの初期姿勢と最終姿勢を設定します。
jointInit = currentRobotJConfig;taskInit = getTransform(机器人,jointInit, endEffector);taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);
タスク空間の軌跡の生成
タスク空間の軌跡の中間点を内挿により計算します。
まず,ツールの移動距離を計算します。
距离=规范(tform2trvec (taskInit) -tform2trvec (taskFinal));
次に,移動距離と目的のツール速度に基づいて軌跡の時間を定義します。
initTime = 0;finalTime =(距离/工具速度)- initTime;trajTimes = initTime:步伐:finalTime;timeInterval = [trajTimes (1);trajTimes(结束)];
taskInitとtaskFinalの間に内挿して,タスク空間の中継の中間点を計算します。
[taskWaypoints, taskVelocities] = transformtraj (taskInit、taskFinal timeInterval, trajTimes);
タスク空間の運動の制御
ジョイントにPD制御を適用するために,ジョイント空間の運動モデルを作成します。taskSpaceMotionModelオブジェクトは,タスク空間の比例——微分制御下にある剛体ツリーモデルの運動をモデル化します。
tsMotionModel = taskSpaceMotionModel (“RigidBodyTree”,机器人,“EndEffectorName”,“EndEffector_Link”）;
方向の比例ゲインと微分ゲインを0に設定します。これにより,制御される動作は単に基準位置を追従します。
tsMotionModel.Kp (1:3, 1:3) = 0;tsMotionModel.Kd (1:3, 1:3) = 0;
初期状態(ジョイントの位置と速度)を定義します。
q0 = currentRobotJConfig;qd0 = 0(大小(q0));
ode15sを使用してロボットの運動をシミュレートします。この問題では,閉ループシステムはスティッフです。つまり,問題中のいずれかの部分でスケールに差があります。その結果,積分器は非常に小さなステップを取らなければならなくなることがあり,また数值などのノンスティッフ颂歌のソルバーで同じ問題を解決する場合は非常に長い時間がかかります。詳細については,ドキュメンテーションのODEソルバーの選択およびスティッフ颂歌の求解を参照してください。
基準状態は各時点で変化するため,各時点で状態導関数への内挿軌跡入力を更新するためにラッパー関数が必要です。このため,例の補助関数が関数ハンドルとして颂歌ソルバーに渡されます。結果のマニピュレーター状態はstateTaskに出力されます。
[task,stateTask] = ode15s(@(t,state) examplehelpertimebasedtaskputs (tsMotionModel,timeInterval,taskInit,taskFinal,t,state)，timeInterval，[q0;qd0]);
ジョイント空間の軌跡の生成
ロボットの逆運動学オブジェクトを作成します。
本土知识= inverseKinematics (“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;Weights = [1 1 1 1 1];
逆運動学を使用して,初期および目的のジョイントコンフィギュレーションを計算します。値をパイにラップし,内挿の領域を最小限にします。
initialGuess = jointInit;本土知识jointFinal = (endEffector taskFinal,重量、initialGuess);
既定では,反向の解はジョイント制限を考慮します。ただし,連続ジョイント(範囲が無限の回転ジョイント)の場合,結果の値は不必要に大きくなる可能性があります。(π-π,)にラップすることで,最終的な軌跡の距離を最小限にできます。Gen3の非連続ジョイントでは既にこの区間内に範囲が設定されているため,ジョイント値をπにラップするだけで十分です。連続ジョイントは区間(π-π,)にマップされ,他の値は同じままとなります。
wrappedJointFinal = wrapToPi (jointFinal);
3次多項関数を使用してこれらの間に内挿し,等間隔のジョイントコンフィギュレーションの配列を生成します。Bスプラインを使用して滑らかな軌跡を生成します。
ctrlpoints = [jointInit, wrappedJointFinal];jointConfigArray = cubicpolytraj (ctrlpoints timeInterval trajTimes);jointWaypoints = bsplinepolytraj (jointConfigArray timeInterval 1);
ジョイント空間の軌跡の制御
ジョイントにPD制御を適用するために,ジョイント空間の運動モデルを作成します。jointSpaceMotionModelオブジェクトは,剛体ツリーモデルの運動をモデル化し,指定されたジョイント位置に比例——微分制御を使用します。
jsMotionModel = jointSpaceMotionModel (“RigidBodyTree”,机器人,“MotionType”,“PDControl”）;
初期状態(ジョイントの位置と速度)を設定します。
q0 = currentRobotJConfig;qd0 = 0(大小(q0));
ode15sを使用してロボットの運動をシミュレートします。ここでも,基準入力を各時点で更新するため,例の補助関数を颂歌ソルバーへの関数ハンドル入力として使用します。ジョイント空間の状態がstateJointに出力されます。
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state)，timeInterval，[q0;qd0]);
ロボットの軌跡の可視化と比較
ロボットの初期コンフィギュレーションを表示します。
表演(currentRobotJConfig的机器人“PreservePlot”假的,“帧”,“关闭”）;持有在轴([-1 1 -1 1 -0.1 1.5]);
タスク空間の軌跡を可視化します。stateTask状態を反復し,現在の時間に基づいて内挿します。
为i = 1:长度(trajTimes)%当前时间tNow = trajTimes(我);插入模拟的关节位置以获得当前的配置configNow = interp1 (tTask stateTask (: 1: numJoints), tNow);poseNow = getTransform(机器人,configNow, endEffector);表演(configNow的机器人“PreservePlot”假的,“帧”,“关闭”）;taskSpaceMarker = plot3 (poseNow(1、4),poseNow(2、4),poseNow(3、4),“b”。,“MarkerSize”, 20);drawnow;结束
ジョイント空間の軌跡を可視化します。jointTask状態を反復し,現在の時間に基づいて内挿します。
%返回初始配置表演(currentRobotJConfig的机器人“PreservePlot”假的,“帧”,“关闭”）;为i = 1:长度(trajTimes)%当前时间tNow = trajTimes(我);插入模拟的关节位置以获得当前的配置configNow = interp1 (tJoint stateJoint (: 1: numJoints), tNow);poseNow = getTransform(机器人,configNow, endEffector);表演(configNow的机器人“PreservePlot”假的,“帧”,“关闭”）;jointSpaceMarker = plot3 (poseNow(1、4),poseNow(2、4),poseNow(3、4),“r”。,“MarkerSize”, 20);drawnow;结束%添加一个图例和标题传奇([taskSpaceMarker jointSpaceMarker) {在工作空间中定义的,在关节空间中定义的}）;标题(“机械手轨迹”）
你也可以从以下列表中选择一个网站:
