Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add sym for FK and Linearvel

This commit is contained in:
cosmic_power 2023-11-04 17:21:51 +08:00
parent 10097c8db4
commit 9274ea0a1a
6 changed files with 137 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
packages/MATLAB/mr/BodyVelToLinearVel.m Normal file
View File

@ -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);

33
packages/MATLAB/mr/FKinSpaceExpand.m Normal file
View File

@ -0,0 +1,33 @@
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));
% Tlist(:,:,size(thetalist,1)) = M;
% T=M;
for i = size(thetalist): -1: 1
Tlist(:,:,i) = FKinSpace(Mlist(:,:,i), Slist(:,1:i), thetalist(1:i));
end
end

33
packages/MATLAB/mr/FKinSpaceExpand_Sym.m Normal file
View File

@ -0,0 +1,33 @@
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));
% Tlist(:,:,size(thetalist,1)) = M;
% T=M;
for i = size(thetalist): -1: 1
Tlist(:,:,i) = FKinSpace_Sym(Mlist(:,:,i), Slist(:,1:i), thetalist(1:i));
end
end

31
packages/MATLAB/mr/FKinSpace_Sym.m Normal file
View File

@ -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

23
packages/MATLAB/mr/MatrixExp6_Sym.m Normal file
View File

@ -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

9
packages/MATLAB/mr/SpaceVelToLinearVel.m Normal file
View File

@ -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);