anchor.diff:  i21(A) -> ?I1 + ?M1*(?Rh*?D*math:sin(?Eta-A) + ?D*?D).
   i22(A,B) -> 2*?M1*?D*?D*math:sin(A+B)*math:cos(A+B)-?M1*?Rh*?D*math:cos(?Eta-A).
   i23(A) -> ?I1 +?I2 + ?M1*(?Rh*?Rh + 2*?Rh*?D*math:sin(?Eta-A) + ?D*?D).
  -i24(A,B) -> 2*?M1*?D*?D*math:sin(A+B)*math:cos(A+B) - 2*?Rh*?D*math:cos(?Eta-A). 
  +i24(A,B) -> 2*?M1*?D*?D*math:sin(A+B)*math:cos(A+B). 
  +i25(A) -> -2*?M1*?Rh*?D*math:cos(?Eta-A).
   
   
   f1(A,B,Psi) ->  ?M1*?G*?D*math:sin(A+B)
  -				- ?K1*?Z1*(?Dz + ?Z0*math:sin(Psi) - ?Z1*math:cos(A))*math:sin(A)
  -				+ ?K2*?Z2*(?Dz - ?Z0*math:sin(Psi) + ?Z2*math:cos(A))*math:sin(A).
  +				- ?K1*?Z1*(?Dz + ?Z0*math:sin(Psi) - ?Z1*math:cos(A) )*math:sin(A)
  +				+ ?K2*?Z2*(?Dz - ?Z0*math:sin(Psi) + ?Z2*math:cos(A) )*math:sin(A).
   
   f2(A,B) ->	- ?M2*?G*?R0*math:cos(?Eta0+B)
   			- ?M1*?G*(?Rh*math:cos(?Eta+B) - ?D*math:sin(A+B)).
  
positive.code: %
  % I11*A'' + i12*A'^2 + i13*B'' + i14*B'^2 + i15*A'*B' = f1(A,B,Psi)
  %
  % I21*A'' + i22*A'^2 +i23*B'' + i24*B'^2  = f2(A,B)
  %
  i11(_A,_B) -> ?I1 - ?K0 + ?M1*?D*?D.
  i12(_A,_B) -> 0.
  
negative.code: -define(Foot,0.05).
  -define(Eta, math:atan(?L/?Foot)).
  -define(Rh, math:sqrt(?L*?L + ?Foot*?Foot)).
  -define(Eta0, 0.45).
  -define(R0, 0.04). % distance from tip of foot to the leg's CoM
  
  %% springs params
  