Defined friction coeff per joint, setup YOGA balancing only, RobotVis…

…ualizer at 10ms

- Tuning tests 1 to 3 (Refer to #121)
- Tuning tests 4,5,6
    Controller Motor reflected inertia ON/OFF / Harmonic Drive inertia ON/OFF / Coupling ON/OFF
    Simulator Motor reflected inertia ON
    Simulator Friction = 0.2

controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m

@@ -153,7 +153,34 @@
 
 motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m);
 
-KvmechMat = diag(0.5*ones(1,robot_config.N_DOF));
+% Joint friction
+
+% === Mapping ===
+jointDefaultOrder = {...
+    'torso_pitch','torso_roll','torso_yaw', ...
+    'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow', ...
+    'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow', ...
+    'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll', ...
+    'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
+
+KvmechMappingTorso = 0.2*ones(3,1);
+KvmechMappingLeftArm = 0.2*ones(4,1);
+KvmechMappingRightArm = 0.2*ones(4,1);
+KvmechMappingLeftLeg = [0.2 0.2 0.2 0.2 0.2 0.2];
+KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.2 0.2];
+
+KvmechMapping = containers.Map(...
+    jointDefaultOrder, ...
+    [
+    KvmechMappingTorso
+    KvmechMappingLeftArm
+    KvmechMappingRightArm
+    KvmechMappingLeftLeg
+    KvmechMappingRightLeg
+    ]);
+
+KvmechMat = diag(cell2mat(KvmechMapping.values(robot_config.jointOrder)));
+
 jointFrictionMat = wbc.computeMotorsReflectedInertia(eye(robot_config.N_DOF),T,KvmechMat);
 
 %% size of the square you see around the robot

controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m

@@ -48,7 +48,7 @@
 StateMachine.initialState                 = 1;
 
 % other configuration parameters for state machine
-StateMachine.tBalancing                   = 1;
+StateMachine.tBalancing                   = 1000;
 StateMachine.tBalancingBeforeYoga         = 1;
 StateMachine.yogaExtended                 = true;
 StateMachine.skipYoga                     = false;

controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m

@@ -9,7 +9,7 @@
 confVisualizer.aroundRobot = 1; % [m]
 
 % refresh rate of the picure
-confVisualizer.tStep = 0.040; % here equal to the time step used in the simulink model
+confVisualizer.tStep = 0.010; % here equal to the time step used in the simulink model
 
 
 %% Parameters copied from robot_config