%   Copyright 1998-2008, Harbin Institute of Technology.
%   Jiang Hong-Zhou deg2rad = pi/180;
x_axis = [1 0 0];
y_axis = [0 1 0];
z_axis = [0 0 1]; % Connection points on base and top plate w.r.t. World frame at the center
% of the base plate
pos_base = [];
pos_top = [];
A=[];B=[];
height = 1.63874; % 1.40473 meter height in home configuration
radius_b = 2.4/2; % base radius in meters
radius_t = 1.12/2; % top radius in meters
du=0.26;
dl=0.45;
alpha_b = asin(...