Integrate the YOGA++ (floating-base-balancing-torque-control) with the matlab-whole-body-simulator

controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt

@@ -0,0 +1 @@
+register_mdl(MODELNAME "torqueControlBalancingWithSimu")

controllers/floating-base-balancing-torque-control-with-simulator/README.md

@@ -0,0 +1,29 @@
+## Module description
+
+This module implements a torque control balancing strategy. It computes the interaction forces at the feet in order to stabilise a desired `centroidal momentum` dynamics, which implies the tracking of a desired center-of-mass trajectory. A cost function penalizing high joint torques - that generate the feet forces - is added to the control framework.
+
+<img src="/doc/pics/torqueControl.png" width="1800"> 
+
+For details see also: [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and [Stability Analysis and Design of Momentum-Based Controllers for Humanoid Robots](https://ieeexplore.ieee.org/document/7759126).
+
+### Compatibility
+
+The folder contains the Simulink model `torqueControlBalancing.mdl`, which is generated by using Matlab R2017b.
+
+### Supported robots
+
+Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`.
+
+## Module details
+
+### How to run the demo
+
+For information on how to use the controllers both in **simulation** and with the **real robot**, please refer to the **wiki** of the repo.
+
+### Configuration file
+
+At start, the module calls the initialization file initTorqueControlBalancing.m. Once opened, this file contains some configuration variables. Please follow the instruction inside the script to properly configure your simulation.
+
+### Robot and demo specific configurations
+
+The gains and references for a specific robot (specified by the variable YARP_ROBOT_NAME) or a specific demo can be found in the folder `app/robots/YARP_ROBOT_NAME`.

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

@@ -0,0 +1,103 @@
+% CONFIGROBOT initializes parameters specific of a particular robot
+%             (e.g., icubGazeboSim)
+
+%% --- Initialization ---
+
+Config.ON_GAZEBO         = true;
+ROBOT_DOF                = 23;
+Config.GRAV_ACC          = 9.81;
+
+% Robot configuration for WBToolbox
+WBTConfigRobot           = WBToolbox.Configuration;
+WBTConfigRobot.RobotName = 'icubSim';
+WBTConfigRobot.UrdfFile  = 'model.urdf';
+WBTConfigRobot.LocalName = 'WBT';
+
+% Controlboards and joints list. Each joint is associated to the corresponding controlboard 
+WBTConfigRobot.ControlBoardsNames     = {'torso','left_arm','right_arm','left_leg','right_leg'};
+WBTConfigRobot.ControlledJoints       = [];
+Config.numOfJointsForEachControlboard = [];
+
+ControlBoards                                        = struct();
+ControlBoards.(WBTConfigRobot.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'};
+ControlBoards.(WBTConfigRobot.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'};
+ControlBoards.(WBTConfigRobot.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'};
+ControlBoards.(WBTConfigRobot.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'};
+ControlBoards.(WBTConfigRobot.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
+
+for n = 1:length(WBTConfigRobot.ControlBoardsNames)
+
+    WBTConfigRobot.ControlledJoints       = [WBTConfigRobot.ControlledJoints, ...
+                                             ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})];
+    Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))];
+end
+
+% Total degrees of freedom
+Config.N_DOF = numel(WBTConfigRobot.ControlledJoints);
+
+% Frames list
+Frames.BASE       = 'root_link'; 
+Frames.IMU        = 'imu_frame';
+Frames.LEFT_FOOT  = 'l_sole';
+Frames.RIGHT_FOOT = 'r_sole';
+Frames.COM        = 'com';
+
+% Config.SATURATE_TORQUE_DERIVATIVE: if true, the derivative of the control
+% input is saturated. In this way, it is possible to reduce high frequency
+% oscillations and discontinuities in the control input.
+Config.SATURATE_TORQUE_DERIVATIVE         = true;
+
+% if TRUE, the controller will STOP if the joints hit the joints limits
+% and/or if the (unsigned) difference between two consecutive joints
+% encoders measurements is greater than a given threshold.
+Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS  = false;
+Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false;
+
+% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected
+% inertias are included in the system mass matrix. If
+% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints
+% motion is the result of more than one motor motion) is taken into account.
+% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive
+% reflected inertia is also considered
+Config.USE_MOTOR_REFLECTED_INERTIA    = false;
+Config.INCLUDE_COUPLING               = false;
+Config.INCLUDE_HARMONIC_DRIVE_INERTIA = false;
+
+% Config.USE_IMU4EST_BASE: if set to false, the base frame is estimated by 
+% assuming that either the left or the right foot stay stuck on the ground. 
+% Which foot the controller uses depends on the contact forces acting on it. 
+% If set to true, the base orientation is estimated by using the IMU, while
+% the base position by assuming that the origin of either the right or the
+% left foot do not move. 
+Config.USE_IMU4EST_BASE = false;
+
+% Config.YAW_IMU_FILTER when the flag Config.USE_IMU4EST_BASE = true, then 
+% the orientation of the floating base is estimated as explained above. However,
+% the foot is usually perpendicular to gravity when the robot stands on flat 
+% surfaces, and rotation about the gravity axis may be affected by the IMU drift 
+% in estimating this angle. Hence, when either of the flags Config.YAW_IMU_FILTER
+% is set to true, then the yaw angle of the contact foot is ignored and kept 
+% equal to the initial value.
+Config.FILTER_IMU_YAW   = true;
+
+% Config.CORRECT_NECK_IMU: when set equal to true, the kinematics from the
+% IMU and the contact foot is corrected by using the neck angles. If it set
+% equal to false, recall that the neck is assumed to be in (0,0,0). Valid
+% ONLY while using the ICUB HEAD IMU!
+Config.CORRECT_NECK_IMU = false;
+
+% Config.USE_QP_SOLVER: if set to true, a QP solver is used to account for 
+% inequality constraints of contact wrenches.
+Config.USE_QP_SOLVER    = true; 
+
+% Ports name list
+Ports.WRENCH_LEFT_FOOT  = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o';
+Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o';
+Ports.IMU               = ['/' WBTConfigRobot.RobotName '/inertial'];
+Ports.NECK_POS          = ['/' WBTConfigRobot.RobotName '/head/state:o'];
+
+% Ports dimensions
+Ports.NECK_POS_PORT_SIZE         = 3;
+Ports.IMU_PORT_SIZE              = 12;
+Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3];
+Ports.WRENCH_PORT_SIZE           = 6;

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

@@ -0,0 +1,189 @@
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                                                                         %
+%              COMMON ROBOT CONFIGURATION PARAMETERS                      %
+%                                                                         %
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Init simulator core physics paramaters
+physics_config.GRAVITY_ACC = [0,0,-9.81];
+physics_config.TIME_STEP = Config.tStepSim;
+
+% Robot configuration for WBToolbox
+WBTConfigRobotSim = WBToolbox.Configuration;
+WBTConfigRobotSim.RobotName = 'icubSim';
+WBTConfigRobotSim.UrdfFile = 'model.urdf';
+WBTConfigRobotSim.LocalName = 'WBT';
+WBTConfigRobotSim.GravityVector = physics_config.GRAVITY_ACC;
+
+% Controlboards and joints list. Each joint is associated to the corresponding controlboard
+WBTConfigRobotSim.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; %,'head'};
+WBTConfigRobotSim.ControlledJoints = [];
+numOfJointsForEachControlboard = [];
+
+ControlBoards = struct();
+ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'};
+ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'};
+ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'};
+ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'};
+ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
+% ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{6}) = {'neck_pitch','neck_roll','neck_yaw'};
+
+for n = 1:length(WBTConfigRobotSim.ControlBoardsNames)
+    WBTConfigRobotSim.ControlledJoints = [WBTConfigRobotSim.ControlledJoints, ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n})];
+    numOfJointsForEachControlboard = [numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n}))];
+end
+
+% structure used to configure the Robot class
+% 
+robot_config.jointOrder = WBTConfigRobotSim.ControlledJoints;
+robot_config.numOfJointsForEachControlboard = numOfJointsForEachControlboard;
+
+% Note: Since iDynTree 3.0.0, if meshFilePrefix='', the standard iDynTree workflow of locating the
+% mesh via the ExternalMesh.getFileLocationOnLocalFileSystem method is used. The iCub model meshes
+% file tree is compatible with this workflow.
+robot_config.meshFilePrefix = '';
+robot_config.fileName = WBTConfigRobotSim.UrdfFile;
+robot_config.N_DOF = numel(WBTConfigRobotSim.ControlledJoints);
+robot_config.N_DOF_MATRIX = eye(robot_config.N_DOF);
+
+% Initial condition of iCub and for the integrators.
+initialConditions.base_position = [0; 0; 0.619];
+initialConditions.orientation = diag([-1, -1, 1]);
+initialConditions.w_H_b = mwbs.State.Rp2H(initialConditions.orientation, initialConditions.base_position);
+
+% joint (inital) position
+initialConditions.s = [
+    0; 0; 0; ...
+    -35.97; 29.97; 0.06; 50.00; ...
+    -35.97; 29.97; 0.06; 50.00; ...
+    10; 0; 0; -20; -10; 0; ...
+    10; 0; 0; -20; -10; 0]*pi/180;
+
+% velocty initial conditions
+initialConditions.base_linear_velocity = [0; 0; 0];
+initialConditions.base_angular_velocity = [0; 0; 0];
+initialConditions.base_pose_dot = [initialConditions.base_linear_velocity; initialConditions.base_angular_velocity];
+initialConditions.s_dot = zeros(robot_config.N_DOF, 1);
+
+robot_config.initialConditions = initialConditions;
+
+% Reflected inertia
+robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = true;
+INCLUDE_COUPLING = true;
+
+% Robot frames list
+FramesSim.BASE = 'root_link';
+FramesSim.IMU = 'imu_frame';
+FramesSim.LEFT_FOOT = 'l_sole';
+FramesSim.RIGHT_FOOT = 'r_sole';
+FramesSim.COM = 'com';
+
+robot_config.robotFrames = FramesSim;
+
+% structure used to configure the Contacts class
+% 
+
+% foot print of the feet (iCub)
+vertex = zeros(3, 4);
+vertex(:, 1) = [-0.06; 0.04; 0];
+vertex(:, 2) = [0.11; 0.04; 0];
+vertex(:, 3) = [0.11; -0.035; 0];
+vertex(:, 4) = [-0.06; -0.035; 0];
+
+contact_config.foot_print = vertex;
+contact_config.total_num_vertices = size(vertex,2)*2;
+
+% friction coefficient for the feet
+contact_config.friction_coefficient = 0.1;
+
+%% Motors reflected inertia
+
+% transmission ratio (1/N)
+Gamma                 = 0.01*eye(ROBOT_DOF);
+
+% modify the value of the transmission ratio for the hip pitch. 
+% TODO: avoid to hard-code the joint numbering
+Gamma(end-5, end-5)   = 0.0067;
+Gamma(end-11,end-11)  = 0.0067;
+
+% motors inertia (Kg*m^2)
+legsMotors_I_m               = 0.0827*1e-4;
+torsoPitchRollMotors_I_m     = 0.0827*1e-4;
+torsoYawMotors_I_m           = 0.0585*1e-4;
+armsMotors_I_m               = 0.0585*1e-4;
+
+% add harmonic drives reflected inertia
+legsMotors_I_m           = legsMotors_I_m + 0.054*1e-4;
+torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4;
+torsoYawMotors_I_m       = torsoYawMotors_I_m + 0.021*1e-4;
+armsMotors_I_m           = armsMotors_I_m + 0.021*1e-4;
+
+I_m                   = diag([torsoPitchRollMotors_I_m*ones(2,1);
+                                     torsoYawMotors_I_m;
+                                     armsMotors_I_m*ones(8,1);
+                                     legsMotors_I_m*ones(12,1)]);
+
+% parameters for coupling matrices. Updated according to the wiki:
+%
+% http://wiki.icub.org/wiki/ICub_coupled_joints 
+%
+% and corrected according to https://github.com/robotology/robots-configuration/issues/39
+t            = 0.615;
+r            = 0.022;
+R            = 0.04;
+
+% coupling matrices
+T_LShoulder  = [-1  0  0;
+                -1 -t  0;
+                 0  t -t];
+
+T_RShoulder  = [ 1  0  0;
+                 1  t  0;
+                 0 -t  t];
+
+T_torso      = [ 0.5    -0.5     0;
+                 0.5     0.5     0;
+                 r/(2*R) r/(2*R) r/R];
+
+if INCLUDE_COUPLING
+    T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
+else          
+    T = eye(robot_config.N_DOF);
+end
+
+motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m);
+
+% 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.6 0.6]';
+KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.6 0.6]';
+
+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
+visualizerAroundRobot = 1; % mt
+
+clear ControlBoards numOfJointsForEachControlboard FramesSim initialConditions vertex