% 原作：Copyright 2019 The MathWorks, Inc.
% 改写、中文注解：几位耶 jiweiyecau@163.com
animateOn = true;
% 详解双足机器人步态的三维线性倒立摆模型
% 定义质心与足部在x方向上的偏移量
xTorso = 0.12; 
g = 9.807;  % 重力加速度
Ts = 0.005; % sample time (s)

zRobot = 0.78; %机器人直立时高度
zModel = 0.68;  %倒立摆高度
% 我们还需要定义脚轨迹的摆动高度。这是一个设计参数，目前在可视化之外没有任何影响。
swingHeight = 0.1; 


% 定义运动学初始条件。
% x0、y0是质心起始坐标，z轴起始坐标为倒立摆高度
% dx0、dy0是起始速度两个方向的分量
stepLength = 0.2; 
dy_mid = 0.06;  % 当y=0时y方向的速度，这是y方向的最小速度
x0 = 0.12;      % x0需要在xTorso附近 
[dx0, y0, dy0, tSingleSupport] = findInitialConditions(stepLength, dy_mid, x0, zModel, g, Ts);
% tSingleSupport = 1.335; %到达中点所需的时间
% x0 = 0.12; 
% dx0 = -0.4501;
% y0 = -0.1; 
% dy0 = 0.3845;



% 创建线性倒立摆的离散状态空间模型。
A = [0   1 0    0
    g/zModel 0 0    0
    0    0 0    1
    0    0 g/zModel 0]; 
B = [0 0
     1 0
     0 0
     0 1]; 
C = [1 0 0 0
     0 0 1 0]; 
D = [0 0;0 0]; 

lipm = ss(A,B,C,D);  %State-space model创建连续的状态空间模型
lipmD = c2d(lipm, Ts);  %将连续时间系统转换为离散时间系统，Ts为采样时间

Ad = lipmD.A; 
Bd = lipmD.B;
Cd = lipmD.C;
Dd = lipmD.D;
% 初始状态state0是允许对称轨迹的初始条件。
state0 = [x0; dx0; y0; dy0]; 
u0 = [0; 0];  % 假设无外力驱动，输入向量u0都是零。


% 将所有模型参数放置在一个结构中，可以使用单个变量将它们传递到其他函数中。
modelVars.g = g; 
modelVars.Ts = Ts; 
modelVars.zModel = zModel;
modelVars.Ad = Ad; 
modelVars.Bd = Bd; 
modelVars.Cd = Cd; 
modelVars.Dd = Dd; 
modelVars.tSingleSupport = tSingleSupport; 



% 类似地，创建一个simStates结构来跟踪模拟信息并将其传递给其他函数。
simStates = struct('bodyPos',[],'footPos',[],'timeVec',[]);
robotpos0 = [0; 0; zRobot];  % 很容易理解，这是倒立摆模型机器人质心起始位置


% 存储左脚和右脚的位置数据
simStates.footPos(:,end+1) = [-xTorso; 0; 0]; % 左脚起始位置，在simStates.footPos数组中增加一列。
simStates.footPos(:,end+1) = [ xTorso; 0; 0]; % 右脚起始位置
% 假设机器人首先向左迈出一步，此后在存储数据时都是先存储左脚数据，再存储右脚，以此类推。


% 设置仿真的步数，这里设置为6，那么程序会在仿真6个步态后停止。
numSteps = 6;



% 绘图相关，只要知道matlab绘图函数的含义就能很容易理解。
hFig = figure; 
hFig.Visible = 'on'; % for livescript 

% 指定绘图窗口大小，'Normalized'指这些单位依据父容器进行归一化。容器的左下角映射到 (0,0)，右上角映射到 (1,1)。
% 下面两句代码的效果就是绘图时总是充满屏幕
hFig.Units = 'Normalized'; 
% 外部边界的位置和大小，指定为 [left bottom width height] 形式的向量
hFig.OuterPosition = [0 0 1 1]; 
hAx  = axes(hFig);  % 创建笛卡尔坐标区

