From 8e44679d86a37dacc840645cb90c2adb83dbe5a9 Mon Sep 17 00:00:00 2001 From: Nuno Guedelha Date: Tue, 4 May 2021 05:39:31 +0200 Subject: [PATCH] Defined friction coeff per joint, setup YOGA balancing only, RobotVisualizer at 10ms - Tuning tests 1 to 3 (Refer to https://github.com/robotology/whole-body-controllers/issues/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 --- .../robots/iCubGazeboV2_5/configRobotSim.m | 29 ++++++++++++++++++- .../iCubGazeboV2_5/configStateMachine.m | 2 +- .../src/initVisualizer.m | 2 +- 3 files changed, 30 insertions(+), 3 deletions(-) diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m index 6a5ea83..ea702f5 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m +++ b/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 diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m b/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m index bace98b..608d226 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m +++ b/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; diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m b/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m index 4615d4f..8d7d8ae 100644 --- a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m +++ b/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