hexapod_calc_payload_3leg.m

%计算身体最大承载与位置的关系
clear;
close all
u2y = 0.232; %config(1)
u2z = 0.134; %config(2)
u3y = 0.232; %config(3)
u3z = -0.134; %config(4)
s2x = 0; %config(5)
s2y = 0.059; %config(6)
s2z = 0.034; %config(7)
s3x = 0; %config(8)
s3y = 0.059; %config(9)
s3z = -0.034; %config(10)
sfx = 0.142; %config(11)
sfy = -0.034; %config(12)
sfz = 0; %config(13)
home_pos = [0.669, 0.69, 0.69];  %config(14:16)
config = [u2y, u2z, u3y, u3z, s2x, s2y, s2z, s3x, s3y, s3z, sfx, sfy, sfz, home_pos];
stroke = 0.41;
leg_base_pe = [ -0.43322 , 0 , -0.19907,   pi/2,  pi* 2/3,   -pi/2-pi*70/180;
                -0.48305 , 0 ,  0,         pi/2,  pi* 3/3,   -pi/2-pi*70/180;
                -0.43322 , 0 ,  0.19907,   pi/2,  pi* 4/3,   -pi/2-pi*70/180;
                 0.43322 , 0 , -0.19907,   pi/2,  pi* 1/3,   -pi/2-pi*70/180;
                 0.48305 , 0 ,  0,         pi/2,  pi* 0/3,   -pi/2-pi*70/180;
                 0.43322 , 0 ,  0.19907,   pi/2,  pi* 5/3,   -pi/2-pi*70/180];
hexa = Hexapod(config,stroke,leg_base_pe);
rc_pee = [ -0.60,  -0.95,  -0.60;
           -0.80,  -0.95,   0;
           -0.60,  -0.95,   0.60;
            0.60,  -0.95,  -0.60;
            0.80,  -0.95,   0;
            0.60,  -0.95,   0.60 ];
peb = [0 0 0 0 0 0];
F_y = [0 1 0 0 0 0]';
x = -50:5:50;
payload = zeros(size(x));
for i = 1:length(x)
    hexa.invKin(peb,rc_pee');
    
end