42 lines
1.8 KiB
Mathematica
42 lines
1.8 KiB
Mathematica
|
function JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist)
|
||
|
|
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
|
||
|
|
% Takes thetalist: A list of joint variables,
|
||
|
|
% 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 screw axes as the columns,
|
||
|
|
% Returns JTFtip: The joint forces and torques required only to create the
|
||
|
|
% end-effector force Ftip.
|
||
|
|
% This function calls InverseDynamics with g = 0, dthetalist = 0, and
|
||
|
|
% ddthetalist = 0.
|
||
|
|
% Example Input (3 Link Robot):
|
||
|
|
%
|
||
|
|
% clear; clc;
|
||
|
|
% thetalist = [0.1; 0.1; 0.1];
|
||
|
|
% 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]];
|
||
|
|
% JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist)
|
||
|
|
%
|
||
|
|
% Output:
|
||
|
|
% JTFtip =
|
||
|
|
% 1.4095
|
||
|
|
% 1.8577
|
||
|
|
% 1.3924
|
||
|
|
|
||
|
|
n = size(thetalist, 1);
|
||
|
|
JTFtip = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1), ...
|
||
|
|
[0; 0; 0], Ftip, Mlist, Glist, Slist);
|
||
|
|
end
|