WIP1: Add qpOASES as an alternative solver in the contacts computation

- Add separated step_block_contacts MATLAB system block for computing the contacts
  (block is still unfinished)
- Add shared configuration class StepBlockInit
- Add option to select qpOASES insteas of quadprog

init.m

@@ -34,6 +34,10 @@
 Config.GRAVITY_ACC = [0,0,-9.81];
 Config.tStep = 0.001;
 
+% Use qpOASES instead of quadprog, typically in the case where the optimization toolbox is not
+% available.
+Config.USE_QPOASES = true;
+
 % Do you want to enable the Visualizer?
 confVisualizer.visualizeRobot = true;
 
@@ -47,4 +51,5 @@
 %% Init simulator core physics paramaters
 physics_config.GRAVITY_ACC = Config.GRAVITY_ACC;
 physics_config.TIME_STEP = Config.tStep;
+physics_config.USE_QPOASES = Config.USE_QPOASES;
 robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = Config.SIMULATE_MOTOR_REFLECTED_INERTIA;

lib/+wbs/@Contacts/Contacts.m

@@ -11,12 +11,13 @@
         is_in_contact = ones(8, 1); % this vector says if the vertex is in contact (1) or not (0)
         S; % selector matrix for the robot torque
         mu; % friction coefficient
+        useQPoases; % Use the qpOASES solver instead of quadprog for the optim. prob. computing the reaction forces at the feet
         A; b; Aeq; beq; % matrix used in the optimization problem
     end
 
     methods
 
-        function obj = Contacts(foot_print, robot, friction_coefficient)
+        function obj = Contacts(foot_print, robot, friction_coefficient,useQPoases)
             %CONTACTS The Contact class needs the coordinates of the vertices of the foot
             % Arguments
             %   foot_print - the coordinates of every vertex in xyz
@@ -30,7 +31,8 @@
             obj.S = [zeros(6, robot.NDOF); ...
                     eye(robot.NDOF)];
             obj.mu = friction_coefficient;
-            obj.prepare_optimization_matrix();
+            obj.useQPoases = useQPoases;
+            obj.prepare_optimization_matrix_for_quadprog();
         end
 
         function [generalized_total_wrench, wrench_left_foot, wrench_right_foot, base_pose_dot, s_dot] = ...
@@ -233,8 +235,9 @@
 
         end
 
-        function prepare_optimization_matrix(obj)
-            % prepare_optimization_matrix Fills the matrix used in the optimization problem
+        function prepare_optimization_matrix_for_quadprog(obj)
+            % prepare_optimization_matrix Fills the matrix used by the optimization problem solver
+            % quadprog: vertical concatenation of Ax <= b and -x <= 0.
 
             total_num_vertices = obj.num_vertices * 2; % number of vertex per foot * number feet
             num_variables = 3 * total_num_vertices; % number of unknowns - 3 forces per vertex
@@ -258,6 +261,30 @@ function prepare_optimization_matrix(obj)
 
         end
 
+        function prepare_optimization_matrix_for_qpOASES(obj)
+            % prepare_optimization_matrix Fills the matrix used by the optimization problem solver
+            % qpOASES: lbA <= Ax <= ubA and the constraint on the output lb <= x <= ub.
+
+            total_num_vertices = obj.num_vertices * 2; % number of vertex per foot * number feet
+            num_variables = 3 * total_num_vertices; % number of unknowns - 3 forces per vertex
+            num_constr = 5 * total_num_vertices; % number of constraint: simplified friction cone + non negativity of vertical force
+            % fill the optimization matrix
+            obj.A = zeros(num_constr, num_variables);
+            obj.b = zeros(num_constr, 1);
+            obj.Aeq = zeros(total_num_vertices, num_variables);
+            obj.beq = zeros(total_num_vertices, 1);
+
+            constr_matrix = [1, 0, -obj.mu; ...% first 4 rows: simplified friction cone
+                        0, 1, -obj.mu; ...
+                            -1, 0, -obj.mu; ...
+                            0, -1, -obj.mu; ...
+                            0, 0, -1]; ...% non negativity of vertical force
+
+            % fill a block diagonal matrix with all the constraints
+            Ar = repmat(constr_matrix, 1, total_num_vertices); % Repeat Matrix for every vertex
+            Ac = mat2cell(Ar, size(constr_matrix, 1), repmat(size(constr_matrix, 2), 1, total_num_vertices)); % Create Cell Array Of Orignal Repeated Matrix
+            obj.A = blkdiag(Ac{:});
+        end
     end
 
 end

