% ----------------------------------------------------
% Created by Uluc Saranli Mon Mar 18 16:20:15 EST 2002
% ----------------------------------------------------
%
% function slip = com_to_slip( com, y_vf )
%
% Returns the SLIP polar coordinates given the cartesian 
% coordinates and the location of the foot
%
% com = [y z ydot zdot]
% slip = [xi psi xidot psidot]
%
function slip = com_to_slip( com, y_vf )

  y = com(1);
  z = com(2);
  ydot = com(3);
  zdot = com(4);
  
  xi = sqrt( (y - y_vf)^2 + z^2 );
  psi = atan2( y_vf - y, z );
  xidot = ((y - y_vf) * ydot + z * zdot) / xi;
  psidot = ( z * ydot - ( y - y_vf ) * zdot ) / (xi * xi );
  
  slip = [xi psi xidot psidot];
