Odd things. As systems moves left in double support, the right leg is
loaded more and the left leg is unloaded.

need better handling of ground level for uneven ground.

need uniforn coordinate systems for robot that take into account
facing direction. 

****************************************************************

We are using states very close to sdfast states in the optimization.
Double support is different, since
sdfast does not not use minimal double support states.
This means the controller and the optimization stuff needs to
know about the sdfast stuff, and that only dynamics-impact.c
can be used. We need to symmetrize sdfast states.

****************************************************************
Possible parameters:

  s->predicted_x_d = 0.03;

torque1  5.0 opt end: feedforward for ds_torque

  THIS NEEDS TO BE RELATIVE TO SOMETHING?
NO, This is a velocity!
comd1  0.18 opt end: when to shut off double support OBSOLETE

hip_ff1  100.0 opt end: feedforward during SS1
knee_length_d1  0.1 opt end: knee length desired in SS1
knee_extend1  0.05 opt end: when to extend knee is SS2
ds_torque_gain  0.5 opt end OBSOLETE
ss_torque_gain  50.0 opt end
orbital_energy_d  1.0 opt end OBSOLETE

ds:
knee desired
knee PDI gains
knee FF
knee desired
knee PDI gains
knee FF
SS1:
hip PDI gains
hip FF
hip PDI gains
hip FF
knee desired
knee PDI gains
knee FF
knee desired
knee PDI gains
knee FF
      s->ankle_command[LEFT] 
	= s->ss_torque_gain*(s->orbital_energy - s->orbital_energy_d);

****************************************************************

[D,names,units,freq] = mrdplot_convert('d02317');
char(names(26,:))
com_x
char(names(28,:))
com_xd
pos = D(:,26);
vel = D(:,28);
plot(pos,vel)
c = 1:10000;
c = c/10000.0;
c = c';

plot(pos - 0.161*c,vel)
plot(pos - 0.161*c - 0.0855,0.113*vel)
z = 0.0:0.01:6.28;
plot(pos - 0.161*c - 0.0855,0.14*vel - 0.002,'b',0.02*cos(z),0.02*sin(z),'r')

[D,names,units,freq] = mrdplot_convert('d02361');
z = 0.0:0.01:6.28;
radius = 43;
plot((pos -0.112)*radius ,0.16*vel*radius,'b',cos(z),sin(z),'r')

****************************************************************
