Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Velocity Controller-Relax Momentum Constraint #107

Merged
merged 4 commits into from
Feb 11, 2021
Merged

Velocity Controller-Relax Momentum Constraint #107

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1575137
fixes centroidalTransformaion and ComputeReferencesController
CarlottaSartore Feb 8, 2021
bb58266
fixed some typos, removed hardcoded variables, clean up
CarlottaSartore Feb 8, 2021
ee1654a
fixed indeces for velocity limits
CarlottaSartore Feb 10, 2021
c194285
minor fixes
CarlottaSartore Feb 10, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file modified library/simulink-library/MomentumVelocityControl/momentumVelocityControl_lib.slx
View file
Open in desktop
Binary file not shown.
14 changes: 5 additions & 9 deletions library/simulink-library/MomentumVelocityControl/src/ComputeReferencesController.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
function [L_d, 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, 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;

Expand All @@ -12,18 +12,14 @@
joint_pos_0 = joint_pos;
end


%% for balancing
%L_d = L_0;

% L_d = zeros(6,1);
%L_d(4:6)= L_ang_des;
L_d_n = zeros(6,1);
L_dot_d = zeros(6,1);
L_d(1:3) = M(1,1)*desired_pos_vel_COM(:,2)';
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 = zeros(size(s_des_k));
s_dot_des_k = joint_vel_desired;


end
16 changes: 16 additions & 0 deletions library/simulink-library/MomentumVelocityControl/src/SupportPolygonWRTContactStatusFCN.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
% This function output the support polygon depending on the current contact
% status

function CoM_limits = SupportPolygonWRTContactStatusFCN(contactStatus,Config)
CoM_limits = Config.CoM_limit_double;

if(and(contactStatus(1),contactStatus(2))) % Double Support
CoM_limits = Config.CoM_limit_double;
elseif(and(~contactStatus(1),~contactStatus(2))) % Flying phase
CoM_limits = Config.CoM_limit_double;
elseif(contactStatus(1)) % Left Foot contact
CoM_limits =Config.CoM_limit_left;
elseif(contactStatus(2)) % Right Foot contact
CoM_limits = Config.CoM_limit_right;
end
end
216 changes: 126 additions & 90 deletions library/simulink-library/MomentumVelocityControl/src/casadi_block.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
classdef casadi_block < matlab.System & matlab.system.mixin.Propagates
% untitled Add summary here
%
% This template includes the minimum set of functions required
% to define a System object with discrete state.
% Casadi Implementation of the Momentum Based Velocity COntrol.

properties
% Public, tunable properties.
Expand All @@ -13,32 +10,40 @@
end

properties (Access = private)
% Pre-computed constants.
casadi_optimizer
M
h
Jc
Jcmm
nu_k
C_friction
b_friction
s_dot_star
H_star
tau_k
s_dot_k_1
v_b_k_1
f_k
Jc_dot_nu
sol
casadi_optimizer;
M;
h;
Jc;
Jcmm;
nu_k;
C_friction;
b_friction;
s_dot_star;
H_star;
tau_k;
s_dot_k_1;
v_b_k_1;
f_k;
Jc_dot_nu;
sol;
tau_meas;
f_meas;
solver_fails_counter;
s
s;
CoM_measured;
K_friction;
t_step;
epsilon_CoM;
CoM_max;
CoM_min;
CoM_vel_max;
CoM_vel_min;

end

methods (Access = protected)
function num = getNumInputsImpl(~)
num = 27;
num = 31;
end
function num = getNumOutputsImpl(~)
num = 4;
Expand All @@ -52,7 +57,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] = getInputDataTypeImpl(~)
dt25,dt26,dt27,dt28,dt29,dt30,dt31] = getInputDataTypeImpl(~)
dt1 = 'double';
dt2 = 'double';
dt3 = 'double';
Expand Down Expand Up @@ -80,15 +85,21 @@
dt25 = 'double';
dt26 = 'double';
dt27 = 'double';
dt28 = 'double';
dt29 = 'double';
dt30 = 'double';
dt31 = 'double';
end
function [sz1,sz2,sz3,sz4] = getOutputSizeImpl(~)
function [sz1,sz2,sz3,sz4] = getOutputSizeImpl(obj)
% Hack for avoiding hard coding the DoF of the Robot
size_temp = propagatedInputSize(obj,1);
sz1 = [6,1];
sz2 = [23,1];
sz3 = [23,1];
sz2 = [size_temp(1)-6,1];
sz3 = [size_temp(1)-6,1];
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] = getInputSizeImpl(~)
sz23,sz24,sz25,sz26,sz27,sz28,sz29,sz30,sz31] = getInputSizeImpl(~)
sz1 = [1,1];
sz2 = [1,1];
sz3 = [1,1];
Expand Down Expand Up @@ -116,9 +127,14 @@
sz25 = [1,1];
sz26 = [1,1];
sz27 = [1,1];
sz28 = [1,1];
sz29 = [1,1];
sz30 = [1,1];
sz31 = [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] = isInputComplexImpl(~)
cp22,cp23,cp24,cp25,cp26,cp27,cp28,cp29,cp30,cp31] = isInputComplexImpl(~)
cp1 = false;
cp2 = false;
cp3 = false;
Expand Down Expand Up @@ -146,6 +162,10 @@
cp25 = false;
cp26 = false;
cp27 = false;
cp28 = false;
cp29 = false;
cp30 = false;
cp31 = false;
end
function [cp1,cp2,cp3,cp4] = isOutputComplexImpl(~)
cp1 = false;
Expand All @@ -154,7 +174,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] = isInputFixedSizeImpl(~)
fz22,fz23,fz24,fz25,fz26,fz27,fz28,fz29,fz30,fz31] = isInputFixedSizeImpl(~)
fz1 = true;
fz2 = true;
fz3 = true;
Expand Down Expand Up @@ -182,60 +202,25 @@
fz25 = true;
fz26 = true;
fz27 = true;
fz28 = true;
fz29 = true;
fz30 = true;
fz31 = true;
end
function [fz1,fz2,fz3,fz4] = isOutputFixedSizeImpl(~)
fz1 = true;
fz2 = true;
fz3 = true;
fz4 = true;
end
function setupImpl(obj,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF)

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)