% 创建绘图句柄，初始化
hold(hAx, 'on'); 
hLeg  = plot3(hAx,[0,robotpos0(1)],[0,robotpos0(2)],[0,robotpos0(3)],...
                '-o','MarkerIndices',2,'LineWidth',2,'MarkerSize',8); % 代表
hCom = plot3(hAx,robotpos0(1),robotpos0(2),robotpos0(3)); % 代表质心点轨迹
hLineFootHold = plot3(hAx, simStates.footPos(1,:), simStates.footPos(2,:), simStates.footPos(3,:)); % 
hLineLeftFoot = plot3(hAx, simStates.footPos(1,1), simStates.footPos(2,1), simStates.footPos(3,1)); % 左脚位置
hLineRightFoot = plot3(hAx, simStates.footPos(1,2), simStates.footPos(2,2), simStates.footPos(3,2)); % 右脚位置
hold(hAx, 'off'); 

grid(hAx, 'on'); 
view(hAx,3)
axis equal 
hAx.XLim = [-3*zRobot 3*zRobot];
hAx.YLim = [-3*zRobot 3*zRobot]; 
legend(hAx, 'Virtual Leg and COM of LIP','Trajectory of COM', 'Foot Hold', 'Left Foot', 'Right Foot', ...
       'Location', 'northeast');

% 这个变量在这个程序里没有什么用，是传递信息给另一个程序的，这里我们用不到
stepinfos = cell(1,numSteps+2);

% 下面开始模拟双足运动
% 机器人在开始时将保持静止（在zRobot位置），必须降低其质心（到zModel位置）然后开始移动。 
% 首先将 COM 移至一脚上方

% 进入我们之前设置的初始条件 (x0, y0, dx0, dy0)。
% 通过 Robotics System Toolbox 中的cubicpolytraj 函数生成三次多项式轨迹，拟合轨迹。
% 您可以将其替换为另一个函数以创建不同的插值轨迹。
% 注意：变量 walkRdyPoints 的值，含义是命令机器人降低质心位置，同时向右脚移动。
% 因为在初始条件 (x0, y0, dx0, dy0)时，它需要用右脚站立。
% 这些值可以根据个人喜好来修改。
walkRdyPoints = [0      0.67*x0; 
                 0      0      ; 
                 zRobot zModel];  
% 注意，此时已经部分地将机器人重心转移到右脚上了。
timepoints = [0 1]; % 给1秒的时间来执行walkRdyPoints
timevec = timepoints(1):Ts:timepoints(end)-Ts; 
% 拟合生成三阶多项式轨迹，返回值q是位置，qd是速度
[q, qd] = cubicpolytraj(walkRdyPoints, timepoints, timevec); 
simStates.bodyPos = [simStates.bodyPos q];  %存储相关数据
simStates.timeVec = [simStates.timeVec timevec]; 


% save data
fhold_x = simStates.footPos(1,1);  %左脚x坐标
fhold_y = simStates.footPos(2,1);  %左脚y坐标
stepinfo.index = 1; %存储数据索引，第一步
% bodystate是质心轨迹和速度数据，[x坐标，x速度，y坐标，y速度，z坐标，z速度]
bodystate = [q(1,:); qd(1,:); q(2,:); qd(2,:); q(3,:); qd(3,:)];
% relstate是质心相对于左脚的位置，相对运动数据
relstate = bodystate - [fhold_x; 0; fhold_y; 0; 0; 0];
stepinfo.state = relstate;
stepinfo.timevec = timevec; 
stepinfo.mode = 'doublesupport'; %起初是双足支撑状态
stepinfo.footplant = [fhold_x; fhold_y; 0]; % 左脚绝对坐标
stepinfos{1} = stepinfo; %这是一个cell类型数据

% 更新绘图区
for idx = 1:size(simStates.bodyPos,2)
    % 由于是起初状态，所以有一个3x1的0向量
    points = [zeros(3,1) simStates.bodyPos(:,idx)]; 
    updateLine(hLeg, points); %updateLine函数在最下方
    appendLine(hCom, simStates.bodyPos(:,idx)); 
    if animateOn
        pause(0.005); % 如果开启了仿真，就做一个动画效果
    end
