Robotics System Toolbox學習筆記（三）：jointSpaceMotionModel、taskSpaceMotionModel

文章目錄

taskSpaceMotionModel

例子

本文軟件版本

Matlab：R2019b

Robotics System Toolbox

jointSpaceMotionModel

給定關節空間的輸入，對剛體樹模型的運動進行建模。

% 創建默認的二自由度操作臂的運動模型
motionModel = jointSpaceMotionModel

% 創建指定rigidBodyTree對象的運動模型
motionModel = jointSpaceMotionModel("RididBodyTree", tree)

% 設置指定爲名稱-值對的其他屬性,可以按任何順序指定多個屬性
motionModel = jointSpaceMotionModel(Name, Value)

性質：

RigidBodyTree	rigidBodyTree對象	定義了機器人的慣性及運動學屬性
NaturalFrequency	動態誤差的固有頻率	n維向量，n爲機器人中非固定的關節數量。默認爲[10 10]
DampingRatio	動態誤差的阻尼係數	n維向量，n爲機器人中非固定的關節數量。默認爲[1 1]
Kp	PD控制的比例增益	標量或者 $n\times n$ 矩陣，其中n爲機器人中非固定的關節數量。默認爲 $100 \times eye(2)$ 。如果指定了一個標量，則Kp爲 $s \times eye(n)$
Kd	PD控制的微分增益	標量或者 $n\times n$ 矩陣，其中n爲機器人中非固定的關節數量。默認爲 $100 \times eye(2)$ 。如果指定了一個標量，則Kd爲 $s \times eye(n)$
MotionType	運動模型計算的運動類型	- "ComputedTorqueControl"補償全身動力學並分配在NaturalFrequency和DampingRatio屬性中指定的誤差動力學，這是默認屬性 - "IndependentJointMotion"使用NaturalFrequency和DampingRatio屬性指定的動態誤差將每個關節建模爲獨立的二階系統。 -"PDControl"使用指定的Kp和Kd值在每個關節上進行比例微分控制

derivative

機械手模型狀態的時間導數。

% taskMotionModel---taskSpaceMotionModel類的對象，定義了運動模型的性質

% refPose---4*4齊次變換矩陣，表示末端執行器在任務空間中的參考位姿，以米爲單位

% refVel---6*1向量，表示末端執行器在任務空間中的參考速度，指定爲[omega v],omega表示圍繞x，y和z軸的三個角速度的行向量，單位爲rad/s，v表示沿x，y和z軸的三個線速度的行向量，以米每秒爲單位

% fExt---外力，指定爲m元素向量，其中m是關聯的rigidBodyTree對象中的剛體數量

% state---關節位置和速度，2n元素向量，指定爲[q; qDot]，n是motionModel關聯的rigidbodyBodyTree對象中的關節數，q表示每個關節的位置，以弧度表示。qDot表示每個關節的速度，以rad/s爲單位

% cmds---指示所需運動的控制命令，n*2矩陣或者n*3矩陣
% cmds的大小取決於運動模型的MotionType屬性。對於"PDControl"，cmds爲n*2矩陣，即[qRef; qRefDot]，分別表示關節位置和速度；對於"ComputedTorqueControl"和"IndependentJointMotion"，cmds是一個n*3矩陣，[qRef; qRefDot; qRefDDot]，分別表示關節位置、速度、加速度

% 使用任務空間模型基於當前狀態和運動命令來計算運動模型的時間導數
stateDot = derivative(taskMotionModel,state,refPose,refVel)
stateDot = derivative(taskMotionModel,state,refPose, refVel,fExt)
stateDot = derivative(jointMotionModel,state,cmds)
stateDot = derivative(jointMotionModel,state,cmds,fExt)

% stateDot---基於當前狀態和指定控制命令的時間導數，返回的是[qDot; qDDot]

updateErrorDynamicsFromStep

給定所需的階躍響應，更新NaturalFrequency和DampingRatio屬性的值。

