Merge pull request #130 from robotology/devel

Merge devel into master - minor changes and fixes

controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobot.m

@@ -50,8 +50,8 @@
 % if TRUE, the controller will STOP if the joints hit the joints limits
 % and/or if the (unsigned) difference between two consecutive joints
 % encoders measurements is greater than a given threshold.
-Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS  = false;
-Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false;
+Config.EVENT_WITH_JOINTS_LIMITS  = EventWithJointLimits.None;
+Config.EVENT_WITH_ENCODER_SPIKES = EventWithEncoderSpikes.Warning;
 
 % Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected
 % inertias are included in the system mass matrix. If

controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobotSim.m

@@ -49,7 +49,7 @@
 % Initial condition of iCub and for the integrators.
 initialConditions.base_position = [0; 0; 0.619];
 initialConditions.orientation = diag([-1, -1, 1]);
-initialConditions.w_H_b = mwbs.State.Rp2H(initialConditions.orientation, initialConditions.base_position);
+initialConditions.w_H_b = mwbs.Utils.Rp2H(initialConditions.orientation, initialConditions.base_position);
 
 % joint (inital) position
 initialConditions.s = [

controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/EventWithEncoderSpikes.m

@@ -0,0 +1,12 @@
+classdef EventWithEncoderSpikes < Simulink.IntEnumType
+    %EVENTWITHENCODERSPIKES Class for selecting a None, Warning or EmergencyStop signal
+    %   
+    %   Class for selecting a None, Warning or EmergencyStop signal triggered by an encoder spike
+    %   event.
+
+    enumeration
+        None(0)
+        Warning(1)
+        EmergencyStop(2)
+    end
+end

controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/EventWithJointLimits.m

@@ -0,0 +1,12 @@
+classdef EventWithJointLimits < Simulink.IntEnumType
+    %EVENTWITHJOINTLIMITS Class for selecting a None, Warning or EmergencyStop signal
+    %
+    %   Class for selecting a None, Warning or EmergencyStop signal triggered by the joints hitting
+    %   the limits.
+
+    enumeration
+        None(0)
+        Warning(1)
+        EmergencyStop(2)
+    end
+end

controllers/+floatingBaseBalancingTorqueControlWithSimulator/stopTorqueControlBalancingWithSimu.m

@@ -61,7 +61,7 @@
 
 % If a joint hits the limits or an encoder spike is detected, print a
 % warning message displaying the name of the joint
-if Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES && exist('res_check_spikes','var')
+if exist('res_check_spikes','var')
 
     if ~isempty(res_check_spikes.signals.values)
 
@@ -76,7 +76,7 @@
         end
     end
 end
-if Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS && exist('res_check_range','var')
+if exist('res_check_range','var')
 
     if ~isempty(res_check_range.signals.values)
 

library/matlab-wbc/+wbc/checkInputSpikesWithSimulator.m

@@ -0,0 +1,29 @@
+function [noSpikes, res_check_spikes] = checkInputSpikesWithSimulator(u_prev, u, delta_u_max)
+
+    % CHECKINPUTSPIKESWITHSIMULATOR checks the (unsigned) difference between two consecutive 
+    %                               measurements of the input u, i.e. delta = abs(u(k)-u(k-1))
+    %                               returns FALSE if delta is bigger than a user-defined threshold.
+    %
+    % FORMAT:  [noSpikes, res_check_spikes] = checkInputSpikesWithSimulator(u_prev, u, delta_u_max)
+    %
+    % INPUT:  - u = [n x 1] input values;
+    %         - u_prev = delayed [n x 1] input values;
+    %         - delta_u_max = user-defined threshold;
+    %
+    % OUTPUT: - noSpikes = FALSE if delta is greater than delta_u_max;
+    %         - res_check_spikes = vector of booleans to check the single joints.
+    %
+    % Authors: Gabriele Nava
+    %          
+    %          all authors are with the Italian Istitute of Technology (IIT)
+    %          email: name.surname@iit.it
+    %
+    % Genoa, Dec 2018
+    %
+
+    delta = abs(u - u_prev);
+
+    % u may be a vector
+    res_check_spikes = (delta >= delta_u_max);
+    noSpikes         = double(~any(res_check_spikes));
+end

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
+