end



% 下一个阶段：迈出半步（假定步态都是对称的，抬脚和落脚两半是对称的）
% 使机器人进入初始状态并开始移动
% 注意：walkRdyPoints 变量，
% 下面的0.1是机器人在进行姿态转换（从右到左或从左到右）之前将其 COM 移动的距离。
walkRdyPoints(:,end+1) = [0     ; 
                          0.1   ; 
                          zModel];  
%第二行第三列，就是上面的0.1的值
yoff = walkRdyPoints(2,3); 
% 初始脚的位置
fhold_x = -x0; 
fhold_y = -y0 + yoff; 
simStates.footPos(:,end+1) = [fhold_x; fhold_y; 0]; % 左脚 
%注意此时的z坐标是0，这在后面计算时又加上了swingHeight

% 使站立脚行走的轨迹处于初始状态
rdyPos1 = walkRdyPoints(:,2); 
rdypos2 = walkRdyPoints(:,3); 
rdyvel1 = [0; 0; 0]; 
rdyvel2 = [dx0; dy0; 0]; 
waypoints = [rdyPos1 rdypos2];
velpoints = [rdyvel1 rdyvel2]; 


timepoints = [1 1.5]; % 一秒一步，半步就是0.5秒
timevec = timepoints(1):Ts:timepoints(end)-Ts; 
% 拟合轨迹，位置，速度，加速度
[q, qd, qdd] = cubicpolytraj(waypoints, timepoints, timevec, ...
                  'VelocityBoundaryCondition', velpoints); 
simStates.bodyPos = [simStates.bodyPos q]; 
simStates.timeVec = [simStates.timeVec timevec]; 

% 获取摆动脚的轨迹，摆动脚是左脚，左脚坐标分别存在第一列、第三列中。
swingfootpos0 = simStates.footPos(:,1); 
swingfootpos1 = simStates.footPos(:,3); 
[qswing, dqswing, ddqswing] = getSwingFootTraj(swingfootpos0, swingfootpos1, swingHeight, ...
                                timepoints(1), timepoints(end),Ts); 


% 存储数据
stepinfo.index = 2; 
bodystate = [q(1,:); qd(1,:); q(2,:); qd(2,:); q(3,:); qd(3,:)]; % 质心坐标及速度
relstate = bodystate - [simStates.footPos(1,2); 0; simStates.footPos(2,2); 0; 0; 0]; % 相对坐标 
stepinfo.state = relstate;
stepinfo.timevec = timevec; 
stepinfo.mode = 'singlesupportright'; %单脚支撑阶段，右脚单脚支撑
stepinfo.swing = qswing; %摆动脚坐标数据
stepinfo.footplant = [simStates.footPos(1,2); simStates.footPos(2,2); 0]; %摆动脚坐标
stepinfos{2} = stepinfo; 

% 更新图像，与前面一样
for idx = 1:size(q,2)
    points = [zeros(3,1) q(:,idx)]; 
    updateLine(hLeg, points); 
    appendLine(hCom, q(:,idx)); 
    % add points to swing foot 
    appendLine(hLineLeftFoot, qswing(:,idx)); 
    if animateOn
        pause(0.005);
    end

end
appendLine(hLineFootHold, [fhold_x; fhold_y; 0])





% 完整流程：初始：双足支撑
% -左脚迈出，右脚支撑--（进入循环）左脚支撑，右脚迈出--左脚支撑，右脚迈出...
% 迈出脚：左-（进入循环）右-左-右-左-右-左（循环结束，6步）
% 支撑脚：右-（进入循环）左-右-左-右-左-右（循环结束，6步）


% 连续步行阶段，这个算法很简陋。
% 我们将初始速度（dx0 和 dy0）设置为与前一个姿势的最终速度（dxf 和 dyf）相同
% 注 1：不存在双支撑阶段。 该模型始终由单脚支撑。瞬间从左脚站立变为右脚站立，
% 大多数机器人在两者之间都会有双支撑阶段，以提高稳定性。 为了简化轨迹，省略了双支撑阶段。
% 注 2：此处创建摆动脚轨迹只是为了可视化。 脚部轨迹不必在此处定义，因为可以在稍后执行脚部运动计划时完成。