NDOF = size(K_viscous_friction,1);
import casadi.*
obj.casadi_optimizer = casadi.Opti();

obj.solver_fails_counter = 0;

%%TODO this should be conf variable not hardcoded


%K friction
K_friction = zeros(NDOF+6);

if(ADD_FRICTION)

invTGamma = eye(size(Gamma))/(T*Gamma);
invTGamma_t = eye(size(Gamma))/(transpose(T*Gamma));
K_friction(7:end, 7:end) = invTGamma_t*K_viscous_friction*invTGamma;
end

%% Saturation Min PureQP
Sat.BaseLinearVelocity_min = baseVelocitySat(1:3,1);
Sat.BaseAngVelocity_min = baseVelocitySat(4:6,1);
Sat.JointsVelocity_min = jointsVelocitySat(:,1);
Sat.torques_min = torqueSat(:,1);
Sat.wrenches_min = wrenchesSat(:,1);
%% Saturation Max PureQP
Sat.BaseLinearVelocity_max = baseVelocitySat(1:3,2);
Sat.BaseAngVelocity_max = baseVelocitySat(4:6,2);
Sat.JointsVelocity_max = jointsVelocitySat(:,2);
Sat.torques_max = torqueSat(:,2);
Sat.wrenches_max = wrenchesSat(:,1);

%Weigth
Weigth.PosturalTask = 6;
% Weigth.MomentumLinear = 2.0;
Weigth.MomentumAngular = 3.00;
Weigth.RegTorques = 0.001;
Weigth.RegVelocities = (0.01)/10;
Weigth.Wrenches = 0.0*5.;

% Selector Matrix

B = [ zeros(6,NDOF); ...
eye(NDOF,NDOF)];

% Setting the solver for the optimization problem
options = struct;
options.qpsol = 'qpoases';
Expand Down Expand Up @@ -270,35 +255,69 @@ function setupImpl(obj,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,ADD_FRICTION, K_viscous_f
obj.tau_meas = obj.casadi_optimizer.parameter(NDOF);
obj.f_meas = obj.casadi_optimizer.parameter(12);
obj.s = obj.casadi_optimizer.parameter(NDOF);
obj.CoM_measured = obj.casadi_optimizer.parameter(3);
obj.K_friction = obj.casadi_optimizer.parameter(NDOF+6,NDOF+6);
obj.t_step = obj.casadi_optimizer.parameter(1);
obj.CoM_max = obj.casadi_optimizer.parameter(2);
obj.CoM_min = obj.casadi_optimizer.parameter(2);
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);


%Weigth
Weigth.PosturalTask = 10;
Weigth.MomentumLinear = 2.0;
Weigth.MomentumAngular = 30.00;
Weigth.RegTorques = 0.001;
Weigth.RegVelocities = (0.01)/10;
Weigth.Wrenches = 0.0*5.;

