CollisionModel

COLLISIONMODEL implements the collision model for the Compass Gait Model

 Calling sequence:
       [xo2]=CollisionModel(xo1);

 Revisions:
       Date          Programmer      Description

       2009.05.02    Fumiya Iida     Original code

0001 function [ xo ] = CollisionModel( xi )
0002 %COLLISIONMODEL implements the collision model for the Compass Gait Model
0003 %
0004 % Calling sequence:
0005 %       [xo2]=CollisionModel(xo1);
0006 %
0007 % Revisions:
0008 %       Date          Programmer      Description
0009 %
0010 %       2009.05.02    Fumiya Iida     Original code
0011 %
0012 
0013 global mH m a b l;
0014 
0015 q=xi(1:2)';
0016 q_d=xi(3:4)';
0017  
0018 alpha=(q(1)-q(2))/2;
0019 Qp=[m*b*(b-l*cos(2*alpha)), m*l*(l-b*cos(2*alpha))+m*a*a+mH*l*l; m*b*b, -m*b*l*cos(2*alpha)];   %% Eq. A.36
0020 Qm=[-m*a*b, -m*a*b+(mH*l*l+2*m*a*l)*cos(2*alpha); 0, -m*a*b];                                   %% Eq. A.35
0021 q_d=inv(Qp)*Qm*q_d;
0022 q=[q(2) q(1)]';
0023 
0024 xo=[q' q_d'];
0025 
0026