
myparams.m_body = 7.0;
myparams.k_slip = 7000;
myparams.xi_rest = 0.17;
myparams.gravity = 9.8;

mystate.ydot = 0.8;
mystate.z = 0.2;
mystate.psi_td = 0.3;
mystate.xi_td = 0.16;

anglespan = linspace( 0.0, 0.5, 20 );

figure(1);
clf;
hold on;
for ind = 1:length( anglespan )

  mystate.psi_td = anglespan( ind );
  
  slipresults = slip_return_map( mystate, myparams );
  meanresults = mean_value_return_map( mystate, myparams );

  y_vf = meanresults.y_vf;

  y_t = y_vf - slipresults.xi_td * sin( slipresults.psi_td );
  z_t = slipresults.xi_td * cos( slipresults.psi_td );
  plot( y_t, z_t, 'bx');
  y_t = y_vf - meanresults.xi_td * sin( meanresults.psi_td );
  z_t = meanresults.xi_td * cos( meanresults.psi_td );
  plot( y_t, z_t, 'rx');

  y_b = y_vf - slipresults.xi_b * sin( slipresults.psi_b );
  z_b = slipresults.xi_b * cos( slipresults.psi_b );
  plot( y_b, z_b, 'bx');
  y_b = y_vf - meanresults.xi_b * sin( meanresults.psi_b );
  z_b = meanresults.xi_b * cos( meanresults.psi_b );
  plot( y_b, z_b, 'rx');

  y_l = y_vf - slipresults.xi_l * sin( slipresults.psi_l );
  z_l = slipresults.xi_l * cos( slipresults.psi_l );
  plot( y_l, z_l, 'bx');
  y_l = y_vf - meanresults.xi_l * sin( meanresults.psi_l );
  z_l = meanresults.xi_l * cos( meanresults.psi_l );
  plot( y_l, z_l, 'rx');

  plot( meanresults.y_vf, 0, 'gx' );
  plot( slipresults.y_vf, 0, 'go' );

  drawnow;
end

