77 lines
3.0 KiB
Matlab
77 lines
3.0 KiB
Matlab
function [Vi,Vdi,AdTi,Flist,taulist] = InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
|
|
g, Ftip,Mlist, Glist, Slist)
|
|
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
|
|
% Takes thetalist: n-vector of joint variables,
|
|
% dthetalist: n-vector of joint rates,
|
|
% ddthetalist: n-vector of joint accelerations,
|
|
% g: Gravity vector g,
|
|
% Ftip: Spatial force applied by the end-effector expressed in frame
|
|
% {n+1},
|
|
% Mlist: List of link frames {i} relative to {i-1} at the home
|
|
% position,
|
|
% Glist: Spatial inertia matrices Gi of the links,
|
|
% Slist: Screw axes Si of the joints in a space frame, in the format
|
|
% of a matrix with the screw axes as the columns.
|
|
% Returns taulist: The n-vector of required joint forces/torques.
|
|
% This function uses forward-backward Newton-Euler iterations to solve the
|
|
% equation:
|
|
% taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ...
|
|
% + g(thetalist) + Jtr(thetalist) * Ftip
|
|
% Example Input (3 Link Robot):
|
|
%
|
|
% clear; clc;
|
|
% thetalist = [0.1; 0.1; 0.1];
|
|
% dthetalist = [0.1; 0.2; 0.3];
|
|
% ddthetalist = [2; 1.5; 1];
|
|
% g = [0; 0; -9.8];
|
|
% Ftip = [1; 1; 1; 1; 1; 1];
|
|
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
|
|
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
|
|
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
|
|
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
|
|
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
|
|
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
|
|
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
|
|
% Glist = cat(3, G1, G2, G3);
|
|
% Mlist = cat(3, M01, M12, M23, M34);
|
|
% Slist = [[1; 0; 1; 0; 1; 0], ...
|
|
% [0; 1; 0; -0.089; 0; 0], ...
|
|
% [0; 1; 0; -0.089; 0; 0.425]];
|
|
% taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ...
|
|
% Ftip, Mlist, Glist, Slist)
|
|
%
|
|
% Output:
|
|
% taulist =
|
|
% 74.6962
|
|
% -33.0677
|
|
% -3.2306
|
|
|
|
n = size(thetalist, 1);
|
|
Mi = eye(4);
|
|
Ai = zeros(6, n);
|
|
AdTi = zeros(6, 6, n + 1);
|
|
Vi = zeros(6, n + 1);
|
|
Vdi = zeros(6, n + 1);
|
|
Vdi(4: 6, 1) = -g;
|
|
AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1)));
|
|
Fi = Ftip;
|
|
Flist = zeros(n,6);
|
|
taulist = zeros(n, 1);
|
|
for i=1: n
|
|
Mi = Mi * Mlist(:, :, i);
|
|
Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i);
|
|
AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ...
|
|
* -thetalist(i))) * TransInv(Mlist(:, :, i)));
|
|
Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i);
|
|
Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ...
|
|
+ Ai(:, i) * ddthetalist(i) ...
|
|
+ ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i);
|
|
end
|
|
for i = n: -1: 1
|
|
Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ...
|
|
- ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1));
|
|
% Fi = Glist(:, :, i) * Vdi(:, i + 1) ;
|
|
Flist(i,:) = Fi;
|
|
taulist(i) = Fi' * Ai(:, i);
|
|
end
|
|
end |