fhold_x1 = fhold_x; 
fhold_y1 = fhold_y; 
% （进入循环）
for stp = 1:numSteps  
    % 更新支撑脚坐标，注意z坐标是单独计算的
    fhold_x0 = fhold_x1;
    fhold_y0 = fhold_y1;

    % 在单脚支撑阶段计算线性倒立摆模型
    [states, timevec, simStates] = stanceSim(fhold_x0, fhold_y0, state0, u0, modelVars, simStates);

    % 保存信息
    index = stp + 2;  %加2的原因是前面已经有2个了
    stepinfo.index = index; 
    savestate = [states; repmat([zModel; 0],[1 size(states,2)])]; % 增加z坐标和z方向速度，repmat是重复数组副本
    stepinfo.state = savestate; 
    stepinfo.footplant = [fhold_x0; fhold_y0; 0]; 
    stepinfo.timevec = timevec; 
    % 求余运算，判断是左脚还是右脚迈出
    if rem(stp,2) > 0
        stepinfo.mode = 'singlesupportleft';
    else
        stepinfo.mode = 'singlesupportright';
    end     

    % 换腿，更新fhold_x1和fhold_y1
    [state0, fhold_x1, fhold_y1] = changeLeg(states(:,end), simStates);
    simStates.footPos(:,end+1) = [fhold_x1; fhold_y1; 0];

    % 用新的腿位置定义摆动脚轨迹
    [q, dq, ddq] = getSwingFootTraj(simStates.footPos(:,end-2), simStates.footPos(:,end), ...
        swingHeight,timevec(1), timevec(end), Ts); 
    stepinfo.swing = q; 

    % 仿真落脚点
    for idx = 1:size(states,2)
        state1 = states(:,idx); 
        meas1 = Cd*state1 + Dd*u0; 

        bodypos = [fhold_x0 + meas1(1);
                   fhold_y0 + meas1(2);
                   zModel]; 
        % 仿真站立腿和质心
        points = [fhold_x0, bodypos(1);
                  fhold_y0, bodypos(2);
                  0,        bodypos(3)]; 
        updateLine(hLeg, points)
        appendLine(hCom, bodypos); 

        % 仿真摆动腿
        if rem(stp,2) > 0
            appendLine(hLineRightFoot, q(:,idx))
        else
            appendLine(hLineLeftFoot, q(:,idx))
        end
        if animateOn
            pause(0.005);
        end
    end 
    % 更新绘图区：
    % 根据新落脚点仿真
    points = [fhold_x1, bodypos(1);
              fhold_y1, bodypos(2);
              0,        bodypos(3)]; 
    updateLine(hLeg, points)
    appendLine(hLineFootHold, [fhold_x1; fhold_y1; 0])
    if animateOn
        pause(0.005); 
    end

    % 保存数据
    stepinfos{stp+2} = stepinfo;  
end
% （循环结束，6步）




%dy_mid是当Y＝0时，在轨迹段的中点处的期望Y速度。
function [dx0, y0, dy0, tsinglesupport] = findInitialConditions(stepLength, dy_mid, x0, zModel, g, Ts)
    %Tc是倒立摆系统的特征时间
    Tc = sqrt(zModel/g); 
    % Desired midstance y state
    y_mid = 0; 
    % 轨道能量，参阅前文的公式（28）
    E = -g/(2*zModel)*y_mid^2 + 0.5*dy_mid^2; 

    y0 = -stepLength/2;
    % 能量守恒，根据中点处能量计算起始速度
    dy0 = sqrt(2*(E+g/(2*zModel)*y0^2));

    % 根据最终状态和起始状态的关系可以求得单脚支撑阶段的时间
    tsinglesupport = 2*asinh(stepLength/2/(Tc*dy_mid))*Tc;
    % 保证可以被采样时间整除，将计算得到的时间量化为离散时间点
    tsinglesupport = floor(tsinglesupport/Ts)*Ts;
    tf = tsinglesupport/2; 
    dx0 = -x0/Tc * sinh(tf/Tc) / cosh(tf/Tc);
