mech_stewartplatform

%   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(0.5*dl/radius_b); % +- offset angle from 120 degree spacing on base
alpha_t = asin(0.5*du/radius_t); % +- offset angle from 120 degree spacing on top

for i = 1:3,
  % top points
  % Top points are 60 degrees offset
  angle_m_t = (2*pi/3)* (i-1) - alpha_t ;
  angle_p_t = (2*pi/3)* (i-1) + alpha_t ;
  pos_top(2*i-1,:) = [radius_t* [cos(angle_m_t), sin(angle_m_t)], 0];
  pos_top(2*i,:) = [radius_t* [cos(angle_p_t), sin(angle_p_t)], 0];
  A(2*i-1,:) = [radius_t* [cos(angle_m_t), -sin(angle_m_t)], 0];
  A(2*i,:) = [radius_t* [cos(angle_p_t), -sin(angle_p_t)], 0];   
    % base points
  angle_m_b = (2*pi/3)* (i-1) - alpha_b + 2*pi/6;
  angle_p_b = (2*pi/3)* (i-1) + alpha_b + 2*pi/6;
  pos_base(2*i-1,:) =[radius_b* [cos(angle_m_b), sin(angle_m_b)], -height];
  pos_base(2*i,:) = [radius_b* [cos(angle_p_b), sin(angle_p_b)], -height];
  B(2*i-1,:) =[radius_b* [cos(angle_m_b), -sin(angle_m_b)], height];
  B(2*i,:) = [radius_b* [cos(angle_p_b), -sin(angle_p_b)], height];

end
% permute pos_top points so that legs are end points of base and top points
%permute again -->counterclockwise
pos_top = [pos_top([2:6],:); pos_top(1,:)];
%manufacturing and assembly errors
%  pos_base =pos_base +rand(6,3)*0.005;
%  pos_top =pos_top +rand(6,3)*0.005;

 A = [A([2:6],:);A(1,:)];

Xcg=                    0.000;%m
Ycg=                    0.000;%m
Zcg=              -0.162754;%m
CGofTOP=[Xcg Ycg -Zcg];
Ctrl_pt=[0 0 1.63874]';