Code covered by the BSD License  

Highlights from
Kinematic Control of 3PRR - Articulated form of Hexapod

image thumbnail
from Kinematic Control of 3PRR - Articulated form of Hexapod by Hrishi Shah
A Hexapod is modeled as a 3PRR by taking top view projection and controlled Kinematically.

[qdot]=Mydiff(t,x) 
function [qdot]=Mydiff(t,x)
global lengths radiusx radiusy ell_angle L angles phidot
ell_an=ell_angle*pi/180;

J1 =[L(1)*cos(angles(1)), -lengths(1,1)*sin(x(2)), -lengths(1,2)*sin(x(3));
     L(1)*sin(angles(1)),  lengths(1,1)*cos(x(2)),  lengths(1,2)*cos(x(3));
                       0,                       0,                      1]; % J1
J2 =[L(2)*cos(angles(2)), -lengths(2,1)*sin(x(5)), -lengths(2,2)*sin(x(6));
     L(2)*sin(angles(2)),  lengths(2,1)*cos(x(5)),  lengths(2,2)*cos(x(6));
                       0,                       0,                      1]; % J2
J3 =[L(3)*cos(angles(3)), -lengths(3,1)*sin(x(8)), -lengths(3,2)*sin(x(9));
     L(3)*sin(angles(3)),  lengths(3,1)*cos(x(8)),  lengths(3,2)*cos(x(9));
                       0,                       0,                      1]; % J3
A=[J1,-J2,zeros(3,3);
   J1,zeros(3,3),-J3]; % building the constraint matrix as Xe=J1X1=J2X2=J3X3
A1=[A(:,2:3),A(:,5:6),A(:,8:9)]; %isolating the dependent parameters
B1=[A(:,1),A(:,4),A(:,7)]; % isolating the independent parameters

xdot=-radiusx*sin(t)*cos(ell_an)-radiusy*cos(t)*sin(ell_an); % trajectory information
ydot=-radiusx*sin(t)*sin(ell_an)+radiusy*cos(t)*cos(ell_an);

C=-pinv(A1)*B1; % Matrix relating dependent dof's to independent dof's
D=[1,0,0;
    C(1:2,:);
    0,1,0;
    C(3:4,:);
    0,0,1;
    C(5:6,:)]; % Matrix relating all dof's to independent dof's

Ja=J1*D(1:3,:); % Matrix relating Endeffector dof to independent dof
Ji=pinv(Ja);
NullSpace=eye(3)-Ji*Ja; % calculate NULL SPACE
qidot=Ji*[xdot;ydot;phidot]+NullSpace*[0.5-x(1);0;0];
qdot=D*qidot;

Contact us