lib/+wbs/@StepBlockInit/StepBlockInit.m

@@ -0,0 +1,33 @@
+classdef StepBlockInit < handle
+    % step_block_contacts This block takes as input the joint torques and the
+    % applied external forces and evolves the state of the robot
+
+    properties (Access = private)
+        robot; contacts; state;
+    end
+
+    properties (Constant = true)
+        sharedConfig = wbs.StepBlockInit();
+    end
+
+    methods (Static = true)
+        function setSharedConfig(obj)
+            theSingleton = wbs.StepBlockInit.sharedConfig;
+            theSingleton.robot = obj.robot;
+            theSingleton.contacts = obj.contacts;
+            theSingleton.state = obj.state;
+        end
+
+        function obj = getSharedConfig(obj)
+            theSingleton = wbs.StepBlockInit.sharedConfig;
+            obj.robot = theSingleton.robot;
+            obj.contacts = theSingleton.contacts;
+            obj.state = theSingleton.state;
+        end
+    end
+
+    methods (Access = protected)
+        function obj = StepBlockInit()
+        end
+    end
+end

lib/RobotDynamicsWithContacts/+robotDynWC/ProcessQPOutputFCN.m

@@ -0,0 +1,31 @@
+function  [qp_output, stop] = ProcessQPOutputFCN(qpStatus, qpSolution)
+
+persistent oldQPSolution;
+persistent counter;
+
+coder.extrinsic('warning')
+if isempty(oldQPSolution)
+    oldQPSolution = zeros(size(qpSolution));
+end
+
+if isempty(counter)
+    counter = 0.0 ;
+end
+
+if(qpStatus == 0.0)
+    qp_output = qpSolution;
+    oldQPSolution = qpSolution;
+    counter = 0.0;
+    stop = 0.0;
+
+else
+    counter = counter +1.0;
+    qp_output = oldQPSolution;
+    stop = 0.0;
+
+    warning("QPOasesError: Sending last feaseable solution")
+    if (counter>2.0)
+        stop = 1.0;
+        warning("QPOasesError: Stopping the Controller")
+    end
+end

lib/RobotDynamicsWithContacts/+robotDynWC/step_block.m

@@ -13,18 +13,14 @@
 
     end
 
-    properties (Access = private)
+    properties (Access = {?wbs.StepBlockInit})
         robot; contacts; state;
     end
 
     methods (Access = protected)
 
         function setupImpl(obj)
-            obj.robot = wbs.Robot(obj.robot_config,obj.physics_config.GRAVITY_ACC);
-            obj.contacts = wbs.Contacts(obj.contact_config.foot_print, obj.robot, obj.contact_config.friction_coefficient);
-            obj.state = wbs.State(obj.physics_config.TIME_STEP);
-            obj.state.set(obj.robot_config.initialConditions.w_H_b, obj.robot_config.initialConditions.s, ...
-                obj.robot_config.initialConditions.base_pose_dot, obj.robot_config.initialConditions.s_dot);
+            obj = wbs.StepBlockInit.getSharedConfig(obj);
         end
 
         function [w_H_b, s, base_pose_dot, s_dot, wrench_left_foot, wrench_right_foot, kinDynOut] = stepImpl(obj, generalized_ext_wrench, torque, motorInertias)

lib/RobotDynamicsWithContacts/+robotDynWC/step_block_contacts.m

