Modern robotics mechanics planning and control(book)Rigid-Body Motion Example（book）

clc;clear;close;

rsc=pi/180;

Tsb=[cos(30*rsc) -sin(30*rsc) 0 1;sin(30*rsc) cos(30*rsc) 0 2;0 0 1 0;0 0 0 1]; Tsc=[cos(60*rsc) -sin(60*rsc) 0 2;sin(60*rsc) cos(60*rsc) 0 1;0 0 1 0;0 0 0 1];

q=pi/6;

T=Tsc*inv(Tsb);

R=T(1:3,1:3); trace(R)

w=1/(2*sin(q))*(R-R'); invG=(1/q)*eye(3)-(1/2)*w+(1/q-(1/2)*cot(q/2))*w*w; v=invG*T(1:3,4);