added elastic joint control for icub

controllers/tools/elasticJoints/addElasticJoints.m

@@ -13,20 +13,44 @@
 % ------------Initialization----------------
 %% Configure parameters
 ndof                 = CONFIG.ndof;
-% reduction ratio
-r                    = 100;
+robot_name           = CONFIG.robot_name;
 
-%% Motor dynamics
-% motor inertia
-ELASTICITY.B         = 5.480e-5*(r^2)*eye(ndof);
 
-% stiffness
-ELASTICITY.KS        = 1000*eye(ndof);
+if strcmp(robot_name,'bigman') == 1
+
+    % reduction ratio
+    r                    = 100;
 
-% damping
-ELASTICITY.KD        = 1000*eye(ndof);
+    %% Motor dynamics
+    % motor inertia
+    ELASTICITY.B         = 5.480e-5*(r^2)*eye(ndof);
 
-% contorl gains
-ELASTICITY.KD_gain   = 1000*eye(ndof);
+    % stiffness
+    ELASTICITY.KS        = 0.005*(r^2)*eye(ndof);
+
+    % damping
+    ELASTICITY.KD        = 0.0005*(r^2)*eye(ndof);
+
+    % control gains
+    ELASTICITY.KD_gain   = 100*eye(ndof);
+
+elseif strcmp(robot_name,'icubGazeboSim') == 1
+
+    % reduction ratio
+    r                    = 100;
+
+    %% Motor dynamics
+    % motor inertia
+    ELASTICITY.B         = 1e-5*(r^2)*eye(ndof);
+
+    % stiffness
+    ELASTICITY.KS        = 0.001*(r^2)*eye(ndof);
+
+    % damping
+    ELASTICITY.KD        = 0.0001*(r^2)*eye(ndof);
+
+    % control gains
+    ELASTICITY.KD_gain   = 10*eye(ndof);    
+end
 
 end

controllers/torqueBalancingWalkman_JE/init/runController.m

@@ -53,6 +53,11 @@
 % backstepping on the motor dynamics: desired MOTOR torques (always called tau)
 [controlParam.tau_m,controlParam.dtheta_ref] = motorController(dtheta,theta,ELASTICITY,STATE,controlParam);
 
+if CONFIG.assume_rigid_joints == 1
+
+    controlParam.tau_m  = controlParam.tauModel + controlParam.Sigma*controlParam.fcDes;
+end
+
 %% Feet pose correction
 % this will avoid numerical errors during the forward dynamics integration
 if     feet_on_ground(1) == 1 && feet_on_ground(2) == 0

controllers/torqueBalancingWalkman_JE/initializeTorqueBalancing.m

@@ -26,6 +26,8 @@
 CONFIG.demo_movements                        = 1;                          %either 0 or 1
 CONFIG.feet_on_ground                        = [1,1];                      %either 0 or 1; [left foot,right foot]
 CONFIG.use_QPsolver                          = 1;                          %either 0 or 1
+CONFIG.robot_name                            = 'bigman';
+CONFIG.assume_rigid_joints                   = 1;                          %either 0 or 1
 
 %% Visualization setup
 % robot simulator
@@ -37,7 +39,7 @@
 
 %% Integration time [s]
 CONFIG.tStart                                = 0;
-CONFIG.tEnd                                  = 10;
+CONFIG.tEnd                                  = 2.5;
 CONFIG.sim_step                              = 0.01;
 
 %% %%%%%%%%%%%%%%%%%%%%%%%%%% ADVANCED SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%% %%
@@ -79,7 +81,7 @@
 CONFIG.figureCont = 1;
 
 %% Initialize the robot model
-wbm_modelInitialize('bigman');
+wbm_modelInitialize(CONFIG.robot_name);
 CONFIG.ndof = 25;
 
 %% Initial joints position [deg]

controllers/torqueBalancingWalkman_JE/src/stackOfTaskController.m