% Selector Matrix

B = [ zeros(6,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

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


% Setting Constraint
% Dynamics
obj.casadi_optimizer.subject_to((obj.M/tStep + K_friction)*(nu_k_1)-(obj.M*obj.nu_k)/tStep+obj.h== B*obj.tau_k +obj.Jc'*obj.f_k);
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
obj.casadi_optimizer.subject_to(obj.Jc*(nu_k_1-obj.nu_k)/tStep==-obj.Jc_dot_nu);
% acceleration-wise
% obj.casadi_optimizer.subject_to(obj.Jc*(nu_k_1-obj.nu_k)/tStep==-obj.Jc_dot_nu);
% velocity-wise
obj.casadi_optimizer.subject_to(obj.Jc*(nu_k_1)==zeros(12,1));
% Friction Cones
obj.casadi_optimizer.subject_to(obj.C_friction*obj.f_k<obj.b_friction);
% Linear Momentum
obj.casadi_optimizer.subject_to(obj.H_star(1:3)==obj.Jcmm(1:3,:)*nu_k_1);

% Angular Momentum
%obj.casadi_optimizer.subject_to(obj.H_star(4:6,:)==obj.Jcmm(4:6,:)*nu_k_1);

% Constraint on the CoM Velocity
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)));
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)));
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)));

%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_optimizeru.sbject_to(Sat.torques_min(1:3)<= obj.tau_k(1:3)<= Sat.torques_max(1:3));

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(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,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF)
s,CoM_measured,ADD_FRICTION, K_viscous_friction, T, Gamma, baseVelocitySat, jointsVelocitySat, torqueSat, wrenchesSat, tStep, NDOF,epsilon_CoM, CoM_limits,CoM_vel_limits)
solver_succeded = true;

% Compute Centroidayl Dynamics
Expand All @@ -318,6 +337,15 @@ function setupImpl(obj,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,ADD_FRICTION, K_viscous_f
STATE.x_b = w_H_b(1:3,4);
centroidalDyn = centroidalConversion(DYNAMICS,FORKINEMATICS,STATE);

%K friction
K_friction_new = zeros(NDOF+6);

if(ADD_FRICTION)
invTGamma = eye(size(Gamma))/(T*Gamma);
invTGamma_t = eye(size(Gamma))/(transpose(T*Gamma));
K_friction_new(7:end, 7:end) = invTGamma_t*K_viscous_friction*invTGamma;
end

% Update casadi optimizer parameters
obj.casadi_optimizer.set_value(obj.M, centroidalDyn.M);
obj.casadi_optimizer.set_value(obj.h,( centroidalDyn.C_nu+centroidalDyn.g));
Expand All @@ -332,10 +360,18 @@ function setupImpl(obj,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,ADD_FRICTION, K_viscous_f
obj.casadi_optimizer.set_value(obj.tau_meas, tau_meas);
obj.casadi_optimizer.set_value(obj.f_meas, f_meas);
obj.casadi_optimizer.set_value(obj.s,s);
obj.casadi_optimizer.set_value(obj.CoM_measured,CoM_measured)
obj.casadi_optimizer.set_value(obj.K_friction,K_friction_new);
obj.casadi_optimizer.set_value(obj.t_step,tStep);
obj.casadi_optimizer.set_value(obj.CoM_max, CoM_limits(:,2));
obj.casadi_optimizer.set_value(obj.CoM_min, CoM_limits(:,1));
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);

% Computing the solution
try
obj.sol = obj.casadi_optimizer.solve; % raises error
obj.sol = obj.casadi_optimizer.solve; % could raise an error
v_b_star = full(obj.sol.value(obj.v_b_k_1));
s_dot_k_1_star = full(obj.sol.value(obj.s_dot_k_1));
tau_star = full(obj.sol.value(obj.tau_k));
Expand All @@ -357,8 +393,8 @@ function setupImpl(obj,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,ADD_FRICTION, K_viscous_f
if(~solver_succeded)
obj.solver_fails_counter = obj.solver_fails_counter +1;
v_b_star = zeros(6,1);
s_dot_k_1_star = zeros(23,1);
tau_star = zeros(23,1);
s_dot_k_1_star = zeros(26,1);
tau_star = zeros(26,1);
f_star = zeros(12,1);
if(obj.solver_fails_counter>4)
error('solver failed more that 4 times');
Expand Down
1 change: 0 additions & 1 deletion library/simulink-library/MomentumVelocityControl/src/centroidalConversion.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,4 +41,3 @@
centroidalDyn.Jc = Jc_c;

end

Loading