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

robotology
github-repos/MATLAB/robotology-whole-body-controllers/whole-body-controllers-90ff965a523f0a120e6a8981b71326c1485e7742/library/simulink-library/MomentumVelocityControl/src/ComputeContactJacobianWithFeetContactFCN.m
whole-body-controllers
github-repos/MATLAB/robotology-whole-body-controllers
90ff965a523f0a120e6a8981b71326c1485e7742