@@ -136,8 +136,14 @@
 
 % reference for motor variables and contact forces nullspace
 controlParam.ddtheta_ref  = zeros(ndof,1);
-tauModel                  = pinvLambda*(JcMinv*h - dJc_nu) + ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*dqj + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)...
-                           + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde);
+
+if CONFIG.assume_rigid_joints == 1
+    tauModel                  = pinvLambda*(JcMinv*h - dJc_nu) + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)...
+                                + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde);
+else
+    tauModel                  = pinvLambda*(JcMinv*h - dJc_nu) + ELASTICITY.KS*(qj-theta)+ELASTICITY.KD*dqj + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)...
+                                + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde);
+end
 
 %% Desired contact forces computation
 fcHDot             = pinvA*(HDotDes - f_grav);

controllers/torqueBalancing_JE/config/gains.m

@@ -0,0 +1,74 @@
+function gainsInit = gains(CONFIG)
+%GAINS generates initial control gains for both the momentum task 
+%(primary task in SoT controller) and the postural task.
+%
+% gains = GAINS(CONFIG) takes as an input the structure CONFIG, which
+% contains all the robot configuration parameters. The output is the structure 
+% gainsInit, which contains the initial gains matrices.
+%
+% Author : Gabriele Nava (gabriele.nava@iit.it)
+% Genova, May 2016
+%
+
+% ------------Initialization----------------
+% Config parameters
+ndof = CONFIG.ndof;
+
+%% Gains for two feet on ground
+if sum(CONFIG.feet_on_ground) == 2
+
+    % CoM and angular momentum gains
+    gainsPCoM           = diag([40 45 40]);
+    gainsDCoM           = 2*sqrt(gainsPCoM);
+    gainsPAngMom        = diag([1 5 1]);
+    gainsDAngMom        = 2*sqrt(gainsPAngMom);
+
+    % impedances acting in the null space of the desired contact forces
+    impTorso            = [ 20  30  20];
+    impArms             = [ 10  10  10   5   5];
+    impLeftLeg          = [ 35  40  10  30   5  10];
+    impRightLeg         = [ 35  40  10  30   5  10];
+end
+
+%% Parameters for one foot on ground
+if  sum(CONFIG.feet_on_ground) == 1
+
+    % CoM and angular momentum gains
+    gainsPCoM          = diag([30 35 30]);
+    gainsDCoM          = 2*sqrt(gainsPCoM);
+    gainsPAngMom       = diag([2.5 5 2.5]);
+    gainsDAngMom       = 2*sqrt(gainsPAngMom);
+
+    % impedances acting in the null space of the desired contact forces
+    impTorso           = [ 10   15   10];
+    impArms            = [  5    5    5   2.5   2.5];
+
+    if CONFIG.feet_on_ground(1) == 1
+
+        impLeftLeg     = [ 15   15  15  15  5  5];
+        impRightLeg    = [ 10   10  15  15  5  5];
+    else
+        impLeftLeg     = [ 10   10  15  15  5  5];
+        impRightLeg    = [ 15   15  15  15  5  5];
+    end
+end
+
+%% Definition of the impedances and dampings vectors
+gainsInit.impedances   = [impTorso,impArms,impArms,impLeftLeg,impRightLeg];
+gainsInit.dampings     = 2*sqrt(gainsInit.impedances);
+
+if (size(gainsInit.impedances,2) ~= ndof)
+
+    error('Dimension mismatch between ndof and dimension of the variable impedences. Check these variables in the file gains.m');
+end
+
+%% Momentum and postural gains
+gainsInit.impedances         = diag(gainsInit.impedances);
+gainsInit.dampings           = diag(gainsInit.dampings);
+gainsInit.momentumGains      = [gainsDCoM zeros(3); zeros(3) gainsDAngMom];
+gainsInit.intMomentumGains   = [gainsPCoM zeros(3); zeros(3) gainsPAngMom];
+
+% gains for feet correction to avoid numerical intrgration errors
+gainsInit.corrPosFeet        = 100;
+
+end

