Skip to content

Commit ebb2aa4

Browse files
CarlottaSartoreCarlottaSartore
committed
modifications for contact switching in velocity controller
1 parent a03caae commit ebb2aa4

File tree

4 files changed

+22
-16
lines changed

4 files changed

+22
-16
lines changed

‎library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx

-79.5 KB
Binary file not shown.

‎library/simulink-library/MomentumVelocityControl/src/ComputeReferences.m

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,11 @@
1-
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)
1+
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)
22

33
L_int_lin = Gains.KP_CoM*totalMass*(x_com - x_d_com);
44
L_int_ang = Gains.KI_ang*(int_L_angular);
55
L_int = [L_int_lin; L_int_ang];
66
L_star = Ld-L_int;
77

8-
L_dot_star = L_dot_d -blkdiag(Gains.KD_CoM, Gains.KP_ang)*(L-Ld) -L_int;
9-
10-
s_dot_star_k_1 = s_dot_des_k_1 - Gains.KP_Postural*(s_k-s_des_k);
11-
8+
L_dot_star = L_dot_d -blkdiag(Gains.KD_CoM, Gains.KP_ang)*(L-Ld) -L_int;
9+
s_dot_star_k_1 = s_dot_des_k_1 - KpPostural*(s_k-s_des_k);
10+
%s_dot_star_k_1 = s_dot_des_k_1 - Gains.KP_Postural*(s_k-s_des_k);
1211
end

‎library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,12 +68,15 @@
6868
if(~Cs_old(1) && Cs(1))
6969
w_H_l_0(1,4) = w_H_l(1,4);
7070
w_H_l_0(2,4) = w_H_l(2,4);
71+
w_H_l_0(3,4) = z_0;
7172
end
7273

7374
% Updating only the position, we want to keep the original orientation
7475
if(~Cs_old(2) && Cs(2))
7576
w_H_r_0(1,4) = w_H_r(1,4);
76-
w_H_r_0(2,4) = w_H_r(2,4);
77+
w_H_r_0(2,4) = w_H_r(2,4);
78+
w_H_r_0(1:3,1:3)
79+
w_H_r_0(3,4) = z_0;
7780
end
7881

7982
vel_feet_correction = zeros(12,1);

‎library/simulink-library/MomentumVelocityControl/src/casadi_block.m

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@
4545
JTorso;
4646
SaturationMinJoints;
4747
SaturationMaxJoints;
48+
J_com;
49+
4850
end
4951

5052
methods (Access = protected)
@@ -289,15 +291,16 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s
289291
obj.JTorso= obj.casadi_optimizer.parameter(6,NDOF+6);
290292
obj.SaturationMaxJoints = obj.casadi_optimizer.parameter(NDOF);
291293
obj.SaturationMinJoints = obj.casadi_optimizer.parameter(NDOF);
294+
obj.J_com = obj.casadi_optimizer.parameter(6,NDOF+6);
292295

293296
%Weigth
294297
Weigth.PosturalTask = 100;
295298
Weigth.MomentumLinear = 2.0;
296299
Weigth.MomentumAngular = 30.00;
297300
Weigth.RegTorques = 0.001;
298-
Weigth.RegVelocities = 10;
301+
Weigth.RegVelocities = 128;
299302
Weigth.Wrenches = 0.01;
300-
Weigth.HolonomicConstraint = 0.0001;
303+
Weigth.HolonomicConstraint = 10;
301304
% Selector Matrix
302305

303306
B = [ zeros(6,NDOF); ...
@@ -307,8 +310,8 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s
307310
obj.casadi_optimizer.minimize(Weigth.RegTorques*sumsqr(obj.tau_k - obj.tau_meas)+ ... % Torque Regularization
308311
Weigth.RegVelocities*sumsqr(nu_k_1(1:6))+ ... % Base Velocity Regularization
309312
Weigth.PosturalTask*sumsqr(obj.s_dot_k_1-obj.s_dot_star)); ... % Postural Task
313+
%Weigth.HolonomicConstraint*(1-obj.holonomicConstraintActivation)*sumsqr(obj.Jc*(nu_k_1))); ... %HolonomicCOnstraint
310314
%Weigth.Wrenches*sumsqr(obj.f_k) + ... %Wrenches
311-
%Weigth.HolonomicConstraint*sumsqr(obj.Jc*(nu_k_1))); ... %HolonomicCOnstraint
312315
%Weigth.MomentumAngular*sumsqr(obj.H_star(4:6)-obj.Jcmm(4:6,:)*nu_k_1)); ... % Angular Momentum
313316
%Weigth.MomentumLinear*sumsqr(obj.H_star(1:3)-obj.Jcmm(1:3,:)*nu_k_1)); ... % Linear Momentum
314317

@@ -325,20 +328,20 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s
325328
obj.casadi_optimizer.subject_to(obj.C_friction*obj.f_k<obj.b_friction);
326329

327330
%% Constraint on the CoM Velocity
328-
obj.casadi_optimizer.subject_to(obj.Jcmm(1,:)*nu_k_1<(tanh(obj.epsilon_CoM*(obj.CoM_max(1)-obj.CoM_measured(1)))*obj.CoM_vel_max(1)));
329-
obj.casadi_optimizer.subject_to(obj.Jcmm(1,:)*nu_k_1>(tanh(obj.epsilon_CoM*(obj.CoM_measured(1)-obj.CoM_min(1)))*obj.CoM_vel_min(1)));
330-
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)));
331-
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)));
331+
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)));
332+
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)));
333+
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)));
334+
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)));
332335

333336
% Constraint the Torso velocity on the z axis
334-
%obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.Jcmm(3,:)*nu_k_1 >= obj.ActivateComHeightConstraint*obj.M(1,1)*obj.CoM_Z_reference_velocity);
337+
%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);
335338
obj.casadi_optimizer.subject_to(obj.ActivateComHeightConstraint*obj.JTorso(3,:)*nu_k_1 >= obj.ActivateComHeightConstraint*obj.CoM_Z_reference_velocity);
336-
339+
%
337340
% Angular Momentum
338341
% With Control Law
339342
% obj.casadi_optimizer.subject_to(obj.H_star(4:6,:)==obj.Jcmm(4:6,:)*nu_k_1);
340343
% Setting to Zero
341-
obj.casadi_optimizer.subject_to(obj.Jcmm(4:6,:)*nu_k_1 == zeros(3,1));
344+
%obj.casadi_optimizer.subject_to(obj.Jcmm(4:6,:)*nu_k_1 == zeros(3,1));
342345
%THE BOUND FOR NOW ARE DE-ACTIVATED
343346
% Setting upper and lower bound
344347
% Lower and Upper Bound NOTE by using a casadi optimizer object, we loose the capability of interpreting the lower and
@@ -408,6 +411,7 @@ function setupImpl(obj,M,h,Jc,Jcmm,nu_k,Jc_dot_nu,C_friction,b_friction,H_star,s
408411
obj.casadi_optimizer.set_value(obj.feet_correction, correctionFeet);
409412
obj.casadi_optimizer.set_value(obj.holonomicConstraintActivation, activateHolonomicConstraint);
410413
obj.casadi_optimizer.set_value(obj.JTorso,JTorso);
414+
obj.casadi_optimizer.set_value(obj.J_com,J_com);
411415
% obj.casadi_optimizer.set_value(obj.SaturationMaxJoints,jointsVelocitySat(:,2));
412416
% obj.casadi_optimizer.set_value(obj.SaturationMinJoints, jointsVelocitySat(:,1));
413417

0 commit comments

Comments
 (0)