Feature/add inertia and FK expand
This commit is contained in:
parent
8967270c39
commit
9dfa837e8c
|
|
@ -0,0 +1,8 @@
|
|||
function Vlinear = BodyVelToLinearVel(V,G)
|
||||
% *** CHAPTER x: DYNAMICS OF OPEN CHAINS ***
|
||||
% Takes V: Body frame velocity,
|
||||
% G: Spactial frame G
|
||||
% Returns Glist: Spatial inertia matrices Gi of the links
|
||||
|
||||
[R, p] = TransToRp(G);
|
||||
Vlinear = R*V(4:6);
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
function Tlist = FKinSpaceExpand(Mlist, Slist, thetalist)
|
||||
% *** CHAPTER 4: FORWARD KINEMATICS ***
|
||||
% Takes M: the home configuration (position and orientation) of the
|
||||
% end-effector,
|
||||
% Slist: The joint screw axes in the space frame when the manipulator
|
||||
% is at the home position,
|
||||
% thetalist: A list of joint coordinates.
|
||||
% Returns T in SE(3) representing the end-effector frame, when the joints
|
||||
% are at the specified coordinates (i.t.o Space Frame).
|
||||
% Example Inputs:
|
||||
%
|
||||
% clear; clc;
|
||||
% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
|
||||
% Slist = [[0; 0; 1; 4; 0; 0], ...
|
||||
% [0; 0; 0; 0; 1; 0], ...
|
||||
% [0; 0; -1; -6; 0; -0.1]];
|
||||
% thetalist =[pi / 2; 3; pi];
|
||||
% T = FKinSpace(M, Slist, thetalist)
|
||||
%
|
||||
% Output:
|
||||
% T =
|
||||
% -0.0000 1.0000 0 -5.0000
|
||||
% 1.0000 0.0000 0 4.0000
|
||||
% 0 0 -1.0000 1.6858
|
||||
% 0 0 0 1.0000
|
||||
|
||||
Tlist = zeros(4,4,size(thetalist,1));
|
||||
Mi = eye(4);
|
||||
for i = size(thetalist): -1: 1
|
||||
for j = 1:i
|
||||
Mi = Mi * Mlist(:, :, j);
|
||||
end
|
||||
Tlist(:,:,i) = FKinSpace(Mi, Slist(:,1:i), thetalist(1:i));
|
||||
Mi = eye(4);
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
function Tlist = FKinSpaceExpand_Sym(Mlist, Slist, thetalist)
|
||||
% *** CHAPTER 4: FORWARD KINEMATICS ***
|
||||
% Takes M: the home configuration (position and orientation) of the
|
||||
% end-effector,
|
||||
% Slist: The joint screw axes in the space frame when the manipulator
|
||||
% is at the home position,
|
||||
% thetalist: A list of joint coordinates.
|
||||
% Returns T in SE(3) representing the end-effector frame, when the joints
|
||||
% are at the specified coordinates (i.t.o Space Frame).
|
||||
% Example Inputs:
|
||||
%
|
||||
% clear; clc;
|
||||
% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
|
||||
% Slist = [[0; 0; 1; 4; 0; 0], ...
|
||||
% [0; 0; 0; 0; 1; 0], ...
|
||||
% [0; 0; -1; -6; 0; -0.1]];
|
||||
% thetalist =[pi / 2; 3; pi];
|
||||
% T = FKinSpace(M, Slist, thetalist)
|
||||
%
|
||||
% Output:
|
||||
% T =
|
||||
% -0.0000 1.0000 0 -5.0000
|
||||
% 1.0000 0.0000 0 4.0000
|
||||
% 0 0 -1.0000 1.6858
|
||||
% 0 0 0 1.0000
|
||||
|
||||
Tlist = sym(zeros(4,4,size(thetalist,1)));
|
||||
Mi = sym(eye(4));
|
||||
for i = size(thetalist): -1: 1
|
||||
for j = 1:i
|
||||
Mi = Mi * Mlist(:, :, j);
|
||||
end
|
||||
Tlist(:,:,i) = FKinSpace_Sym(Mi, Slist(:,1:i), thetalist(1:i));
|
||||
Mi = sym(eye(4));
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
function T = FKinSpace(M, Slist, thetalist)
|
||||
% *** CHAPTER 4: FORWARD KINEMATICS ***
|
||||
% Takes M: the home configuration (position and orientation) of the
|
||||
% end-effector,
|
||||
% Slist: The joint screw axes in the space frame when the manipulator
|
||||
% is at the home position,
|
||||
% thetalist: A list of joint coordinates.
|
||||
% Returns T in SE(3) representing the end-effector frame, when the joints
|
||||
% are at the specified coordinates (i.t.o Space Frame).
|
||||
% Example Inputs:
|
||||
%
|
||||
% clear; clc;
|
||||
% M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
|
||||
% Slist = [[0; 0; 1; 4; 0; 0], ...
|
||||
% [0; 0; 0; 0; 1; 0], ...
|
||||
% [0; 0; -1; -6; 0; -0.1]];
|
||||
% thetalist =[pi / 2; 3; pi];
|
||||
% T = FKinSpace(M, Slist, thetalist)
|
||||
%
|
||||
% Output:
|
||||
% T =
|
||||
% -0.0000 1.0000 0 -5.0000
|
||||
% 1.0000 0.0000 0 4.0000
|
||||
% 0 0 -1.0000 1.6858
|
||||
% 0 0 0 1.0000
|
||||
|
||||
T = M;
|
||||
for i = size(thetalist): -1: 1
|
||||
T = MatrixExp6_Sym(VecTose3(Slist(:, i) * thetalist(i))) * T;
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
function T = MatrixExp6_Sym(se3mat)
|
||||
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
|
||||
% Takes a se(3) representation of exponential coordinates.
|
||||
% Returns a T matrix in SE(3) that is achieved by traveling along/about the
|
||||
% screw axis S for a distance theta from an initial configuration T = I.
|
||||
% Example Input:
|
||||
%
|
||||
% clear; clc;
|
||||
% se3mat = [ 0, 0, 0, 0;
|
||||
% 0, 0, -1.5708, 2.3562;
|
||||
% 0, 1.5708, 0, 2.3562;
|
||||
% 0, 0, 0, 0]
|
||||
% T = MatrixExp6(se3mat)
|
||||
%
|
||||
% Output:
|
||||
% T =
|
||||
% 1.0000 0 0 0
|
||||
% 0 0.0000 -1.0000 -0.0000
|
||||
% 0 1.0000 0.0000 3.0000
|
||||
% 0 0 0 1.0000
|
||||
|
||||
T = expm(se3mat);
|
||||
end
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
function Vlinear = BodyVelToLinearVel(V,G)
|
||||
% *** CHAPTER x: DYNAMICS OF OPEN CHAINS ***
|
||||
% Takes V: Body frame velocity,
|
||||
% G: Space frame G
|
||||
% Returns Glist: Spatial inertia matrices Gi of the links
|
||||
|
||||
[R, p] = TransToRp(G);
|
||||
Vlienar = se3ToVec(V)*[p;1];
|
||||
Vlienar = Vlienar(1:3);
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
function Glist = SpatialInertia(G)
|
||||
% *** CHAPTER x: DYNAMICS OF OPEN CHAINS ***
|
||||
% Takes G: A list of inertia,
|
||||
% Returns Glist: Spatial inertia matrices Gi of the links
|
||||
|
||||
n = size(G,1);
|
||||
Glist = zeros(6,6,n);
|
||||
for i = 1:n
|
||||
Glist(:,:,i) = diag(G(i,1:6));
|
||||
end
|
||||
Loading…
Reference in New Issue