机器人打高尔夫球

clear all;
clc;
close all;
r=2;%%%%球的大小
fi=[pi/2,pi/2-(pi/2-pi/14)/3,pi/2-(pi/2-pi/14)/3*2,pi/14,pi/14-(pi/2-pi/14)/3];%%%%左边手的竖直角度
figolf=[-pi,-pi/3*2,-pi/3,0,pi/3];%%%%高尔夫杆的角度
%%% right leg %%%%
%%%%五帧中右边脚关节的位置%%%%
XRL(1,:) = [5 9 17 17];YRL(1,:) = [80 38 5 5];
XRL(2,:) = [7.5 14 17 17];YRL(2,:) = [80 45 5 5];
XRL(3,:) = [10 14 17 17];YRL(3,:) = [80 45 5 5 ];
XRL(4,:) = [18 18 18 18];YRL(4,:) = [80 45 5 5];
XRL(5,:) = [18 18 18 18];YRL(5,:) = [80 45 5 5];
%%% left leg %%%%
XLL(1,:) = [-15 -16 -17 -17];YLL(1,:) = [80 45 5 5];
XLL(2,:) = [-12.5 -14 -17 -17];YLL(2,:) = [80 45 5 5];
XLL(3,:) = [-10 -14 -17 -17];YLL(3,:) = [80 45 5 5];
XLL(4,:) = [-2  -10 -15 -15];YLL(4,:) = [80 38 10 5];
XLL(5,:) = [-2  -10 -15 -15];YLL(5,:) = [80 38 10 5];
for i=1:5%%%%五帧中各个关节和部位的位置，主要是两个角度的不同%%%%
X1=XRL(i,1);Y1=YRL(i,1);
X2=XLL(i,1);Y2=YLL(i,1);
%%% body and head %%
%%%%%身体和头的位置由腿的上端关节位置决定%%
XB(i,:) = [X2-5 X1+5 X1+5 X2-5];
YB(i,:) = [Y2 Y1 Y1+80 Y2+80];
XH(i,:) = [X2-2 X1+2 X1+2 X2-2];
YH(i,:) = [Y2+80 Y1+80 Y1+100 Y2+100];
%%% right hand%%%%
%%%%%手的位置由body的位置决定
XRH(i,1) = XB(i,3);
XRH(i,2) = XRH(i,1)-35*sin(fi(i));
XRH(i,3) = XRH(i,2)-35*sin(fi(i));
YRH(i,1) = YB(i,4)-10;
YRH(i,2) = YRH(i,1)-40*cos(fi(i));
YRH(i,3) = YRH(i,2)-40*cos(fi(i));
%%% left hand %%%%
XLH(i,1) = XB(i,4);
YLH(i,1) = YB(i,3)-10;
XLH(i,2) = (XRH(i,3)+XLH(i,1))/2;
YLH(i,2) = (YRH(i,3)+YLH(i,1))/2;
XLH(i,3) = XRH(i,3);
YLH(i,3) = YRH(i,3);
%%%golf%
%%高尔夫秆子的角度和手的位置来确定杆子的形态
xgolf(i,1)=XRH(i,3);
ygolf(i,1)=YRH(i,3);
xgolf(i,2)=xgolf(i,1)+70*sin(figolf(i));
ygolf(i,2)=ygolf(i,1)-70*cos(figolf(i));
end
%%%%五帧中golf球的位置%%%%
for i=1:4
xball(i,:)=[8 8];
yball(i,:)=[r r];
end
xball(5,:)=[100 100];
yball(5,:)=[r r];

%######################
figure('position',[100,100,400,400])
for j=1:4

    %%%%%每个帧的播放顺序
    No=[4,4,3,2,1,1,1,1,2,3,4,5,5];


    for k=1:length(No)
        i=No(k);
    fill(XH(i,:),YH(i,:),'g'),...
    hold on
    fill(XB(i,:),YB(i,:),'g'),...    

    plot(XLL(i,:), YLL(i,:), 'b-o','LineWidth',10),...
    plot(XLH(i,:), YLH(i,:), 'b-o','LineWidth',7),...
    plot(XRL(i,:), YRL(i,:), 'b-o','LineWidth',10),...
    plot(XRH(i,:), YRH(i,:), 'b-o','LineWidth',7),...

    plot(xgolf(i,:), ygolf(i,:), 'r-o','LineWidth',3),...
    plot(xball(i,:),yball(i,:),'r-o','LineWidth',r),...

    xlim([-150 100]);
    ylim([0 250]);
    pause(0.3);
    hold off
    end
    hold off
pause(1);
end