controllers/torqueBalancing_JE/config/trajectoryGenerator.m

@@ -0,0 +1,81 @@
+function  desired_x_dx_ddx_CoM = trajectoryGenerator(xCoMInit,t,CONFIG)
+%TRAJECTORYGENERATOR generates a desired CoM trajectory. The default trajectory
+%                    is a sinusoid in the Y direction.
+%
+% desired_x_dx_ddx_CoM = TRAJECTORYGENERATOR(xCoMInit,t,CONFIG) takes as an 
+% input the initial CoM position, xCoMInit, the current time t and the structure
+% CONFIG which contains the user-defined parameters.
+% The output desired_x_dx_ddx_CoM is a matrix [3x3] which contains the CoM
+% reference acceleration, velocity and position.
+%
+% Author : Gabriele Nava (gabriele.nava@iit.it)
+% Genova, May 2016
+%
+
+% ------------Initialization----------------
+% Config parameters
+feet_on_ground             = CONFIG.feet_on_ground;
+demo_movements             = CONFIG.demo_movements;
+
+% Initial parameters
+directionOfOscillation     = [0;0;0];
+referenceParams            = [0.0 0.0];   %referenceParams(1) = amplitude of ascillations in meters
+                                          %referenceParams(2) = frequency of ascillations in Hertz
+
+% NB: IF THE INTEGRATION IS PERFORMED USING ODE15S THIS PARAMETER SHOULD REMAIN ZERO                                           
+noOscillationTime          = 0;           % If params.demo_movements = 1, the variable noOscillationTime is the time, in seconds,
+                                          % that the robot waits before starting the left-and-right
+
+%% Trajectory definition
+if  demo_movements == 1
+
+    if  sum(feet_on_ground) == 2
+
+        directionOfOscillation = [0;1;0];
+        referenceParams        = [0.035 0.35];
+    else
+
+        directionOfOscillation = [0;1;0];
+        referenceParams        = [0.015 0.15];
+    end
+end
+
+%% Trajectory generation
+frequency  = referenceParams(2);
+
+if size(t,1)==1 && size(t,2)==1
+
+    if t >= noOscillationTime
+
+        amplitude  = referenceParams(1);
+    else
+        amplitude  = 0;
+    end
+
+    xCoMDes    =  xCoMInit + amplitude*sin(2*pi*frequency*t)*directionOfOscillation;
+    dxCoMDes   =             amplitude*2*pi*frequency*cos(2*pi*frequency*t)*directionOfOscillation;
+    ddxCoMDes  =           - amplitude*(2*pi*frequency)^2*sin(2*pi*frequency*t)*directionOfOscillation;
+
+    desired_x_dx_ddx_CoM = [xCoMDes dxCoMDes ddxCoMDes];
+else
+
+    % in case t is a vector
+    desired_x_dx_ddx_CoM = zeros(3,3,length(t));
+
+    for k = 1:length(t)
+        if t(k) >= noOscillationTime
+
+            amplitude  = referenceParams(1);
+         else
+            amplitude  = 0;
+        end
+
+        xCoMDes    =  xCoMInit + amplitude*sin(2*pi*frequency*t(k))*directionOfOscillation;
+        dxCoMDes   =             amplitude*2*pi*frequency*cos(2*pi*frequency*t(k))*directionOfOscillation;
+        ddxCoMDes  =           - amplitude*(2*pi*frequency)^2*sin(2*pi*frequency*t(k))*directionOfOscillation;
+
+        desired_x_dx_ddx_CoM(:,:,k) = [xCoMDes dxCoMDes ddxCoMDes]; 
+    end
+end
+
+

controllers/torqueBalancing_JE/init/initForwardDynamics.m

