improvements in velocity controller

library/simulink-library/MomentumVelocityControl/README.md

@@ -0,0 +1,14 @@
+# Momentum Based Velocity Control 
+
+This library implements the velocity based momentum controller.
+
+## :hammer: Dependencies 
+
+This simulink library depends on Casadi. 
+Please download Casadi by following this [link](https://web.casadi.org/get/) and add the related folder to your matlab path. 
+
+## How to run 
+
+Add this folder to your matlab path, the momentum velocity control library will be available in the simulink library browser. 
+
+

library/simulink-library/MomentumVelocityControl/src/ComputeContactJacobianWithFeetContactFCN.m

@@ -0,0 +1,20 @@
+function [J_c, J_c_dot_nu] = ComputeContactJacobianWithFeetContactFCN(J_L, J_R, J_dot_nu_L, J_dot_nu_R, feet_contact_status)
+
+NDOF = length(J_L(1,7:end));
+
+J_c        = zeros(12,6+NDOF);
+J_c_dot_nu = zeros(12,1);
+
+% if left foot in contact
+if(feet_contact_status(1))
+    J_c (1:6,:)     = J_L;
+    J_c_dot_nu(1:6) = J_dot_nu_L;
+end
+
+% if right foot in contact
+if(feet_contact_status(2))
+    J_c(7:end,:)      = J_R;
+    J_c_dot_nu(7:end) = J_dot_nu_R;
+end
+
+end

library/simulink-library/MomentumVelocityControl/src/ComputeReferences.m

@@ -1,9 +1,12 @@
-function [L_star, s_dot_star_k_1] = ComputeReferences(x_com, x_d_com, Ld, int_L_angular, s_dot_des_k_1, s_des_k, s_k, Gains)
+function [L_star, L_dot_star, s_dot_star_k_1] = ComputeReferences(x_com, x_d_com,  L, L_dot_d, Ld, int_L_angular, s_dot_des_k_1, s_des_k, s_k, totalMass,Gains)
 
-    L_int_lin = Gains.KP_CoM* (x_com- x_d_com); 
-    L_int_ang = Gains.KI_ang* (int_L_angular); 
-    L_int  = [L_int_lin; L_int_ang]; 
-    L_star = Ld - L_int; 
+L_int_lin = Gains.KP_CoM*totalMass*(x_com - x_d_com); 
+L_int_ang = Gains.KI_ang*(int_L_angular); 
+L_int     = [L_int_lin; L_int_ang]; 
+L_star    = Ld-L_int; 
 
-    s_dot_star_k_1 = s_dot_des_k_1 - Gains.KP_Postural*(s_k- s_des_k); 
-end
+L_dot_star = L_dot_d -blkdiag(Gains.KD_CoM, Gains.KP_ang)*(L-Ld) -L_int;
+
+s_dot_star_k_1 = s_dot_des_k_1 - Gains.KP_Postural*(s_k-s_des_k); 
+
+end

library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m

@@ -0,0 +1,29 @@
+function [L_d, L_dot_d, int_L_ang, x_com_des, s_des_k, s_dot_des_k] = ComputeReferencesController(M,L,desired_pos_vel_COM, joint_pos, joint_pos_desired, L_ang_des,L_d)
+
+persistent L_0; 
+
+if(isempty(L_0))
+    L_0 = L; 
+end
+
+persistent joint_pos_0; 
+
+if(isempty(joint_pos_0))
+    joint_pos_0 = joint_pos; 
+end
+
+
+%% for balancing 
+%L_d          = L_0;
+
+% L_d = zeros(6,1); 
+%L_d(4:6)= L_ang_des;
+L_dot_d   = zeros(6,1); 
+L_d(1:3)  = M(1,1)*desired_pos_vel_COM(:,2)';
+int_L_ang    = zeros(3,1); 
+x_com_des    = desired_pos_vel_COM(:,1); 
+s_des_k      = joint_pos_desired; 
+s_dot_des_k  = zeros(size(s_des_k)); 
+
+
+end