Merge pull request #112 from CarlottaSartore/VelocityControllerPrepar…

…ationForJumping

Velocity controller preparation for jumping

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

@@ -1,20 +1,22 @@
-function [J_c, J_c_dot_nu] = ComputeContactJacobianWithFeetContactFCN(J_L, J_R, J_dot_nu_L, J_dot_nu_R, feet_contact_status)
+function [J_c, J_c_dot_nu, holonomicConstraintActivation] = 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);
-
+holonomicConstraintActivation = 0.0; 
 % 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;
+    holonomicConstraintActivation= 1.0; 
 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;
+    holonomicConstraintActivation = 1.0
 end
 
 end

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

@@ -1,12 +1,11 @@
-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)
+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, KpPostural,Gains)
 
 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; 
 
-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); 
-
+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 - KpPostural*(s_k-s_des_k); 
+%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

@@ -1,17 +1,94 @@
-function [L_d_n, 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,joint_vel_desired, L_ang_des,L_d)
+% function [L_d_n, 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,joint_vel_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
+% 
+% L_d_n = zeros(6,1);
+% L_dot_d   = zeros(6,1); 
+% L_d_n(1:3)  = M(1,1)*desired_pos_vel_COM(:,2)';
+% %L_d_n(4:6)  = L_d(4:6);
+% int_L_ang    = zeros(3,1); 
+% x_com_des    = desired_pos_vel_COM(:,1); 
+% s_des_k      = joint_pos_desired; 
+% s_dot_des_k  = joint_vel_desired; 
+function [L_d_n, L_dot_d, int_L_ang, x_com_des, s_des_k, s_dot_des_k, vel_feet_correction] = ComputeReferencesController(M,L,desired_pos_vel_COM, joint_pos, joint_pos_desired,joint_vel_desired, L_ang_des,L_d, w_H_l, w_H_r,Cs, Gains)
 
 persistent L_0; 
 
+persistent w_H_l_0; 
+
+persistent w_H_r_0; 
+
+persistent z_0; 
+
+persistent joint_pos_0; 
+
+persistent Cs_old ;
+
 if(isempty(L_0))
     L_0 = L; 
 end
 
-persistent joint_pos_0; 
+if(isempty(Cs_old))
+    Cs_old = Cs; 
+end
 
 if(isempty(joint_pos_0))
     joint_pos_0 = joint_pos; 
 end
 
+%%TODO could also check on the contact status,to update it each time the
+%%contact is restored. 
+
+
+if (isempty(w_H_r_0))
+    w_H_r_0 = w_H_r; 
+end
+
+if(isempty(w_H_l_0))
+    w_H_l_0 = w_H_l; 
+end
+
+if(isempty(z_0))
+    z_0 = w_H_l(3,4); 
+end
+
+% For now we are assuming that one foot is always in contact, we will
+% discard this assumption for performing the jump 
+% Updating only the position, we want to keep the original orientation 
+if(~Cs_old(1) && Cs(1)) 
+w_H_l_0(1,4) = w_H_l(1,4);
+w_H_l_0(2,4) = w_H_l(2,4); 
+w_H_l_0(3,4) = z_0; 
+end
+
+% Updating only the position, we want to keep the original orientation
+if(~Cs_old(2) && Cs(2))
+w_H_r_0(1,4) = w_H_r(1,4);
+w_H_r_0(2,4) = w_H_r(2,4);
+w_H_r_0(1:3,1:3)
+w_H_r_0(3,4) = z_0; 
+end
+
+vel_feet_correction = zeros(12,1); 
+if(Cs(1))
+    vel_feet_correction(1:3) = diag(Gains.Kp_feet_correction)*(w_H_l(1:3,4)- w_H_l_0(1:3,4));
+    vel_feet_correction(4:6) = Gains.Kp_feet_correction_angular* wbc.skewVee(w_H_l(1:3,1:3)*transpose(w_H_l_0(1:3,1:3)));
+end
+
+if(Cs(2))
+    vel_feet_correction(7:9) = diag(Gains.Kp_feet_correction)*(w_H_r(1:3,4)- w_H_r_0(1:3,4));
+    vel_feet_correction(10:12) = Gains.Kp_feet_correction_angular* wbc.skewVee(w_H_r(1:3,1:3)*transpose(w_H_r_0(1:3,1:3)));
+end
 L_d_n = zeros(6,1);
 L_dot_d   = zeros(6,1); 
 L_d_n(1:3)  = M(1,1)*desired_pos_vel_COM(:,2)';
@@ -21,5 +98,7 @@
 s_des_k      = joint_pos_desired; 
 s_dot_des_k  = joint_vel_desired; 
 
+Cs_old = Cs; 
 
 end
+