@@ -0,0 +1,92 @@
+function [] = initForwardDynamics(CONFIG)
+%INITFORWARDDYNAMICS configures the forward dynamics integration by setting
+%                    state initial conditions, control gains, etc.  
+%
+% [] = INITFORWARDDYNAMICS(CONFIG) takes as input the structure CONFIG 
+% containing the initial user-defined parameters. It has no output. The 
+% forward dynamics integration will be performed following the options 
+% the user specified in the initialization script.
+%
+% Author : Gabriele Nava (gabriele.nava@iit.it)
+% Genova, May 2016
+%
+
+% ------------Initialization----------------
+%% Setup the configuration and state parameters
+feet_on_ground               = CONFIG.feet_on_ground;
+ndof                         = CONFIG.ndof;
+qjInit                       = CONFIG.qjInit;
+dqjInit                      = zeros(ndof,1);
+dx_bInit                     = zeros(3,1);
+w_omega_bInit                = zeros(3,1);
+
+% initialize motor state
+thetaInit                    = qjInit;
+dthetaInit                   = dqjInit;
+chi_motorInit                = [thetaInit;dthetaInit];
+
+%% Contact constraints definition
+if sum(feet_on_ground) == 2
+
+    CONFIG.constraintLinkNames = {'l_sole','r_sole'};
+
+elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 0
+
+    CONFIG.constraintLinkNames = {'l_sole'};
+
+elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1
+
+    CONFIG.constraintLinkNames = {'r_sole'};
+end
+
+CONFIG.numConstraints = length(CONFIG.constraintLinkNames);
+
+%% Configure the model using initial conditions
+wbm_updateState(qjInit,dqjInit,[dx_bInit;w_omega_bInit]);
+
+% fixing the world reference frame w.r.t. the foot on ground position
+if  feet_on_ground(1) == 1
+
+    [x_bInit,w_R_bInit] = wbm_getWorldFrameFromFixLnk('l_sole',qjInit);
+else
+    [x_bInit,w_R_bInit] = wbm_getWorldFrameFromFixLnk('r_sole',qjInit);
+end
+
+wbm_setWorldFrame(w_R_bInit,x_bInit,[0 0 -9.81]')
+
+% initial state (floating base + joints)
+[basePoseInit,~,~,~]          = wbm_getState();
+chi_robotInit                 = [basePoseInit; qjInit; dx_bInit; w_omega_bInit; dqjInit];
+
+%% Initial state that consider also joints elasticity
+chiInit                       = [chi_robotInit; chi_motorInit];
+
+%% Initial gains
+% the initial gains are defined before the numerical integration
+CONFIG.gainsInit              = gains(CONFIG);
+
+%% Initial dynamics and forward kinematics
+% initial state
+CONFIG.initState              = robotState(chi_robotInit,CONFIG);
+% initial dynamics
+CONFIG.initDynamics           = robotDynamics(CONFIG.initState,CONFIG);
+% initial forward kinematics
+CONFIG.initForKinematics      = robotForKinematics(CONFIG.initState,CONFIG.initDynamics);
+
+%% Forward dynamics integration
+CONFIG.wait           = waitbar(0,'Forward dynamics integration...');
+forwardDynFunc        = @(t,chi)forwardDynamics(t,chi,CONFIG);
+
+% either fixed step integrator or ODE15s
+if CONFIG.integrateWithFixedStep == 1
+    [t,chi]           = euleroForward(forwardDynFunc,chiInit,CONFIG.tEnd,CONFIG.tStart,CONFIG.sim_step);
+else
+    [t,chi]           = ode15s(forwardDynFunc,CONFIG.tStart:CONFIG.sim_step:CONFIG.tEnd,chiInit,CONFIG.options);
+end
+
+delete(CONFIG.wait)
+
+%% Visualize integration results and robot simulator
+CONFIG.figureCont     = initVisualizer(t,chi,CONFIG);
+
+end