Skip to content

Commit c217f05

Browse files
nunoguedelhanunoguedelha
authored
Merge pull request #122 from robotology/feature/replace-gazebo-by-matlab-simulator
Integrate the YOGA++ (floating-base-balancing-torque-control) with the matlab-whole-body-simulator
2 parents 5a1a92c + 4e03377 commit c217f05

19 files changed

+26896
-0
lines changed

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
register_mdl(MODELNAME "torqueControlBalancingWithSimu")

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

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
## Module description
2+
3+
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.
4+
5+
<img src="/doc/pics/torqueControl.png" width="1800">
6+
7+
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).
8+
9+
### Compatibility
10+
11+
The folder contains the Simulink model `torqueControlBalancing.mdl`, which is generated by using Matlab R2017b.
12+
13+
### Supported robots
14+
15+
Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`.
16+
17+
## Module details
18+
19+
### How to run the demo
20+
21+
For information on how to use the controllers both in **simulation** and with the **real robot**, please refer to the **wiki** of the repo.
22+
23+
### Configuration file
24+
25+
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.
26+
27+
### Robot and demo specific configurations
28+
29+
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

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
% CONFIGROBOT initializes parameters specific of a particular robot
2+
% (e.g., icubGazeboSim)
3+
4+
%% --- Initialization ---
5+
6+
Config.ON_GAZEBO = true;
7+
ROBOT_DOF = 23;
8+
Config.GRAV_ACC = 9.81;
9+
10+
% Robot configuration for WBToolbox
11+
WBTConfigRobot = WBToolbox.Configuration;
12+
WBTConfigRobot.RobotName = 'icubSim';
13+
WBTConfigRobot.UrdfFile = 'model.urdf';
14+
WBTConfigRobot.LocalName = 'WBT';
15+
16+
% Controlboards and joints list. Each joint is associated to the corresponding controlboard
17+
WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'};
18+
WBTConfigRobot.ControlledJoints = [];
19+
Config.numOfJointsForEachControlboard = [];
20+
21+
ControlBoards = struct();
22+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'};
23+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'};
24+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'};
25+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'};
26+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
27+
28+
for n = 1:length(WBTConfigRobot.ControlBoardsNames)
29+
30+
WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ...
31+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})];
32+
Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))];
33+
end
34+
35+
% Total degrees of freedom
36+
Config.N_DOF = numel(WBTConfigRobot.ControlledJoints);
37+
38+
% Frames list
39+
Frames.BASE = 'root_link';
40+
Frames.IMU = 'imu_frame';
41+
Frames.LEFT_FOOT = 'l_sole';
42+
Frames.RIGHT_FOOT = 'r_sole';
43+
Frames.COM = 'com';
44+
45+
% Config.SATURATE_TORQUE_DERIVATIVE: if true, the derivative of the control
46+
% input is saturated. In this way, it is possible to reduce high frequency
47+
% oscillations and discontinuities in the control input.
48+
Config.SATURATE_TORQUE_DERIVATIVE = true;
49+
50+
% if TRUE, the controller will STOP if the joints hit the joints limits
51+
% and/or if the (unsigned) difference between two consecutive joints
52+
% encoders measurements is greater than a given threshold.
53+
Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false;
54+
Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = false;
55+
56+
% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected
57+
% inertias are included in the system mass matrix. If
58+
% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints
59+
% motion is the result of more than one motor motion) is taken into account.
60+
% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive
61+
% reflected inertia is also considered
62+
Config.USE_MOTOR_REFLECTED_INERTIA = false;
63+
Config.INCLUDE_COUPLING = false;
64+
Config.INCLUDE_HARMONIC_DRIVE_INERTIA = false;
65+
66+
% Config.USE_IMU4EST_BASE: if set to false, the base frame is estimated by
67+
% assuming that either the left or the right foot stay stuck on the ground.
68+
% Which foot the controller uses depends on the contact forces acting on it.
69+
% If set to true, the base orientation is estimated by using the IMU, while
70+
% the base position by assuming that the origin of either the right or the
71+
% left foot do not move.
72+
Config.USE_IMU4EST_BASE = false;
73+
74+
% Config.YAW_IMU_FILTER when the flag Config.USE_IMU4EST_BASE = true, then
75+
% the orientation of the floating base is estimated as explained above. However,
76+
% the foot is usually perpendicular to gravity when the robot stands on flat
77+
% surfaces, and rotation about the gravity axis may be affected by the IMU drift
78+
% in estimating this angle. Hence, when either of the flags Config.YAW_IMU_FILTER
79+
% is set to true, then the yaw angle of the contact foot is ignored and kept
80+
% equal to the initial value.
81+
Config.FILTER_IMU_YAW = true;
82+
83+
% Config.CORRECT_NECK_IMU: when set equal to true, the kinematics from the
84+
% IMU and the contact foot is corrected by using the neck angles. If it set
85+
% equal to false, recall that the neck is assumed to be in (0,0,0). Valid
86+
% ONLY while using the ICUB HEAD IMU!
87+
Config.CORRECT_NECK_IMU = false;
88+
89+
% Config.USE_QP_SOLVER: if set to true, a QP solver is used to account for
90+
% inequality constraints of contact wrenches.
91+
Config.USE_QP_SOLVER = true;
92+
93+
% Ports name list
94+
Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o';
95+
Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o';
96+
Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial'];
97+
Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o'];
98+
99+
% Ports dimensions
100+
Ports.NECK_POS_PORT_SIZE = 3;
101+
Ports.IMU_PORT_SIZE = 12;
102+
Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3];
103+
Ports.WRENCH_PORT_SIZE = 6;

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

Lines changed: 189 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2+
% %
3+
% COMMON ROBOT CONFIGURATION PARAMETERS %
4+
% %
5+
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
6+
7+
%% Init simulator core physics paramaters
8+
physics_config.GRAVITY_ACC = [0,0,-9.81];
9+
physics_config.TIME_STEP = Config.tStepSim;
10+
11+
% Robot configuration for WBToolbox
12+
WBTConfigRobotSim = WBToolbox.Configuration;
13+
WBTConfigRobotSim.RobotName = 'icubSim';
14+
WBTConfigRobotSim.UrdfFile = 'model.urdf';
15+
WBTConfigRobotSim.LocalName = 'WBT';
16+
WBTConfigRobotSim.GravityVector = physics_config.GRAVITY_ACC;
17+
18+
% Controlboards and joints list. Each joint is associated to the corresponding controlboard
19+
WBTConfigRobotSim.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'}; %,'head'};
20+
WBTConfigRobotSim.ControlledJoints = [];
21+
numOfJointsForEachControlboard = [];
22+
23+
ControlBoards = struct();
24+
ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'};
25+
ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'};
26+
ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'};
27+
ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'};
28+
ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
29+
% ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{6}) = {'neck_pitch','neck_roll','neck_yaw'};
30+
31+
for n = 1:length(WBTConfigRobotSim.ControlBoardsNames)
32+
WBTConfigRobotSim.ControlledJoints = [WBTConfigRobotSim.ControlledJoints, ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n})];
33+
numOfJointsForEachControlboard = [numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobotSim.ControlBoardsNames{n}))];
34+
end
35+
36+
% structure used to configure the Robot class
37+
%
38+
robot_config.jointOrder = WBTConfigRobotSim.ControlledJoints;
39+
robot_config.numOfJointsForEachControlboard = numOfJointsForEachControlboard;
40+
41+
% Note: Since iDynTree 3.0.0, if meshFilePrefix='', the standard iDynTree workflow of locating the
42+
% mesh via the ExternalMesh.getFileLocationOnLocalFileSystem method is used. The iCub model meshes
43+
% file tree is compatible with this workflow.
44+
robot_config.meshFilePrefix = '';
45+
robot_config.fileName = WBTConfigRobotSim.UrdfFile;
46+
robot_config.N_DOF = numel(WBTConfigRobotSim.ControlledJoints);
47+
robot_config.N_DOF_MATRIX = eye(robot_config.N_DOF);
48+
49+
% Initial condition of iCub and for the integrators.
50+
initialConditions.base_position = [0; 0; 0.619];
51+
initialConditions.orientation = diag([-1, -1, 1]);
52+
initialConditions.w_H_b = mwbs.State.Rp2H(initialConditions.orientation, initialConditions.base_position);
53+
54+
% joint (inital) position
55+
initialConditions.s = [
56+
0; 0; 0; ...
57+
-35.97; 29.97; 0.06; 50.00; ...
58+
-35.97; 29.97; 0.06; 50.00; ...
59+
10; 0; 0; -20; -10; 0; ...
60+
10; 0; 0; -20; -10; 0]*pi/180;
61+
62+
% velocty initial conditions
63+
initialConditions.base_linear_velocity = [0; 0; 0];
64+
initialConditions.base_angular_velocity = [0; 0; 0];
65+
initialConditions.base_pose_dot = [initialConditions.base_linear_velocity; initialConditions.base_angular_velocity];
66+
initialConditions.s_dot = zeros(robot_config.N_DOF, 1);
67+
68+
robot_config.initialConditions = initialConditions;
69+
70+
% Reflected inertia
71+
robot_config.SIMULATE_MOTOR_REFLECTED_INERTIA = true;
72+
INCLUDE_COUPLING = true;
73+
74+
% Robot frames list
75+
FramesSim.BASE = 'root_link';
76+
FramesSim.IMU = 'imu_frame';
77+
FramesSim.LEFT_FOOT = 'l_sole';
78+
FramesSim.RIGHT_FOOT = 'r_sole';
79+
FramesSim.COM = 'com';
80+
81+
robot_config.robotFrames = FramesSim;
82+
83+
% structure used to configure the Contacts class
84+
%
85+
86+
% foot print of the feet (iCub)
87+
vertex = zeros(3, 4);
88+
vertex(:, 1) = [-0.06; 0.04; 0];
89+
vertex(:, 2) = [0.11; 0.04; 0];
90+
vertex(:, 3) = [0.11; -0.035; 0];
91+
vertex(:, 4) = [-0.06; -0.035; 0];
92+
93+
contact_config.foot_print = vertex;
94+
contact_config.total_num_vertices = size(vertex,2)*2;
95+
96+
% friction coefficient for the feet
97+
contact_config.friction_coefficient = 0.1;
98+
99+
%% Motors reflected inertia
100+
101+
% transmission ratio (1/N)
102+
Gamma = 0.01*eye(ROBOT_DOF);
103+
104+
% modify the value of the transmission ratio for the hip pitch.
105+
% TODO: avoid to hard-code the joint numbering
106+
Gamma(end-5, end-5) = 0.0067;
107+
Gamma(end-11,end-11) = 0.0067;
108+
109+
% motors inertia (Kg*m^2)
110+
legsMotors_I_m = 0.0827*1e-4;
111+
torsoPitchRollMotors_I_m = 0.0827*1e-4;
112+
torsoYawMotors_I_m = 0.0585*1e-4;
113+
armsMotors_I_m = 0.0585*1e-4;
114+
115+
% add harmonic drives reflected inertia
116+
legsMotors_I_m = legsMotors_I_m + 0.054*1e-4;
117+
torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4;
118+
torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4;
119+
armsMotors_I_m = armsMotors_I_m + 0.021*1e-4;
120+
121+
I_m = diag([torsoPitchRollMotors_I_m*ones(2,1);
122+
torsoYawMotors_I_m;
123+
armsMotors_I_m*ones(8,1);
124+
legsMotors_I_m*ones(12,1)]);
125+
126+
% parameters for coupling matrices. Updated according to the wiki:
127+
%
128+
% http://wiki.icub.org/wiki/ICub_coupled_joints
129+
%
130+
% and corrected according to https://github.com/robotology/robots-configuration/issues/39
131+
t = 0.615;
132+
r = 0.022;
133+
R = 0.04;
134+
135+
% coupling matrices
136+
T_LShoulder = [-1 0 0;
137+
-1 -t 0;
138+
0 t -t];
139+
140+
T_RShoulder = [ 1 0 0;
141+
1 t 0;
142+
0 -t t];
143+
144+
T_torso = [ 0.5 -0.5 0;
145+
0.5 0.5 0;
146+
r/(2*R) r/(2*R) r/R];
147+
148+
if INCLUDE_COUPLING
149+
T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
150+
else
151+
T = eye(robot_config.N_DOF);
152+
end
153+
154+
motorsReflectedInertia = wbc.computeMotorsReflectedInertia(Gamma,T,I_m);
155+
156+
% Joint friction
157+
158+
% === Mapping ===
159+
jointDefaultOrder = {...
160+
'torso_pitch','torso_roll','torso_yaw', ...
161+
'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow', ...
162+
'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow', ...
163+
'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll', ...
164+
'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
165+
166+
KvmechMappingTorso = 0.2*ones(3,1);
167+
KvmechMappingLeftArm = 0.2*ones(4,1);
168+
KvmechMappingRightArm = 0.2*ones(4,1);
169+
KvmechMappingLeftLeg = [0.2 0.2 0.2 0.2 0.6 0.6]';
170+
KvmechMappingRightLeg = [0.2 0.2 0.2 0.2 0.6 0.6]';
171+
172+
KvmechMapping = containers.Map(...
173+
jointDefaultOrder, ...
174+
[
175+
KvmechMappingTorso
176+
KvmechMappingLeftArm
177+
KvmechMappingRightArm
178+
KvmechMappingLeftLeg
179+
KvmechMappingRightLeg
180+
]);
181+
182+
KvmechMat = diag(cell2mat(KvmechMapping.values(robot_config.jointOrder)));
183+
184+
jointFrictionMat = wbc.computeMotorsReflectedInertia(eye(robot_config.N_DOF),T,KvmechMat);
185+
186+
%% size of the square you see around the robot
187+
visualizerAroundRobot = 1; % mt
188+
189+
clear ControlBoards numOfJointsForEachControlboard FramesSim initialConditions vertex

0 commit comments

Comments
 (0)