end





% Replaces points in a graphics line object
function updateLine(gHandle, points)
    %将数据点更新到gHandle句柄指向的图像
    gHandle.XData = points(1,:); 
    gHandle.YData = points(2,:); 
    gHandle.ZData = points(3,:); 
end
% Adds points to a graphics line object
function appendLine(gHandle, points)
    gHandle.XData(end+1) = points(1); 
    gHandle.YData(end+1) = points(2); 
    gHandle.ZData(end+1) = points(3); 
end

% 迈出脚步时，根据起止位置、脚步高度、时间，进行插值。
% 三次多项式轨迹
function [q, qd, qdd] = getSwingFootTraj(footpos0, footpos1, swingheight, timestp0, timestpf, Ts)
    % Copyright 2019 The MathWorks, Inc.

    % x,y坐标，
    waypointsXY = [footpos0(1:2) footpos1(1:2)];
    timestampsXY = [timestp0 timestpf];
    timevecswingXY = timestampsXY(1):Ts:timestampsXY(end);
    [XYq, XYqd, XYqdd, ~] = cubicpolytraj(waypointsXY, timestampsXY, timevecswingXY);

    % z坐标轨迹[0,0+swingHeight,0]
    waypointsZ = [footpos0(3) footpos0(3)+swingheight footpos0(3)];
    timestpMid = (timestp0+timestpf)/2; %到达中点的时间
    timestampsZ = [timestp0 timestpMid timestpf];
    timevecswingZ = timestampsZ(1):Ts:timestampsZ(end);
    [Zq, Zqd, Zqdd, ~] = cubicpolytraj(waypointsZ, timestampsZ, timevecswingZ);

    % combine xy and z trajectory
    q = [XYq; Zq];
    qd = [XYqd; Zqd];
    qdd = [XYqdd; Zqdd];
end


% stanceSim：使用给定的初始条件对单次支撑时间(tSingleSupport) 进行模拟
function [states, timeVec, simStates] = stanceSim(fhold_x, fhold_y, state0, u0, modelVars, simStates)

    % 取出变量
    Ts = modelVars.Ts;
    Ad = modelVars.Ad;
    Bd = modelVars.Bd;

    % 定义时间向量
    tInitial = simStates.timeVec(end) + modelVars.Ts; % 不允许时间重叠
    tFinal   = tInitial + modelVars.tSingleSupport; %起始时间+单脚支撑的时间
    timeVec  = tInitial:Ts:tFinal; % 生成时间序列

    % 仿真
    nSteps = numel(timeVec);  %此函数 返回数组 timeVec 中的元素数目
    states = zeros(size(state0,1),nSteps); % 预分配内存 
    states(:,1) = state0; % 第一列是起始坐标
    % 根据倒立摆模型求解，其中u0是0
    for idx = 1:nSteps-1
        states(:,idx+1) = Ad*states(:,idx) + Bd*u0;
    end

    % 根据求解结果，更新数据
    simStates.timeVec = [simStates.timeVec timeVec(1:end-1)];
    % 将坐标加上起始的偏置
    newStates = [states(1,2:end)+fhold_x; states(3,2:end)+fhold_y; modelVars.zModel*ones([1 nSteps-1])];
    % 将newStates添加到simStates.bodyPos中去
    simStates.bodyPos = [simStates.bodyPos newStates];
end

% changeLeg：使用简单的镜像方法找到下一个脚点。
function [state0, fhold_x, fhold_y] = changeLeg(state1,simStates)  
    % 此前状态
    xf  = state1(1); 
    dxf = state1(2); 
    yf  = state1(3); 
    dyf = state1(4); 

    % 通过对称的办法来简单计算
    x0 = -xf; 
    y0 = -yf; 
    dx0 = dxf; 
    dy0 = dyf; 
    state0 = [x0; dx0; y0; dy0]; 

    % 新的足迹
    fhold_x = simStates.bodyPos(1,end) - x0; 
    fhold_y = simStates.bodyPos(2,end) - y0;
end