ContinuousDynamics

CONTINUOUSDYNAMICS implements the ... for the Compass Gait Model

 Calling sequence:
       [xo1 to]=ContinuousDynamics(xi,phi);

 Revisions:
       Date          Programmer      Description

       2009.05.02    Fumiya Iida     Original code
 
 variables:
       xi    -- 
       phi  --

0001 function [ xo, t ] = ContinuousDynamics( xi, phi )
0002 %CONTINUOUSDYNAMICS implements the ... for the Compass Gait Model
0003 %
0004 % Calling sequence:
0005 %       [xo1 to]=ContinuousDynamics(xi,phi);
0006 %
0007 % Revisions:
0008 %       Date          Programmer      Description
0009 %
0010 %       2009.05.02    Fumiya Iida     Original code
0011 %
0012 % variables:
0013 %       xi    --
0014 %       phi  --
0015 
0016 global mH m a b l hx;
0017 global AMP PER;
0018 global qx0 env; 
0019 global SWSTFlag;
0020 global qHist2;
0021 
0022 g=9.8;  % Gravitational constant (m/s^2)
0023 dt=0.001;
0024 gamma=0.0;  
0025 
0026 q=xi(1:2)';
0027 q_d=xi(3:4)';
0028 
0029 % Swing Leg Dynamics Simulation
0030 qHist=[];
0031 %qHist2=[];
0032 
0033 for t=1:1000
0034     
0035     % Compute Controller Output
0036     ha=Controller(t,q,q_d,phi);
0037     
0038     % Passive walker
0039     % ha = 0;
0040 
0041     % Equation of Motion
0042     H=[m*b*b, -m*l*b*cos(q(2)-q(1)); -m*l*b*cos(q(2)-q(1)), (mH+m)*l*l+m*a*a];  % Eq. A.8
0043     B=[0, m*l*b*sin(q(2)-q(1))*q_d(2); -m*l*b*sin(q(2)-q(1))*q_d(1), 0];        % Eq. A.9
0044     G=[m*b*g*sin(q(1)); -(mH*l+m*a+m*l)*g*sin(q(2))];                           % Eq. A.10
0045     S=[0 -1; 1 1];
0046     U=[0; ha]; % ankle and hip actuation
0047 
0048     % Compute q[n+1], q_d[n+1]
0049     if(inv(H)==Inf)
0050         xo=[NaN NaN NaN NaN];
0051         break;
0052     end
0053     q_dd=inv(H)*(S*U-B*q_d-G); 
0054 
0055     q_new=q+q_d*dt;
0056     q_d_new=q_d+q_dd*dt;
0057     
0058     q=q_new;
0059     q_d=q_d_new;
0060     
0061     if SWSTFlag==0
0062         qHist2=[qHist2; q(1) q_d(1)];
0063     else
0064         qHist2=[qHist2; q(2) q_d(2)];
0065     end
0066      
0067     qHist=[qHist; q' q_d' ha t];
0068     
0069     % Collision Detection
0070 
0071     qx1=qx0+[l*sin(-q(2)), l*cos(-q(2))]'; %hip
0072     qx2=qx1-[-l*sin(q(1)), l*cos(q(1))]';  %sw leg
0073     
0074     if q(2)-q(1)<-0.001
0075         for xpos=1:env(1)
0076             if qx2(1)<env(2*xpos)
0077                 xpos=xpos-1;
0078                 break;
0079             end
0080         end    
0081 
0082         gamma=-atan((env(2*xpos+1)-qx0(2))/(env(2*xpos)-qx0(1)));
0083         
0084         if q(1)>0 & (q(1)+q(2))<=-gamma*2
0085             qx0=[qx2(1) env(2*xpos+1)]';
0086             SWSTFlag=~SWSTFlag;
0087             t
0088             break;
0089         end
0090     end
0091 
0092     % Visualization
0093     if mod(t,50)==0
0094 
0095         figure(hx);
0096         subplot(3,1,1);
0097         cla;
0098         hold on;
0099 
0100         % Draw Ground Surface
0101         for i=1:500
0102             plot([env((i-1)*2+2) env(i*2+2)], [env((i-1)*2+3) env((i-1)*2+3)],'-k');
0103         end
0104 
0105         % Draw Robot
0106         if SWSTFlag==0
0107             plot([qx0(1), qx1(1)],[qx0(2), qx1(2)],'-b');
0108             plot([qx1(1), qx2(1)],[qx1(2), qx2(2)],'-r');
0109         else
0110             plot([qx0(1), qx1(1)],[qx0(2), qx1(2)],'-r');
0111             plot([qx1(1), qx2(1)],[qx1(2), qx2(2)],'-b');
0112         end
0113         xlim([qx1(1)-1 qx1(1)+1.5]);
0114         ylim([qx1(2)-1.1 qx1(2)+0.2]);
0115         
0116         x = qx1(1) + 1.25*[-1,1];
0117         x = 0:0.1:2*pi;
0118         fill(qx1(1) + 0.008*sin(x), qx1(2) + 0.008*cos(x), [0 1 0]);
0119         
0120         drawnow();
0121     end
0122 end %t
0123 
0124 % Set output variables
0125 if t==1000
0126     xo=[NaN NaN NaN NaN];
0127 else
0128     xo=[q' q_d'];
0129 end
0130 
0131 % Visualization
0132 figure(hx);
0133 subplot(3,1,2);
0134 hold on;
0135 plot([0 800],[0 0],'-k');
0136 plot([PER/2 PER/2],[-0.5 0.5],'-k');
0137 plot([qHist(end,6) qHist(end,6)],[-0.3 0.3],'-r');
0138 plot(qHist(:,6),qHist(:,1),'-k');
0139 plot(qHist(:,6),qHist(:,2),'-r');
0140 plot(qHist(:,6),qHist(:,5)/AMP,'-g');
0141 % legend('','PER', '', '', '', 'AMP', 'Location', 'BestOutside' );
0142 drawnow;
0143 
0144 figure(hx);
0145 subplot(3,1,3);
0146 hold on
0147 %plot(qHist2(:,1),qHist2(:,2));
0148 plot(qHist2(:,1),qHist2(:,2));  
0149 %plot(qHist(:,1),qHist(:,3));
0150 drawnow;
0151 
0152 
0153 
0154 
0155 
0156 
0157 
0158 
0159 
0160 
0161 
0162 
0163