Merge pull request #135 from lrapetti/add-iCubGazeboV3

Add configuration files for iCubGazeboV3

controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV3/configJointsControl.m

@@ -0,0 +1,13 @@
+% CONFIGJOINTSCONTROL configures all the options associated to the
+%                     joints controller.
+%
+
+%% --- Initialization ---
+
+% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true,
+% the robot will also move all actuated joints following a sine trajectory
+Config.MOVE_JOINTS = false;
+
+% Max unsigned difference between two consecutive (measured) joint positions, 
+% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad]
+Sat.maxJointsPositionDelta = 15*pi/180;

controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV3/configRobot.m

@@ -0,0 +1,34 @@
+% CONFIGROBOT initializes parameters specific of a particular robot
+%             (e.g., icuGazeboSim)
+%
+
+%% --- Initialization ---
+
+% Gains and parameters for impedance controller
+Config.ON_GAZEBO = true;
+ROBOT_DOF        = 23;
+
+% 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

controllers/fixed-base-joints-torque-control/app/robots/iCubGazeboV3/gainsAndReferences.m

@@ -0,0 +1,28 @@
+% GAINSANDREFERENCES initializes the controller parameters: gains,
+%                    regularization parameters and references.
+%
+
+%% --- Initialization ---
+
+% References for the demo with joints movements
+if Config.MOVE_JOINTS
+
+    % Postural task gains
+    KP = 10*diag(ones(1,ROBOT_DOF));
+    KD = 2*sqrt(KP)/10;
+
+    Ref.Amplitude = 5*ones(1,ROBOT_DOF);
+    Ref.Frequency = 0.25*ones(1,ROBOT_DOF);
+else    
+    % Postural task gains
+    KP = 0*diag(ones(1,ROBOT_DOF));
+    KD = 0*2*sqrt(KP);
+
+    Ref.Amplitude = 0*ones(1,ROBOT_DOF);
+    Ref.Frequency = 0*ones(1,ROBOT_DOF);
+end
+
+if size(KP,1) ~= ROBOT_DOF
+
+    error('Dimension of KP different from ROBOT_DOF')
+end

controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV3/configRobot.m

@@ -0,0 +1,100 @@
+% 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
+
+% 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         = false;
+
+% 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 = true;
+
+% 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_foot_rear/cartesianEndEffectorWrench:o';
+Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot_rear/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/app/robots/iCubGazeboV3/configStateMachine.m

@@ -0,0 +1,160 @@
+% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity
+%                    of the demo, repeat movements, and so on).
+
+%% --- Initialization ---
+
+% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY)
+Config.LEFT_RIGHT_MOVEMENTS = false;
+
+% If equal to one, the desired values of the center of mass are smoothed internally 
+Config.SMOOTH_COM_DES       = true;   
+
+% If equal to one, the desired values of the postural tasks are smoothed internally 
+Config.SMOOTH_JOINT_DES     = true;   
+
+% Joint torques saturation [Nm]
+Sat.torque                  = 60;
+
+% Joint torques rate of change saturation
+Sat.uDotMax                 = 300;
+
+% max unsigned difference between two consecutive (measured) joint positions, 
+% i.e. delta_qj = abs(qj(k) - qj(k-1))
+Sat.maxJointsPositionDelta  = 15*pi/180; % [rad] 
+
+%% Regularization parameters
+Reg.pinvDamp_baseVel        = 1e-7;
+Reg.pinvDamp                = 1; 
+Reg.pinvTol                 = 1e-5;
+Reg.KP_postural             = 0.1;
+Reg.KD_postural             = 0;
+Reg.HessianQP               = 1e-7;    
+
+%% State Machine configuration
+
+% time between two yoga positions
+StateMachine.joints_pauseBetweenYogaMoves = 5;
+
+% contact forces threshold
+StateMachine.wrench_thresholdContactOn    = 50;
+StateMachine.wrench_thresholdContactOff   = 100;
+
+% threshold on CoM and joints error
+StateMachine.CoM_threshold                = 0.01; 
+StateMachine.joints_thresholdNotInContact = 5;
+StateMachine.joints_thresholdInContact    = 50;
+
+% initial state for state machine
+StateMachine.initialState                 = 1;
+
+% other configuration parameters for state machine
+StateMachine.tBalancing                   = 1;
+StateMachine.tBalancingBeforeYoga         = 1;
+StateMachine.yogaExtended                 = true;
+StateMachine.skipYoga                     = false;
+StateMachine.demoOnlyBalancing            = false;
+StateMachine.demoStartsOnRightSupport     = false; % If false, the Yoga demo is performed on the left foot first
+StateMachine.yogaAlsoOnRightFoot          = false; % TO DO: yoga on both feet starting from right foot (not available for now)
+
+%%%% List of possible "Yoga in loop" %%%%
+
+% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
+% foot, back on two feet, yoga right foot, back on two feet). The demo is
+% repeated until the user stops the Simulink model. This option is ignored
+% if Sm.demoStartsOnRightSupport = true.
+StateMachine.twoFeetYogaInLoop            = false;
+
+% the robot will repeat the ONE FOOT yoga for the number of times the user
+% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
+% feet balancing in between to consecutive yoga. WARNING: if the option 
+% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
+% on left foot for the number of times the user specifies in the Sm.yogaCounter,
+% and then it will repeat the yoga on the right foot for the same number of times.
+StateMachine.oneFootYogaInLoop            = false;
+StateMachine.yogaCounter                  = 5;
+
+%% Parameters for motors reflected inertia
+
+% transmission ratio (1/N)
+Config.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
+Config.Gamma(end-5, end-5)   = 0.0067;
+Config.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
+if Config.INCLUDE_HARMONIC_DRIVE_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; 
+end
+
+Config.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 Config.INCLUDE_COUPLING
+
+    Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
+else          
+    Config.T = eye(ROBOT_DOF);
+end
+
+% gain for feedforward term in joint torques calculation. Valid range: a
+% value between 0 and 1
+Config.K_ff  = 0;
+
+% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints
+% accelerations are used for computing the feedforward term in joint
+% torques calculations. Not effective if Config.K_ff = 0.
+Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false;
+
+%% Constraints for QP for balancing
+
+% The friction cone is approximated by using linear interpolation of the circle. 
+% So, numberOfPoints defines the number of points used to interpolate the circle 
+% in each cicle's quadrant
+numberOfPoints               = 4;  
+forceFrictionCoefficient     = 1/3;    
+torsionalFrictionCoefficient = 1/75;
+fZmin                        = 10;
+
+% physical size of the foot                             
+feet_size                    = [-0.12  0.12 ;    % xMin, xMax
+                                -0.05  0.05 ];   % yMin, yMax  
+
+% Compute contact constraints (friction cone, unilateral constraints)
+[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ...
+    (forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin);