% calc_z_from_M_M0 for partially exponential representation of motion assumption: zeta is small, approximation V(r) = I_3 M 4x4 matrix, motion M0 4x4 reference motion matrix zeta 6-vector, twist wf 6/2020
0001 %% calc_z_from_M_M0 for partially exponential representation of motion 0002 % 0003 % assumption: zeta is small, approximation V(r) = I_3 0004 % 0005 % M 4x4 matrix, motion 0006 % M0 4x4 reference motion matrix 0007 % 0008 % zeta 6-vector, twist 0009 % 0010 % wf 6/2020 0011 0012 function zeta = calc_z_from_M_M0(M,M0) 0013 0014 % calculate small motion matrix 0015 M_iM0 = M*inv(M0); 0016 % rotation from upper left 0017 zeta(1:3) = calc_v_from_S(M_iM0(1:3,1:3)); 0018 % translation from differential of upper right 0019 zeta(4:6) = M(1:3,4)-M0(1:3,4); 0020 % twist 0021 zeta=zeta(:); 0022 0023 return