% motionModel---jointSpaceMotionModel對象
% settlingTime---過渡時間Ts，直到被控參數進入新的穩態值2%公差帶(s)所需的時間，指定爲標量或n元素向量。n爲機器人活動關節
% overshoot---階躍系統超調，指定爲標量或n元素向量，n爲機器人活動關節
% jointIndex---在給定單位步長動態誤差的情況下，更新了NaturalFrequency和DampingRatio的關節標號。在這種情況下，必須將過渡時間和超調量指定爲標量

% 給定所需的階躍響應，更新給定的jointSpaceMotionModel對象motionModel的NaturalFrequency和DampingRatio屬性的值。
updateErrorDynamicsFromStep(motionModel,settlingTime,overshoot)

% 更新特定關節的NaturalFrequency和DampingRatio屬性。在這種情況下，必須將SettlingTime和Overshoot的值作爲標量提供，因爲它們適用於單個關節
updateErrorDynamicsFromStep(motionModel,settlingTime,overshoot,jointIndex)

例子

% 創建機器人
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

% 設置仿真時間0到1s，步長爲0.01s
% 設置機器人的初始狀態：home構型+零初始速度
tspan = 0:0.01:1;
initialState = [homeConfiguration(robot); zeros(7,1)];

% 定義目標狀態的位置、速度和加速度。速度和加速度均爲0
targetState = [pi/4; pi/3; pi/2; -pi/3; pi/4; -pi/4; 3*pi/4; zeros(7,1); zeros(7,1)];

% 創建運動模型Motion Model
% 通過ComputedTorqueControl對系統robot進行建模，動態誤差由具有5%超調的適度快速階躍響應定義
motionModel = jointSpaceMotionModel("RigidBodyTree",robot);
updateErrorDynamicsFromStep(motionModel,.3,.05);

% 使用模型的derivative函數作爲ode45求解器的輸入，以此來模擬1s內的行爲
[t,robotState] = ode45(@(t,state)derivative(motionModel,state,targetState),tspan,initialState);

% 畫出響應
% 繪製所有到達其目標狀態的關節位置。
% 起始位置和目標位置之間位移較大的關節比位移較小的關節以更快的速度到達目標。
% 這會導致出現超調，但是所有關節的調節時間都相同
figure
plot(t,robotState(:,1:motionModel.NumJoints));
hold all;

plot(t,targetState(1:motionModel.NumJoints)*ones(1,length(t)),"--");
title("Joint Position (Solid) vs Reference (Dashed)");
xlabel("Time (s)")
ylabel("Position (rad)");

輸出爲：

taskSpaceMotionModel

給定任務空間的參考輸入，對剛體樹模型的運動進行建模。

% 與上文中寫道的jointSpaceMotionModel對應的函數類似，只是這裏都是在任務空間下進行的
motionModel = taskSpaceMotionModel
motionModel = taskSpaceMotionModel("RigidBodyTree",tree)
motionModel = taskSpaceMotionControlModel(Name,Value)

性質與jointSpaceMotionModel也類似，不同點在於加了末端執行器及關節阻尼係數特性：

RigidBodyTree	rigidBodyTree對象	定義了機器人的慣性及運動學屬性
EndEffectorName	末端執行器名稱	默認爲’tool’，將其定義在任務空間中，該屬性必須對應於RigidBodyTree對象中body的主體名稱。如果在不更新末端執行器的情況下更新剛體樹，則默認情況下標號也就是index最大的body將成爲末端執行器
JointDamping	每個關節的動態阻尼係數	n維向量，n爲機器人中非固定的關節數量。默認爲[1 1]
Kp	PD控制的比例增益	$6\times 6$ 矩陣，默認爲 $500 \times eye(6)$
Kd	PD控制的微分增益	$6\times 6$ 矩陣，默認爲 $100 \times eye(6)$
MotionType	運動模型計算的運動類型	指定爲“ PDControl”，通過雅可比轉置控制器映射到關節的比例微分（PD）控制。該控件基於指定的Kp和Kd屬性