@@ -0,0 +1,114 @@
+classdef step_block_contacts < matlab.System & matlab.system.mixin.Propagates
+    % step_block_contacts This block takes as input the joint torques and the
+    % applied external forces and evolves the state of the robot
+
+    properties (Nontunable)
+        robot_config;
+        contact_config;
+        physics_config;
+        OutputBusName = 'bus_name';
+    end
+
+    properties (DiscreteState)
+
+    end
+
+    properties (Access = {?wbs.StepBlockInit})
+        robot; contacts; state;
+    end
+
+    methods (Access = protected)
+
+        function setupImpl(obj)
+            obj.robot = wbs.Robot(obj.robot_config,obj.physics_config.GRAVITY_ACC);
+            obj.contacts = wbs.Contacts(obj.contact_config.foot_print, obj.robot, obj.contact_config.friction_coefficient, obj.physics_config.USE_QPOASES);
+            obj.state = wbs.State(obj.physics_config.TIME_STEP);
+            obj.state.set(obj.robot_config.initialConditions.w_H_b, obj.robot_config.initialConditions.s, ...
+                obj.robot_config.initialConditions.base_pose_dot, obj.robot_config.initialConditions.s_dot);
+            wbs.StepBlockInit.setSharedConfig(obj);
+        end
+
+        function [w_H_b, s, base_pose_dot, s_dot, wrench_left_foot, wrench_right_foot, kinDynOut] = stepImpl(obj, motorInertias, torque, generalized_ext_wrench)
+            % Implement algorithm. Calculate y as a function of input u and
+            % discrete states.
+
+            % computes the contact quantites and the velocity after a possible impact
+            [generalized_total_wrench, wrench_left_foot, wrench_right_foot, base_pose_dot, s_dot] = ...
+                obj.contacts.compute_contact(obj.robot, torque, generalized_ext_wrench, motorInertias, obj.state.base_pose_dot, obj.state.s_dot);
+            % sets the velocity in the state
+            obj.state.set_velocity(base_pose_dot, s_dot);
+            % compute the robot acceleration
+            [base_pose_ddot, s_ddot] = obj.robot.forward_dynamics(torque, generalized_total_wrench,motorInertias);
+            % integrate the dynamics
+            [w_H_b, s, base_pose_dot, s_dot] = obj.state.ode_step(base_pose_ddot, s_ddot);
+            % update the robot state
+            obj.robot.set_robot_state(w_H_b, s, base_pose_dot, s_dot);
+            % Get feet contact state
+            [left_foot_in_contact, right_foot_in_contact] = obj.contacts.getFeetContactState();
+
+            % output the kinematic and dynamic variables
+            kinDynOut.w_H_b = w_H_b;
+            kinDynOut.s = s;
+            kinDynOut.nu = [base_pose_dot;s_dot];
+            [kinDynOut.w_H_l_sole    , kinDynOut.w_H_r_sole    ] = obj.robot.get_feet_H();
+            [kinDynOut.J_l_sole      , kinDynOut.J_r_sole      ] = obj.robot.get_feet_jacobians();
+            [kinDynOut.JDot_l_sole_nu, kinDynOut.JDot_r_sole_nu] = obj.robot.get_feet_JDot_nu();
+            kinDynOut.M = obj.robot.get_mass_matrix(motorInertias);
+            kinDynOut.h = obj.robot.get_bias_forces();
+            kinDynOut.motorGrpI = zeros(obj.robot_config.N_DOF,1);
+            kinDynOut.fc = [wrench_left_foot;wrench_right_foot];
+            kinDynOut.nuDot = [base_pose_ddot;s_ddot];
+            kinDynOut.left_right_foot_in_contact = [left_foot_in_contact,right_foot_in_contact];
+        end
+
+        function resetImpl(obj)
+
+        end
+
+        function [out, out2, out3, out4, out5, out6, out7] = getOutputSizeImpl(obj)
+            % Return size for each output port
+            out = [4 4]; % homogeneous matrix dim
+            out2 = [double(obj.robot_config.N_DOF),1]; % joints position vector dim
+            out3 = [6 1]; % base velocity vector dim
+            out4 = [double(obj.robot_config.N_DOF),1]; % joints velocity vector dim
+            out5 = [6 1]; % wrench left foot vector dim
+            out6 = [6 1]; % wrench right foot vector dim
+            out7 = 1;
+        end
+
+        function [out, out2, out3, out4, out5, out6, out7] = getOutputDataTypeImpl(obj)
+            % Return data type for each output port
+            out = "double";
+            out2 = "double";
+            out3 = "double";
+            out4 = "double";
+            out5 = "double";
+            out6 = "double";
+            out7 = obj.OutputBusName;
+        end
+
+        function [out, out2, out3, out4, out5, out6, out7] = isOutputComplexImpl(~)
+            % Return true for each output port with complex data
+            out = false;
+            out2 = false;
+            out3 = false;
+            out4 = false;
+            out5 = false;
+            out6 = false;
+            out7 = false;
+        end
+
+        function [out, out2, out3, out4, out5, out6, out7] = isOutputFixedSizeImpl(~)
+            % Return true for each output port with fixed size
+            out = true;
+            out2 = true;
+            out3 = true;
+            out4 = true;
+            out5 = true;
+            out6 = true;
+            out7 = true;
+        end
+
+    end
+
+end