83 lines
2.7 KiB
Mathematica
83 lines
2.7 KiB
Mathematica
|
clc; clear all; close all;
|
||
|
|
|
||
|
|
syms l real
|
||
|
|
syms q2 q1_d q2_d real
|
||
|
|
|
||
|
|
p1 = [l;0;0]; % distnace between parallel joints 1 and 2
|
||
|
|
|
||
|
|
% transformation from link 1 frame to joint 2 frame
|
||
|
|
Tj1 = [eye(3), p1; zeros(1,3) 1];
|
||
|
|
invTj = [Tj1(1:3,1:3)', -Tj1(1:3,1:3)'*Tj1(1:3,4); zeros(1,3),1];
|
||
|
|
% transformation from joint 2 frame to link 2 frame
|
||
|
|
Rl1 = [cos(q2) -sin(q2) 0; sin(q2) cos(q2) 0; 0 0 1];
|
||
|
|
Tl1 = [Rl1, zeros(3,1); zeros(1,3),1];
|
||
|
|
% Transfomation from link 1 frame to link 2 frame
|
||
|
|
T = Tj1*Tl1;
|
||
|
|
% Transformation from link 2 frame to link 1 frame
|
||
|
|
invT = [T(1:3,1:3)', -T(1:3,1:3)'*T(1:3,4); zeros(1,3),1];
|
||
|
|
|
||
|
|
|
||
|
|
% You can proceed with two options in terms of notation. First you can
|
||
|
|
% assume twist to be [w v], and second you can assue twist to be
|
||
|
|
% [v w], and you can slightly different results
|
||
|
|
|
||
|
|
% ------------------------------------------------------------------------
|
||
|
|
% [w v]
|
||
|
|
% ------------------------------------------------------------------------
|
||
|
|
Phi1 = [0 0 1 0 0 0]'; % screw axis of joint 1 in frame 1
|
||
|
|
Phi2 = [0 0 1 0 0 0]'; % screw axis of joint 2 in frame 2
|
||
|
|
|
||
|
|
% adjoint transformation from frame of link 2 to frame of link 1
|
||
|
|
invAdT = [invT(1:3,1:3), zeros(3); vec2skewSymMat(invT(1:3,4))*...
|
||
|
|
invT(1:3,1:3), invT(1:3,1:3)];
|
||
|
|
|
||
|
|
invAdTj = [invTj(1:3,1:3), zeros(3); vec2skewSymMat(invTj(1:3,4))*...
|
||
|
|
invTj(1:3,1:3), invTj(1:3,1:3)];
|
||
|
|
|
||
|
|
v1 = Phi1*q1_d; % velocity of link 1
|
||
|
|
|
||
|
|
v2 = invAdT*v1 + Phi2*q2_d; % velocity of link 2
|
||
|
|
|
||
|
|
V2 = [0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0]';
|
||
|
|
|
||
|
|
syms Ixx Iyy Izz Ixy Ixz Iyz m hx hy hz real
|
||
|
|
|
||
|
|
I11 = [Ixx Ixy Ixz; Ixy Iyy Iyz; Ixz Iyz Izz];
|
||
|
|
I12 = vec2skewSymMat([hx,hy,hz]);
|
||
|
|
I21 = -vec2skewSymMat([hx,hy hz]);
|
||
|
|
I22 = eye(3)*m;
|
||
|
|
|
||
|
|
I = [I11, I12; I21, I22];
|
||
|
|
|
||
|
|
momentum_cond = Phi2'*I*V2;
|
||
|
|
|
||
|
|
rlxd_inv_cond = simplify(Phi1'*invAdT'*I*invAdT*Phi1);
|
||
|
|
|
||
|
|
|
||
|
|
% ------------------------------------------------------------------------
|
||
|
|
% [v w]
|
||
|
|
% -----------------------------------------------------------------------
|
||
|
|
Phi1 = [0 0 0 0 0 1]'; % screw axis of joint 1 in frame 1
|
||
|
|
Phi2 = [0 0 0 0 0 1]'; % screw axis of joint 2 in frame 2
|
||
|
|
|
||
|
|
% adjoint transformation from frame of link 2 to frame of link 1
|
||
|
|
invAdT = inv_Ad_transf(T);
|
||
|
|
|
||
|
|
v1 = Phi1*q1_d; % velocity of link 1
|
||
|
|
|
||
|
|
v2 = invAdT*v1 + Phi2*q2_d; % velocity of link 2
|
||
|
|
|
||
|
|
V2 = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 0 0 1]';
|
||
|
|
|
||
|
|
I11 = m*eye(3);
|
||
|
|
I12 = -m*vec2skewSymMat([hx,hy,hz]);
|
||
|
|
I21 = m*vec2skewSymMat([hx,hy,hz]);
|
||
|
|
I22 = [Ixx Ixy Ixz; Ixy Iyy Iyz; Ixz Iyz Izz] - ...
|
||
|
|
m*vec2skewSymMat([hx,hy,hz])*vec2skewSymMat([hx,hy,hz]);
|
||
|
|
|
||
|
|
I = [I11, I12; I21, I22];
|
||
|
|
|
||
|
|
momentum_cond = Phi2'*I*V2
|
||
|
|
|
||
|
|
rlxd_inv_cond = simplify( Phi1'*invAdT'*I*invAdT*Phi1 )
|