diff --git a/init.m b/init.m index 02a6684..b31c404 100644 --- a/init.m +++ b/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; diff --git a/lib/+wbs/@Contacts/Contacts.m b/lib/+wbs/@Contacts/Contacts.m index c684947..f4d7e9e 100644 --- a/lib/+wbs/@Contacts/Contacts.m +++ b/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 diff --git a/lib/+wbs/@StepBlockInit/StepBlockInit.m b/lib/+wbs/@StepBlockInit/StepBlockInit.m new file mode 100644 index 0000000..752beaa --- /dev/null +++ b/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 diff --git a/lib/RobotDynamicsWithContacts/+robotDynWC/ProcessQPOutputFCN.m b/lib/RobotDynamicsWithContacts/+robotDynWC/ProcessQPOutputFCN.m new file mode 100644 index 0000000..b06e2ef --- /dev/null +++ b/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 diff --git a/lib/RobotDynamicsWithContacts/+robotDynWC/step_block.m b/lib/RobotDynamicsWithContacts/+robotDynWC/step_block.m index cebfca0..c89d426 100644 --- a/lib/RobotDynamicsWithContacts/+robotDynWC/step_block.m +++ b/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) diff --git a/lib/RobotDynamicsWithContacts/+robotDynWC/step_block_contacts.m b/lib/RobotDynamicsWithContacts/+robotDynWC/step_block_contacts.m new file mode 100644 index 0000000..e138256 --- /dev/null +++ b/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 diff --git a/lib/RobotDynamicsWithContacts/robotDynamicsWithContacts_lib.slx b/lib/RobotDynamicsWithContacts/robotDynamicsWithContacts_lib.slx index 04dd22d..fab00f3 100644 Binary files a/lib/RobotDynamicsWithContacts/robotDynamicsWithContacts_lib.slx and b/lib/RobotDynamicsWithContacts/robotDynamicsWithContacts_lib.slx differ