同樣具有derivative和updateErrorDynamicsFromStep函數，具體見上文。

例子

% 創建任務空間運動模型
% 本示例說明如何爲任務空間中的機械手機械臂創建和使用taskSpaceMotionModel對象

% 創建機器人模型
robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81])

% 設置仿真參數
% 設置仿真時間0到1s，步長爲0.02s
% 設置機器人的初始狀態：home構型+零初始速度
tspan = 0:0.02:1;
initialState = [homeConfiguration(robot);zeros(7,1)];

% 定義一個參考目標狀態，包括目標位置和零初始速度
refPose = trvec2tform([0.6 -.1 0.5]); % 此時旋轉矩陣爲I，位置向量變爲[0.6 -0.1 0.5]
refVel = zeros(6,1);

% 創建運動模型
% 在比例微分（PD）控制下進行系統建模
% 指定robot剛體樹模型，指定末端執行器模型
motionModel = taskSpaceMotionModel("RigidBodyTree",robot,"EndEffectorName","EndEffector_Link");

% 開始仿真
% 使用剛性求解器在1秒鐘內進行仿真，以更有效地捕獲機器人動力學
% 使用ode15可以在變化率較高的區域周圍實現更高的精度
[t,robotState] = ode15s(@(t,state)derivative(motionModel,state,refPose,refVel),tspan,initialState);

% 畫出響應
% 畫出機器人的位置，並將目標位置用符號"X"標記出來
figure
show(robot,initialState(1:7));
hold all
plot3(refPose(1,4),refPose(2,4),refPose(3,4),"x","MarkerSize",20)

% 通過將機器人繪製成5Hz循環來觀察響應
r = rateControl(5);
for i = 1:size(robotState,1)
    show(robot,robotState(i,1:7)',"PreservePlot",false);
    
    % 畫出末端軌跡
    poseNow = getTransform(robot, robotState(i, 1: 7)', "EndEffector_Link");
    plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20)
    waitfor(r);
end

show

這個函數前面已經用到過很多次了，在figure中顯示機器人模型。

% 使用預定義的home configuration在圖中繪製機器人模型的body frame。框架和視覺效果都將自動顯示
show(robot)

% 指定機器人robot的構型
show(robot,configuration)

% 提供由一個或多個“名稱/值”對參數指定的其他選項
show(___,Name,Value)

% 返回繪製機器人的座標軸
ax = show(___)

此處的名Name-Value參數對爲：

‘Parent’——軸的父級，指定爲以逗號分隔的一對，由父級和在其中繪製機器人的軸對象組成。默認情況下，將在活動軸上繪製機器人。
‘PreservePlot’——保留機械手繪圖的選項，指定爲逗號分隔的對，由“ PreservePlot”和true或false組成，其值默認爲true。如果將此屬性設置爲true，則不會覆蓋通過調用show顯示的先前圖。此設置的功能類似於爲標準MATLAB圖形調用hold on，但僅限於機器人主體框架。當此屬性設置爲false時，機器人的先前圖將被覆蓋。
‘Frame’——剛體座標軸展示，指定爲"on"或者"off"，默認爲"on"，展示機器人模型樹上各個剛體的座標系。
‘Visual’——可視化參數設置，指定爲"on"或者"off"，默認爲"on"。

例子：使用KINOVA Gen3機械手完成任務空間和關節空間的軌跡

% 加載KINOVA Gen3機器人模型樹
% 定義重力加速度爲[0 0 -9.81]
robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

% 得到目前robot的關節構型，home構型
currentRobotJConfig = homeConfiguration(robot);

% 獲取關節數和末端執行器RBT框架
numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

% 指定軌跡時間步長以及期望的末端軌跡速度
timeStep = 0.1; % 秒
toolSpeed = 0.1; % m/s

% 設置末端執行器的初始位姿與最終位姿
jointInit = currentRobotJConfig;
taskInit = getTransform(robot, jointInit, endEffector);
% axang2tform角軸旋轉
% 以軸角形式給出的旋轉，指定爲n個軸角旋轉的n×4矩陣。
% 每行的前三個元素指定旋轉軸，最後一個元素定義旋轉角度（以弧度爲單位）
taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

% 執行任務空間軌跡
% 通過插值計算任務空間軌跡的waypoints
% 首先，計算末端執行器從初始位姿到最終位姿的距離
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對象在任務空間PD控制下對剛體樹模型的運動進行建模
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

% 將orientation方向(向量的前三個元素)上的比例和微分增益設置爲零，以便受控行爲僅遵循參考位置
tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

% 定義初始狀態，即關節的位置以及速度
q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

% 開始仿真
% 使用ode15s模擬機器人運動
% 由於參考狀態在每個時刻都發生變化，因此需要封裝函數將輸入的插值軌跡更新爲每個時刻的狀態導數
% 這裏的exampleHelperTimeBasedTaskInputs函數是內部函數，該函數將時變輸入傳遞給taskSpaceMotionModel對象的derivative函數作爲輸入
% stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)
% 在未來可能被移除，是下面兩個函數的封裝
% [refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);
% stateDot = derivative(motionModel, state, refPose, refVel);
% 因此，示例輔助函數將作爲函數句柄傳遞給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);

