torquebalancing is connected with mpc and working in simulation

controllers/torqueBalancing/app/robots/icubGazeboSim/initCoordinator.m → controllers/torqueBalancingFootstepPlanner/app/robots/icubGazeboSim/initCoordinator.m

@@ -19,7 +19,7 @@
 references.smoothingTimeMinJerkComDesQDes = 3.0;
 CONFIG.smoothingTimeTranDynamics          = 0.05;
 
-sat.torque = 34;
+sat.torque = 60;
 
 ROBOT_DOF_FOR_SIMULINK = eye(ROBOT_DOF);
 gain.qTildeMax         = 20*pi/180;

controllers/torqueBalancing/initTorqueBalancing.m → controllers/torqueBalancingFootstepPlanner/initTorqueBalancing.m

@@ -71,6 +71,7 @@
 % CONFIG.CHECK_LIMITS: if set to true, the controller will stop as soon as 
 % any of the joint limit is touched. 
 CONFIG.CHECK_LIMITS          = false;
+CONFIG.CHECK_PORTS_WALKING   = true;
 
 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % CONFIGURATIONS COMPLETED: loading gains and parameters for the specific robot
@@ -116,11 +117,15 @@
 % inequality constraints of contact wrenches
 CONFIG.USE_QP_SOLVER     = true; 
 
+% Ports from footstep planner
+PORTS.COM_DES         = '/walking-coordinator/com:o';
+PORTS.Q_DES           = '/walking-coordinator/joints:o';
+PORTS.LFOOT_DES       = '/walking-coordinator/leftFoot:o';
+PORTS.RFOOT_DES       = '/walking-coordinator/rightFoot:o';
+PORTS.ACTIVE_CONTACTS = '/walking-coordinator/contact:o';
+PORTS.LFOOT_IS_FIXED  = '/walking-coordinator/leftStanding:o';
+
 PORTS.IMU                = '/icub/inertial';
-PORTS.COM_DES            = ['/' WBT_modelName '/comDes:i'];
-PORTS.Q_DES              = ['/' WBT_modelName '/qDes:i'];
-PORTS.ACTIVE_CONTACTS    = ['/' WBT_modelName '/constraints:i'];
-PORTS.BASE_POSE          = ['/' WBT_modelName '/basePose:i'];
 PORTS.WBD_LEFTLEG_EE     = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o';
 PORTS.WBD_RIGHTLEG_EE    = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o';
 
@@ -163,9 +168,3 @@
 %% Generate contact constraint model
 [ConstraintsMatrix,bVectorConstraints] = constraints(forceFrictionCoefficient,numberOfPoints,torsionalFrictionCoefficient,gain.footSize,fZmin);
 
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%% [TO BE REMOVED WHEN FULLY OPERATIVE] TEST PLANNER CONFIGURATION
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-PORTS.LEFT_FOOT_IN_CONTACT = ['/' WBT_modelName '/leftInContact:i'];
-run(fullfile('tests/getJointPosFromTxt.m'));
-

controllers/torqueBalancingFootstepPlanner/src/fromQuatToTransfMatrix.m

@@ -0,0 +1,18 @@
+function w_H_fixedLink = fromQuatToTransfMatrix(qt_fixed_LFoot, LFoot_is_active, qt_fixed_RFoot)
+
+
+    q    = qt_fixed_RFoot;
+    toll = 0.1;
+
+    if LFoot_is_active > (1-toll)
+
+        q = qt_fixed_LFoot;
+    end
+
+    % transformation matrix from position + quaternions rapresentation
+    w_R_fixedLink = rotationFromQuaternion(q(4:7));
+
+    w_H_fixedLink = [w_R_fixedLink, q(1:3);
+                       0   0   0    1];
+
+end

controllers/torqueBalancingFootstepPlanner/src/stateMachineWalking.m

@@ -0,0 +1,50 @@
+function [CoMDes, impedances, kpCom, kdCom, currentState] = ...
+             stateMachineWalking(wrench_rightFoot, wrench_leftFoot, CoM_0, q0, x_CoM, qj, t, sm, gain,...
+                                 CoMDesInput, qjDesInput, constraintsInput)
+
+    % state machine for teleoperated walking
+    persistent state;
+
+    if isempty(state) 
+        state         = sm.stateAt0;
+    end
+
+    impedances  = gain.impedances(1,:);
+    kpCom       = gain.PCOM(1,:);   
+    kdCom       = gain.DCOM(1,:); 
+    toll        = 0.1;
+
+    %% TWO FEET BALANCING
+    if constraintsInput(1)>(1-toll) && constraintsInput(2)>(1-toll)
+
+        % nothing to do actually. Just wait to switch in another state
+        state = 1;
+    end
+
+    %% LEFT FOOT BALANCING 
+    if constraintsInput(1)>(1-toll) && constraintsInput(2)<(toll)
+
+        % update gains
+        impedances  = gain.impedances(state,:);
+        kpCom       = gain.PCOM(state,:);   
+        kdCom       = gain.DCOM(state,:); 
+        state       = 2;
+    end
+
+    %% RIGHT FOOT BALANCING 
+    if constraintsInput(1)<(toll) && constraintsInput(2)>(1-toll)
+
+        % update gains
+        impedances  = gain.impedances(state,:);
+        kpCom       = gain.PCOM(state,:);   
+        kdCom       = gain.DCOM(state,:);   
+        state       = 3;
+    end
+
+    %% The following variables actually does not depend upon the current state
+    CoMDes       = [CoMDesInput(1:3),CoMDesInput(4:6),CoMDesInput(7:9)];
+
+    %% Current state and smoothing time
+    currentState = state;
+
+end