Skip to content

Commit 32f4499

Browse files
author
Gabriele Nava
authored
Merge pull request #135 from lrapetti/add-iCubGazeboV3
Add configuration files for iCubGazeboV3
2 parents c5a9e64 + 3540b82 commit 32f4499

File tree

9 files changed

+715
-0
lines changed

9 files changed

+715
-0
lines changed

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

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
% CONFIGJOINTSCONTROL configures all the options associated to the
2+
% joints controller.
3+
%
4+
5+
%% --- Initialization ---
6+
7+
% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true,
8+
% the robot will also move all actuated joints following a sine trajectory
9+
Config.MOVE_JOINTS = false;
10+
11+
% Max unsigned difference between two consecutive (measured) joint positions,
12+
% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad]
13+
Sat.maxJointsPositionDelta = 15*pi/180;

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

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
% CONFIGROBOT initializes parameters specific of a particular robot
2+
% (e.g., icuGazeboSim)
3+
%
4+
5+
%% --- Initialization ---
6+
7+
% Gains and parameters for impedance controller
8+
Config.ON_GAZEBO = true;
9+
ROBOT_DOF = 23;
10+
11+
% Robot configuration for WBToolbox
12+
WBTConfigRobot = WBToolbox.Configuration;
13+
WBTConfigRobot.RobotName = 'icubSim';
14+
WBTConfigRobot.UrdfFile = 'model.urdf';
15+
WBTConfigRobot.LocalName = 'WBT';
16+
17+
% Controlboards and joints list. Each joint is associated to the corresponding controlboard
18+
WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm','left_leg','right_leg'};
19+
WBTConfigRobot.ControlledJoints = [];
20+
Config.numOfJointsForEachControlboard = [];
21+
22+
ControlBoards = struct();
23+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{1}) = {'torso_pitch','torso_roll','torso_yaw'};
24+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{2}) = {'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'};
25+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{3}) = {'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow'};
26+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{4}) = {'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll'};
27+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{5}) = {'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
28+
29+
for n = 1:length(WBTConfigRobot.ControlBoardsNames)
30+
31+
WBTConfigRobot.ControlledJoints = [WBTConfigRobot.ControlledJoints, ...
32+
ControlBoards.(WBTConfigRobot.ControlBoardsNames{n})];
33+
Config.numOfJointsForEachControlboard = [Config.numOfJointsForEachControlboard; length(ControlBoards.(WBTConfigRobot.ControlBoardsNames{n}))];
34+
end

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

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
% GAINSANDREFERENCES initializes the controller parameters: gains,
2+
% regularization parameters and references.
3+
%
4+
5+
%% --- Initialization ---
6+
7+
% References for the demo with joints movements
8+
if Config.MOVE_JOINTS
9+
10+
% Postural task gains
11+
KP = 10*diag(ones(1,ROBOT_DOF));
12+
KD = 2*sqrt(KP)/10;
13+
14+
Ref.Amplitude = 5*ones(1,ROBOT_DOF);
15+
Ref.Frequency = 0.25*ones(1,ROBOT_DOF);
16+
else
17+
% Postural task gains
18+
KP = 0*diag(ones(1,ROBOT_DOF));
19+
KD = 0*2*sqrt(KP);
20+
21+
Ref.Amplitude = 0*ones(1,ROBOT_DOF);
22+
Ref.Frequency = 0*ones(1,ROBOT_DOF);
23+
end
24+
25+
if size(KP,1) ~= ROBOT_DOF
26+
27+
error('Dimension of KP different from ROBOT_DOF')
28+
end

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

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
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+
% Frames list
36+
Frames.BASE = 'root_link';
37+
Frames.IMU = 'imu_frame';
38+
Frames.LEFT_FOOT = 'l_sole';
39+
Frames.RIGHT_FOOT = 'r_sole';
40+
Frames.COM = 'com';
41+
42+
% Config.SATURATE_TORQUE_DERIVATIVE: if true, the derivative of the control
43+
% input is saturated. In this way, it is possible to reduce high frequency
44+
% oscillations and discontinuities in the control input.
45+
Config.SATURATE_TORQUE_DERIVATIVE = false;
46+
47+
% if TRUE, the controller will STOP if the joints hit the joints limits
48+
% and/or if the (unsigned) difference between two consecutive joints
49+
% encoders measurements is greater than a given threshold.
50+
Config.EMERGENCY_STOP_WITH_JOINTS_LIMITS = false;
51+
Config.EMERGENCY_STOP_WITH_ENCODER_SPIKES = true;
52+
53+
% Config.USE_MOTOR_REFLECTED_INERTIA: if set to true, motors reflected
54+
% inertias are included in the system mass matrix. If
55+
% Config.INCLUDE_COUPLING is true, then the coupling effect (some joints
56+
% motion is the result of more than one motor motion) is taken into account.
57+
% Config.INCLUDE_HARMONIC_DRIVE_INERTIA is true, then the harmonic drive
58+
% reflected inertia is also considered
59+
Config.USE_MOTOR_REFLECTED_INERTIA = false;
60+
Config.INCLUDE_COUPLING = false;
61+
Config.INCLUDE_HARMONIC_DRIVE_INERTIA = false;
62+
63+
% Config.USE_IMU4EST_BASE: if set to false, the base frame is estimated by
64+
% assuming that either the left or the right foot stay stuck on the ground.
65+
% Which foot the controller uses depends on the contact forces acting on it.
66+
% If set to true, the base orientation is estimated by using the IMU, while
67+
% the base position by assuming that the origin of either the right or the
68+
% left foot do not move.
69+
Config.USE_IMU4EST_BASE = false;
70+
71+
% Config.YAW_IMU_FILTER when the flag Config.USE_IMU4EST_BASE = true, then
72+
% the orientation of the floating base is estimated as explained above. However,
73+
% the foot is usually perpendicular to gravity when the robot stands on flat
74+
% surfaces, and rotation about the gravity axis may be affected by the IMU drift
75+
% in estimating this angle. Hence, when either of the flags Config.YAW_IMU_FILTER
76+
% is set to true, then the yaw angle of the contact foot is ignored and kept
77+
% equal to the initial value.
78+
Config.FILTER_IMU_YAW = true;
79+
80+
% Config.CORRECT_NECK_IMU: when set equal to true, the kinematics from the
81+
% IMU and the contact foot is corrected by using the neck angles. If it set
82+
% equal to false, recall that the neck is assumed to be in (0,0,0). Valid
83+
% ONLY while using the ICUB HEAD IMU!
84+
Config.CORRECT_NECK_IMU = false;
85+
86+
% Config.USE_QP_SOLVER: if set to true, a QP solver is used to account for
87+
% inequality constraints of contact wrenches.
88+
Config.USE_QP_SOLVER = true;
89+
90+
% Ports name list
91+
Ports.WRENCH_LEFT_FOOT = '/wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o';
92+
Ports.WRENCH_RIGHT_FOOT = '/wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o';
93+
Ports.IMU = ['/' WBTConfigRobot.RobotName '/inertial'];
94+
Ports.NECK_POS = ['/' WBTConfigRobot.RobotName '/head/state:o'];
95+
96+
% Ports dimensions
97+
Ports.NECK_POS_PORT_SIZE = 3;
98+
Ports.IMU_PORT_SIZE = 12;
99+
Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3];
100+
Ports.WRENCH_PORT_SIZE = 6;

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

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
% CONFIGSTATEMACHINE configures the state machine (type of demo, velocity
2+
% of the demo, repeat movements, and so on).
3+
4+
%% --- Initialization ---
5+
6+
% If true, the robot CoM will follow a desired reference trajectory (COORDINATOR DEMO ONLY)
7+
Config.LEFT_RIGHT_MOVEMENTS = false;
8+
9+
% If equal to one, the desired values of the center of mass are smoothed internally
10+
Config.SMOOTH_COM_DES = true;
11+
12+
% If equal to one, the desired values of the postural tasks are smoothed internally
13+
Config.SMOOTH_JOINT_DES = true;
14+
15+
% Joint torques saturation [Nm]
16+
Sat.torque = 60;
17+
18+
% Joint torques rate of change saturation
19+
Sat.uDotMax = 300;
20+
21+
% max unsigned difference between two consecutive (measured) joint positions,
22+
% i.e. delta_qj = abs(qj(k) - qj(k-1))
23+
Sat.maxJointsPositionDelta = 15*pi/180; % [rad]
24+
25+
%% Regularization parameters
26+
Reg.pinvDamp_baseVel = 1e-7;
27+
Reg.pinvDamp = 1;
28+
Reg.pinvTol = 1e-5;
29+
Reg.KP_postural = 0.1;
30+
Reg.KD_postural = 0;
31+
Reg.HessianQP = 1e-7;
32+
33+
%% State Machine configuration
34+
35+
% time between two yoga positions
36+
StateMachine.joints_pauseBetweenYogaMoves = 5;
37+
38+
% contact forces threshold
39+
StateMachine.wrench_thresholdContactOn = 50;
40+
StateMachine.wrench_thresholdContactOff = 100;
41+
42+
% threshold on CoM and joints error
43+
StateMachine.CoM_threshold = 0.01;
44+
StateMachine.joints_thresholdNotInContact = 5;
45+
StateMachine.joints_thresholdInContact = 50;
46+
47+
% initial state for state machine
48+
StateMachine.initialState = 1;
49+
50+
% other configuration parameters for state machine
51+
StateMachine.tBalancing = 1;
52+
StateMachine.tBalancingBeforeYoga = 1;
53+
StateMachine.yogaExtended = true;
54+
StateMachine.skipYoga = false;
55+
StateMachine.demoOnlyBalancing = false;
56+
StateMachine.demoStartsOnRightSupport = false; % If false, the Yoga demo is performed on the left foot first
57+
StateMachine.yogaAlsoOnRightFoot = false; % TO DO: yoga on both feet starting from right foot (not available for now)
58+
59+
%%%% List of possible "Yoga in loop" %%%%
60+
61+
% the robot will repeat the FULL DEMO (two feet balancing, yoga on left
62+
% foot, back on two feet, yoga right foot, back on two feet). The demo is
63+
% repeated until the user stops the Simulink model. This option is ignored
64+
% if Sm.demoStartsOnRightSupport = true.
65+
StateMachine.twoFeetYogaInLoop = false;
66+
67+
% the robot will repeat the ONE FOOT yoga for the number of times the user
68+
% specifies in the Sm.yogaCounter option. The robot WILL NOT go back to two
69+
% feet balancing in between to consecutive yoga. WARNING: if the option
70+
% Sm.yogaAlsoOnRightFoot is true, then the robot will repeat first the yoga
71+
% on left foot for the number of times the user specifies in the Sm.yogaCounter,
72+
% and then it will repeat the yoga on the right foot for the same number of times.
73+
StateMachine.oneFootYogaInLoop = false;
74+
StateMachine.yogaCounter = 5;
75+
76+
%% Parameters for motors reflected inertia
77+
78+
% transmission ratio (1/N)
79+
Config.Gamma = 0.01*eye(ROBOT_DOF);
80+
81+
% modify the value of the transmission ratio for the hip pitch.
82+
% TODO: avoid to hard-code the joint numbering
83+
Config.Gamma(end-5, end-5) = 0.0067;
84+
Config.Gamma(end-11,end-11) = 0.0067;
85+
86+
% motors inertia (Kg*m^2)
87+
legsMotors_I_m = 0.0827*1e-4;
88+
torsoPitchRollMotors_I_m = 0.0827*1e-4;
89+
torsoYawMotors_I_m = 0.0585*1e-4;
90+
armsMotors_I_m = 0.0585*1e-4;
91+
92+
% add harmonic drives reflected inertia
93+
if Config.INCLUDE_HARMONIC_DRIVE_INERTIA
94+
95+
legsMotors_I_m = legsMotors_I_m + 0.054*1e-4;
96+
torsoPitchRollMotors_I_m = torsoPitchRollMotors_I_m + 0.054*1e-4;
97+
torsoYawMotors_I_m = torsoYawMotors_I_m + 0.021*1e-4;
98+
armsMotors_I_m = armsMotors_I_m + 0.021*1e-4;
99+
end
100+
101+
Config.I_m = diag([torsoPitchRollMotors_I_m*ones(2,1);
102+
torsoYawMotors_I_m;
103+
armsMotors_I_m*ones(8,1);
104+
legsMotors_I_m*ones(12,1)]);
105+
106+
% parameters for coupling matrices. Updated according to the wiki:
107+
%
108+
% http://wiki.icub.org/wiki/ICub_coupled_joints
109+
%
110+
% and corrected according to https://github.com/robotology/robots-configuration/issues/39
111+
t = 0.615;
112+
r = 0.022;
113+
R = 0.04;
114+
115+
% coupling matrices
116+
T_LShoulder = [-1 0 0;
117+
-1 -t 0;
118+
0 t -t];
119+
120+
T_RShoulder = [ 1 0 0;
121+
1 t 0;
122+
0 -t t];
123+
124+
T_torso = [ 0.5 -0.5 0;
125+
0.5 0.5 0;
126+
r/(2*R) r/(2*R) r/R];
127+
128+
if Config.INCLUDE_COUPLING
129+
130+
Config.T = blkdiag(T_torso,T_LShoulder,1,T_RShoulder,1,eye(12));
131+
else
132+
Config.T = eye(ROBOT_DOF);
133+
end
134+
135+
% gain for feedforward term in joint torques calculation. Valid range: a
136+
% value between 0 and 1
137+
Config.K_ff = 0;
138+
139+
% Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA if true, the desired joints
140+
% accelerations are used for computing the feedforward term in joint
141+
% torques calculations. Not effective if Config.K_ff = 0.
142+
Config.USE_DES_JOINT_ACC_FOR_MOTORS_INERTIA = false;
143+
144+
%% Constraints for QP for balancing
145+
146+
% The friction cone is approximated by using linear interpolation of the circle.
147+
% So, numberOfPoints defines the number of points used to interpolate the circle
148+
% in each cicle's quadrant
149+
numberOfPoints = 4;
150+
forceFrictionCoefficient = 1/3;
151+
torsionalFrictionCoefficient = 1/75;
152+
fZmin = 10;
153+
154+
% physical size of the foot
155+
feet_size = [-0.12 0.12 ; % xMin, xMax
156+
-0.05 0.05 ]; % yMin, yMax
157+
158+
% Compute contact constraints (friction cone, unilateral constraints)
159+
[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ...
160+
(forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin);

0 commit comments

Comments
 (0)