% 使用三次多項式函數在它們之間進行插值以生成均勻間隔的關節角度數組
% 使用B樣條曲線生成平滑軌跡
ctrlpoints = [jointInit',jointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

% 控制關節空間軌跡
% 爲關節上的PD控制創建關節空間運動模型
% jointSpaceMotionModel對象爲剛體樹模型的運動建模，並在指定的關節位置上使用PD控制
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

% 定義初始狀態，即關節的位置以及速度
q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

% 使用ode15s模擬機器人運動
% 同樣，示例輔助函數用作ODE求解器的函數句柄輸入，以便在每個時間點更新參考輸入
% stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)
% 是下面兩個函數的封裝體，該函數將時變輸入傳遞給jointSpaceMotionModel對象的derivative函數作爲輸入
% [qd, qdDot] = bsplinepolytraj(configWaypoints,  timeInterval , t);
% stateDot = derivative(motionModel, state, [qd; qdDot]);
% 關節空間狀態在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

% 可視化關節空間的軌跡。遍歷stateJoint狀態並根據當前時間進行插值
% 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

function stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)
    % Use a B-spline curve to ensure the trajectory is smooth and moves
    % through the waypoints with non-zero velocity
    [qd, qdDot] = bsplinepolytraj(configWaypoints,  timeInterval , t);
    
    % Compute state derivative
    stateDot = derivative(motionModel, state, [qd; qdDot]);
end

function stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)

[refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);

stateDot = derivative(motionModel, state, refPose, refVel);

end

來源

Robot Models — Functions

Plan and Execute Task- and Joint-space Trajectories using KINOVA Gen3 Manipulator

Robotics System Toolbox學習筆記（三）：jointSpaceMotionModel、taskSpaceMotionModel

文章目錄

本文軟件版本

jointSpaceMotionModel

derivative

updateErrorDynamicsFromStep

例子

taskSpaceMotionModel

例子

show

例子：使用KINOVA Gen3機械手完成任務空間和關節空間的軌跡

來源

985 碩士程序員，空窗 4 個月沒有 Offer！

一文搞懂 Spring 循環依賴

賽博鬥地主——使用大語言模型扮演Agent智能體玩牌類遊戲。

VScode右鍵打開(添加到右鍵)

自寫的C語言矩陣簡易運算庫

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與拋物線擬合的線性函數）、S型曲線規劃

在Qt Creator上使用eigen庫時無法ctrl到某些成員函數的定義

基於拋物線過渡（梯形加減速）的空間直線插補算法與空間圓弧插補算法（Matlab）

五自由度機械臂正逆運動學算法（Ｃ語言+Matlab）

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結