R = robot.kine.R;
P = robot.kine.t;
F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1

FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
FF2 = F_Simpack(end-1,:,1)'+FF1
%%
F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
%%
F_Simpack(end,:,1)
F_Simpack(end-2,:,1)
%%
robot_pi_vecoter = reshape(robot.pi,[90,1]);
F = robot.regressor.U*robot_pi_vecoter;
FF = reshape(F,[6,9])
F_Simpack(:,:,1)