diff --git a/library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx b/library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx index fcd20b7..286344b 100644 Binary files a/library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx and b/library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx differ diff --git a/library/simulink-library/MomentumVelocityControl/src/ComputeContactJacobianWithFeetContactFCN.m b/library/simulink-library/MomentumVelocityControl/src/ComputeContactJacobianWithFeetContactFCN.m index 4fc55dd..1831848 100644 --- a/library/simulink-library/MomentumVelocityControl/src/ComputeContactJacobianWithFeetContactFCN.m +++ b/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 diff --git a/library/simulink-library/MomentumVelocityControl/src/ComputeReferences.m b/library/simulink-library/MomentumVelocityControl/src/ComputeReferences.m index c68d079..b45ce0d 100644 --- a/library/simulink-library/MomentumVelocityControl/src/ComputeReferences.m +++ b/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 diff --git a/library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m b/library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m index 1d0c927..a28ff3c 100644 --- a/library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m +++ b/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 + diff --git a/library/simulink-library/MomentumVelocityControl/src/casadi_block.m b/library/simulink-library/MomentumVelocityControl/src/casadi_block.m index 63d7e7d..f0633ed 100644 --- a/library/simulink-library/MomentumVelocityControl/src/casadi_block.m +++ b/library/simulink-library/MomentumVelocityControl/src/casadi_block.m @@ -1,5 +1,5 @@ classdef casadi_block < matlab.System & matlab.system.mixin.Propagates - % Casadi Implementation of the Momentum Based Velocity COntrol. + % Casadi Implementation of the Momentum Based Velocity COntrol. properties % Public, tunable properties. @@ -10,7 +10,7 @@ end properties (Access = private) - casadi_optimizer; + casadi_optimizer; M; h; Jc; @@ -38,12 +38,20 @@ CoM_min; CoM_vel_max; CoM_vel_min; + ActivateComHeightConstraint; + CoM_Z_reference_velocity; + feet_correction; + holonomicConstraintActivation; + JTorso; + SaturationMinJoints; + SaturationMaxJoints; + J_com; end methods (Access = protected) function num = getNumInputsImpl(~) - num = 31; + num = 36; end function num = getNumOutputsImpl(~) num = 4; @@ -57,7 +65,7 @@ end function [dt1,dt2,dt3,dt4,dt5,dt6,dt7,dt8,dt9,dt10,dt11,dt12,dt13,dt14,dt15,dt16,dt17,dt18,dt19,dt20,dt21,dt22,dt23,dt24, ... - dt25,dt26,dt27,dt28,dt29,dt30,dt31] = getInputDataTypeImpl(~) + dt25,dt26,dt27,dt28,dt29,dt30,dt31,dt32,dt33,dt34,dt35,dt36] = getInputDataTypeImpl(~) dt1 = 'double'; dt2 = 'double'; dt3 = 'double'; @@ -89,9 +97,14 @@ dt29 = 'double'; dt30 = 'double'; dt31 = 'double'; + dt32 = 'double'; + dt33 = 'double'; + dt34 = 'double'; + dt35 = 'double'; + dt36 = 'double'; end function [sz1,sz2,sz3,sz4] = getOutputSizeImpl(obj) - % Hack for avoiding hard coding the DoF of the Robot + % Hack for avoiding hard coding the DoF of the Robot size_temp = propagatedInputSize(obj,1); sz1 = [6,1]; sz2 = [size_temp(1)-6,1]; @@ -99,7 +112,7 @@ sz4 = [12,1]; end function [sz1,sz2,sz3,sz4,sz5,sz6,sz7,sz8,sz9,sz10,sz11,sz12,sz13,sz14,sz15,sz16, sz17,sz18,sz19,sz20,sz21,sz22, ... - sz23,sz24,sz25,sz26,sz27,sz28,sz29,sz30,sz31] = getInputSizeImpl(~) + sz23,sz24,sz25,sz26,sz27,sz28,sz29,sz30,sz31,sz32,sz33,sz34, sz35,sz36] = getInputSizeImpl(~) sz1 = [1,1]; sz2 = [1,1]; sz3 = [1,1]; @@ -131,10 +144,14 @@ sz29 = [1,1]; sz30 = [1,1]; sz31 = [1,1]; - + sz32 = [1,1]; + sz33 = [1,1]; + sz34 = [1,1]; + sz35 = [1,1]; + sz36 = [1,1]; end function [cp1,cp2,cp3,cp4,cp5,cp6,cp7,cp8,cp9,cp10,cp11,cp12,cp13, cp14, cp15,cp16, cp17,cp18,cp19,cp20,cp21,... - cp22,cp23,cp24,cp25,cp26,cp27,cp28,cp29,cp30,cp31] = isInputComplexImpl(~) + cp22,cp23,cp24,cp25,cp26,cp27,cp28,cp29,cp30,cp31,cp32,cp33,cp34,cp35,cp36] = isInputComplexImpl(~) cp1 = false; cp2 = false; cp3 = false; @@ -166,6 +183,11 @@ cp29 = false; cp30 = false; cp31 = false; + cp32 = false; + cp33 = false; + cp34 = false; + cp35 = false; + cp36 = false; end function [cp1,cp2,cp3,cp4] = isOutputComplexImpl(~) cp1 = false; @@ -174,7 +196,7 @@ cp4 = false; end function [fz1,fz2,fz3,fz4,fz5,fz6,fz7,fz8,fz9,fz10, fz11,fz12,fz13,fz14,fz15,fz16, fz17,fz18,fz19,fz20,fz21, ... - fz22,fz23,fz24,fz25,fz26,fz27,fz28,fz29,fz30,fz31] = isInputFixedSizeImpl(~) + fz22,fz23,fz24,fz25,fz26,fz27,fz28,fz29,fz30,fz31,fz32,fz33,fz34, fz35,fz36] = isInputFixedSizeImpl(~) fz1 = true; fz2 = true; fz3 = true; @@ -206,6 +228,11 @@ fz29 = true; fz30 = true; fz31 = true; + fz32 = true; + fz33 = true; + fz34 = true; + fz35 = true; + fz36 = true; end function [fz1,fz2,fz3,fz4] = isOutputFixedSizeImpl(~) fz1 = true; @@ -214,25 +241,19 @@ fz4 = true; end function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s_dot_star,tau_meas, contact_status, f_meas, x_com, J_com, w_H_b,... - s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF, epsilon_CoM, CoM_limits,CoM_vel_limits) + s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF, epsilon_CoM, CoM_limits,CoM_vel_limits, activateCoMHeightTask, CoMZReferenceVelocity, correctionFeet, ActivateHolonomicConstraint, JTorso) NDOF = size(K_viscous_friction,1); + obj.solver_fails_counter = 0; import casadi.* - obj.casadi_optimizer = casadi.Opti(); obj.solver_fails_counter = 0; + optimization_type = 'conic'; + solver = 'osqp'; + p_opts = struct('expand',true); + s_opts = struct(); - % Setting the solver for the optimization problem - options = struct; - options.qpsol = 'qpoases'; - options.print_header = false; - options.qpsol_options.print_problem = false; - options.qpsol_options.print_problem = false; - options.qpsol_options.print_time = false; - options.qpsol_options.sparse = true; - options.print_status = false; - options.qpsol_options.error_on_fail= false; - obj.casadi_optimizer.solver('sqpmethod', options); - + obj.casadi_optimizer = casadi.Opti(optimization_type); + obj.casadi_optimizer.solver(solver, p_opts, s_opts); %Definition of the decision variables obj.tau_k = obj.casadi_optimizer.variable(NDOF); @@ -263,27 +284,36 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s obj.CoM_vel_max = obj.casadi_optimizer.parameter(2); obj.CoM_vel_min = obj.casadi_optimizer.parameter(2); obj.epsilon_CoM = obj.casadi_optimizer.parameter(2); - + obj.ActivateComHeightConstraint = obj.casadi_optimizer.parameter(1); + obj.CoM_Z_reference_velocity = obj.casadi_optimizer.parameter(1); + obj.feet_correction = obj.casadi_optimizer.parameter(12); + obj.holonomicConstraintActivation = obj.casadi_optimizer.parameter(1); + obj.JTorso= obj.casadi_optimizer.parameter(6,NDOF+6); + obj.SaturationMaxJoints = obj.casadi_optimizer.parameter(NDOF); + obj.SaturationMinJoints = obj.casadi_optimizer.parameter(NDOF); + obj.J_com = obj.casadi_optimizer.parameter(6,NDOF+6); %Weigth - Weigth.PosturalTask = 10; + Weigth.PosturalTask = 100; Weigth.MomentumLinear = 2.0; Weigth.MomentumAngular = 30.00; Weigth.RegTorques = 0.001; - Weigth.RegVelocities = (0.01)/10; - Weigth.Wrenches = 0.0*5.; - + Weigth.RegVelocities = 128; + Weigth.Wrenches = 0.01; + Weigth.HolonomicConstraint = 10; % Selector Matrix B = [ zeros(6,NDOF); ... - eye(NDOF,NDOF)]; + eye(NDOF,NDOF)]; % Setting the objective function - obj.casadi_optimizer.minimize(Weigth.RegTorques*sumsqr(obj.tau_k - obj.tau_meas)+ ... % Torque Regularization - Weigth.RegVelocities*sumsqr(nu_k_1(1:6))+ ... % Velocity Regularization - Weigth.PosturalTask*sumsqr(obj.s_dot_k_1-obj.s_dot_star)+ ...Postural Task - Weigth.MomentumAngular*sumsqr(obj.H_star(4:6)-obj.Jcmm(4:6,:)*nu_k_1)); ... % Angular Momentum - %Weigth.MomentumLinear*sumsqr(obj.H_star(1:3)-obj.Jcmm(1:3,:)*nu_k_1)); ... % Linear Momentum + obj.casadi_optimizer.minimize(Weigth.RegTorques*sumsqr(obj.tau_k - obj.tau_meas)+ ... % Torque Regularization + Weigth.RegVelocities*sumsqr(nu_k_1(1:6))+ ... % Base Velocity Regularization + Weigth.PosturalTask*sumsqr(obj.s_dot_k_1-obj.s_dot_star)); ... % Postural Task + %Weigth.HolonomicConstraint*(1-obj.holonomicConstraintActivation)*sumsqr(obj.Jc*(nu_k_1))); ... %HolonomicCOnstraint + %Weigth.Wrenches*sumsqr(obj.f_k) + ... %Wrenches + %Weigth.MomentumAngular*sumsqr(obj.H_star(4:6)-obj.Jcmm(4:6,:)*nu_k_1)); ... % Angular Momentum + %Weigth.MomentumLinear*sumsqr(obj.H_star(1:3)-obj.Jcmm(1:3,:)*nu_k_1)); ... % Linear Momentum % Setting Constraint @@ -291,33 +321,41 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s obj.casadi_optimizer.subject_to((obj.M/obj.t_step + obj.K_friction)*(nu_k_1)-(obj.M*obj.nu_k)/obj.t_step+obj.h== B*obj.tau_k +obj.Jc'*obj.f_k); % Holonomic Constraint % acceleration-wise - % obj.casadi_optimizer.subject_to(obj.Jc*(nu_k_1-obj.nu_k)/tStep==-obj.Jc_dot_nu); + %obj.casadi_optimizer.subject_to(obj.holonomicConstraintActivation*(obj.Jc*(nu_k_1 - obj.nu_k)/obj.t_step) == obj.holonomicConstraintActivation*(-obj.Jc_dot_nu)); % velocity-wise - obj.casadi_optimizer.subject_to(obj.Jc*(nu_k_1)==zeros(12,1)); + obj.casadi_optimizer.subject_to(obj.holonomicConstraintActivation*obj.Jc*(nu_k_1)==obj.holonomicConstraintActivation*obj.feet_correction); % Friction Cones obj.casadi_optimizer.subject_to(obj.C_friction*obj.f_k(tanh(obj.epsilon_CoM*(obj.CoM_measured(1)-obj.CoM_min(1)))*obj.CoM_vel_min(1))); - obj.casadi_optimizer.subject_to(obj.Jcmm(2,:)*nu_k_1<(tanh(obj.epsilon_CoM*(obj.CoM_max(2)-obj.CoM_measured(2)))*obj.CoM_vel_max(2))); - obj.casadi_optimizer.subject_to(obj.Jcmm(2,:)*nu_k_1>(tanh(obj.epsilon_CoM*(obj.CoM_measured(2)-obj.CoM_min(2)))*obj.CoM_vel_min(2))); + + %% Constraint on the CoM Velocity + obj.casadi_optimizer.subject_to(obj.J_com(1,:)*nu_k_1<=(tanh(obj.epsilon_CoM*(obj.CoM_max(1)-obj.CoM_measured(1)))*obj.CoM_vel_max(1))); + obj.casadi_optimizer.subject_to(obj.J_com(1,:)*nu_k_1>=(tanh(obj.epsilon_CoM*(obj.CoM_measured(1)-obj.CoM_min(1)))*obj.CoM_vel_min(1))); + obj.casadi_optimizer.subject_to(obj.J_com(2,:)*nu_k_1<=(tanh(obj.epsilon_CoM*(obj.CoM_max(2)-obj.CoM_measured(2)))*obj.CoM_vel_max(2))); + obj.casadi_optimizer.subject_to(obj.J_com(2,:)*nu_k_1>=(tanh(obj.epsilon_CoM*(obj.CoM_measured(2)-obj.CoM_min(2)))*obj.CoM_vel_min(2))); + % Constraint the Torso velocity on the z axis + %obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.Jcmm(3,7:end)*nu_k_1(7:end) >= obj.ActivateComHeightConstraint*obj.M(1,1)*obj.CoM_Z_reference_velocity); + obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.JTorso(3,:)*nu_k_1 >= obj.ActivateComHeightConstraint*obj.CoM_Z_reference_velocity); +% + % Angular Momentum + % With Control Law + % obj.casadi_optimizer.subject_to(obj.H_star(4:6,:)==obj.Jcmm(4:6,:)*nu_k_1); + % Setting to Zero + %obj.casadi_optimizer.subject_to(obj.Jcmm(4:6,:)*nu_k_1 == zeros(3,1)); %THE BOUND FOR NOW ARE DE-ACTIVATED % Setting upper and lower bound % Lower and Upper Bound NOTE by using a casadi optimizer object, we loose the capability of interpreting the lower and % uppe bound in a smart way. % obj.casadi_optimizer.subject_to(Sat.BaseLinearVelocity_min<= obj.v_b_k_1(1:3)<=Sat.BaseLinearVelocity_max); % obj.casadi_optimizer.subject_to(Sat.BaseAngVelocity_min<= obj.v_b_k_1(4:end)<=Sat.BaseAngVelocity_max); - % obj.casadi_optimizer.subject_to(Sat.JointsVelocity_min<= obj.s_dot_k_1<= Sat.JointsVelocity_max); + %obj.casadi_optimizer.subject_to(obj.SaturationMinJoints<= obj.s_dot_k_1); + %obj.casadi_optimizer.subject_to(obj.s_dot_k_1<= obj.SaturationMaxJoints); % obj.casadi_optimizeru.sbject_to(Sat.torques_min(1:3)<= obj.tau_k(1:3)<= Sat.torques_max(1:3)); end function [v_b_star,s_dot_k_1_star,tau_star,f_star] = stepImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s_dot_star,tau_meas, contact_status, f_meas, x_com, J_com, w_H_b,... - s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF,epsilon_CoM, CoM_limits,CoM_vel_limits) + s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF,epsilon_CoM, CoM_limits,CoM_vel_limits, activateCoMHeightTask, CoMZReferenceVelocity, correctionFeet, activateHolonomicConstraint, JTorso ) solver_succeded = true; % Compute Centroidayl Dynamics @@ -368,6 +406,14 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s obj.casadi_optimizer.set_value(obj.CoM_vel_max, CoM_vel_limits(:,2)); obj.casadi_optimizer.set_value(obj.CoM_vel_min, CoM_vel_limits(:,1)); obj.casadi_optimizer.set_value(obj.epsilon_CoM, epsilon_CoM); + obj.casadi_optimizer.set_value(obj.ActivateComHeightConstraint,activateCoMHeightTask); + obj.casadi_optimizer.set_value(obj.CoM_Z_reference_velocity, CoMZReferenceVelocity); + obj.casadi_optimizer.set_value(obj.feet_correction, correctionFeet); + obj.casadi_optimizer.set_value(obj.holonomicConstraintActivation, activateHolonomicConstraint); + obj.casadi_optimizer.set_value(obj.JTorso,JTorso); + obj.casadi_optimizer.set_value(obj.J_com,J_com); +% obj.casadi_optimizer.set_value(obj.SaturationMaxJoints,jointsVelocitySat(:,2)); +% obj.casadi_optimizer.set_value(obj.SaturationMinJoints, jointsVelocitySat(:,1)); % Computing the solution try