Merge pull request #71 from robotology/controllers_dev

Controllers dev

.startup_mexWholeBodyModel.m.in

@@ -0,0 +1,45 @@
+%% startup_mexWholeBodyModel.m
+%  Run this script only once to permanently add the required folders for using mexWholebodymodel toolbox  to your 
+%  MATLAB path. 
+
+fprintf('\nmexWholeBodyModel Toolbox\n');
+
+installDir = '@CMAKE_INSTALL_PREFIX@';
+mexDir     = [installDir, filesep, 'mex'];
+mexWrapDir = [mexDir, filesep, 'mexwbi-wrappers'];
+mexUtDir   = [mexDir, filesep, 'mexwbi-utilities'];
+
+if exist(mexDir, 'dir')
+    addpath(mexDir);
+end
+
+if exist(mexUtDir, 'dir')
+    addpath(mexUtDir);
+end
+
+if exist(mexWrapDir, 'dir')
+    addpath(mexWrapDir);
+end
+
+fileDir               = userpath;
+pathSeparatorLocation = strfind(fileDir, pathsep);
+
+if isempty(fileDir)
+    error('Empty userpath. Please set the userpath before running this script');
+elseif size(pathSeparatorLocation, 2) > 1
+    error('Multiple userpaths. Please set a single userpath before running this script');
+end
+
+if (~isempty(pathSeparatorLocation))
+    fileDir(pathSeparatorLocation) = [];
+end
+
+fprintf('Saving paths to %s\n\n', [fileDir, filesep, 'pathdef.m']);
+
+if (~savepath([fileDir, filesep, 'pathdef.m']))
+    fprintf(['A file called pathdef.m has been created in your %s folder.\n', ...
+        'This should be enough to permanentely add all the mexWholeBodyModel-Toolbox to ', ...
+        'your MATLAB installation.\n'], fileDir);
+else
+    disp('There was an error generating pathdef.m To proceed please manually add the contents of variables mexDir, mexUtDir, mexWrapDir to your matlabpath');
+end

CMakeLists.txt

@@ -59,3 +59,11 @@ enable_testing()
 
 add_subdirectory(tests)
 
+#STARTUP mexWholeBodyModel-TOOLBOX
+# The following line is to properly configure the installation script of the mexWholeBodyModel-toolbox
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/.startup_mexWholeBodyModel.m.in ${CMAKE_BINARY_DIR}/startup_mexWholeBodyModel.m)
+# Install configuration files
+install(FILES ${CMAKE_BINARY_DIR}/startup_mexWholeBodyModel.m DESTINATION mex)
+
+
+

README.md

@@ -5,29 +5,23 @@ Matlab MEX interface to the wholeBodyModel C++ interface, implemented to be able
 It is recommended to install the `mex-wholebodymodel` throught the [`codyco-superbuild`](https://github.com/robotology/codyco-superbuild/).
 Once you installed the `codyco-superbuild`, the `mex-wholebodymodel` files should have been installed in
 `${CODYCO_SUPERBUILD_ROOT}/build/install/mex` and its subdirectories (for the `mex-wholebodymodel`, this directories are `mexwbi-wrappers` and `mexwbi-utilities`).
-To execute scripts that use `mex-wholebodymodel`, make sure that this directories are part of your [MATLAB search path](http://www.mathworks.com/help/matlab/ref/path.html).
+To execute scripts that use `mex-wholebodymodel`, make sure that this directories are part of your [MATLAB search path](http://www.mathworks.com/help/matlab/ref/path.html). To this purpose, run only once the script `startup_mexWholeBodyModel.m` in 
+`${CODYCO_SUPERBUILD_ROOT}/build/install/mex`. This should be enough to premanently add the required directories to your MATLAB path.
 
 ## Examples 
 
 ### Rigid Body Dynamics
 An example on how to use mex-wholebodymodel to compute the dynamics quantities of 
 a rigid body is available at [examples/rigidBodyDynamics.m](examples/rigidBodyDynamics.m).
 
-## Matlab-src
-
-### Controllers simulations
-In [matlab-src/torqueBalancing_matlab](matlab-src/torqueBalancing_matlab/) the user can find two different
-simulations of whole-body controller implemented using the mex-wholebodymodel interface. One uses the Stack of task approach while 
-the other is a Joint Space controller. 
-For more information please check the relative [README_SoT](matlab-src/torqueBalancing_matlab/StackOfTask_balancing/) and
-[README_Jc](matlab-src/torqueBalancing_matlab/JointSpace_balancing/)
-
-### Joint space linearization
-In [matlab-src/jointSpaceLinearization](matlab-src/jointSpaceLinearization/) there is a program which linearize the joint space of the system, when balancing 
-on 1 foot, controlled with 'Stack of Task' approach. For further informations check the relative [README](matlab-src/jointSpaceLinearization/).
+## Controllers
+In [controllers](controllers/) the user can find different
+simulations of whole-body controller implemented using the mex-wholebodymodel interface. The official controller version is
+in the folder [controllers/torqueBalancing](controllers/torqueBalancing/), while in the folder
+[controllers/experiments](controllers/experiments/) other balancing controllers are implemented. 
+All the utility function for control are in [controllers/utilityMatlabFunctions](controllers/utilityMatlabFunctions/). For more informations about the balancing controller check the relative [README](controllers/torqueBalancing/).
 
 ## Mex-wholebodymodel
-
 This folder contains all the matlab and C++ utilities used for tests and controllers.
 
 ## Tests

controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m

@@ -0,0 +1,132 @@
+%% INITIALIZETORQUEBALANCING
+%
+% This is the initialization script for torque balancing simulation of the robot
+% iCub using Matlab.
+% The user can set the parameters below to generate different simulations.
+% The forward dynamics integration is available for both the robot balancing
+% on one foot and two feet, and for the robot standing or moving, following
+% a CoM trajectory. It is also possible to use a QP program to ensure the contact
+% forces at feet are inside the friction cones. A linearization setup for both
+% analysis and gains tuning purpose is available, too.
+%
+% Author : Gabriele Nava (gabriele.nava@iit.it)
+% Genova, May 2016
+%
+
+% ------------Initialization----------------
+clear
+close all
+clc
+%% %%%%%%%%%%%%%%%%%%%%%%%%%%% BASIC SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%
+%% Configure the simulation
+CONFIG.demo_movements                        = 0;                          %either 0 or 1
+CONFIG.feet_on_ground                        = [0,1];                      %either 0 or 1; [left,right]
+
+%% QP solver and gains tuning procedure
+CONFIG.use_QPsolver                          = 0;                          %either 0 or 1
+CONFIG.gains_tuning                          = 1;                          %either 0 or 1
+
+%% Visualization setup
+% robot simulator
+CONFIG.visualize_robot_simulator             = 1;                          %either 0 or 1
+% forward dynamics integration results
+CONFIG.visualize_integration_results         = 0;                          %either 0 or 1
+CONFIG.visualize_joints_dynamics             = 1;                          %either 0 or 1
+% linearization and gains tuning
+CONFIG.visualize_gains_tuning_results        = 1;                          %either 0 or 1; available only if gains_tuning = 1
+
+%% Integration time [s]
+CONFIG.tStart                                = 0;
+CONFIG.tEnd                                  = 5;
+CONFIG.sim_step                              = 0.01;
+
+%% Generate the joint references with the inverse kinematics solver
+CONFIG.jointRef_with_ikin                    = 1;                          %either 0 or 1
+CONFIG.visualize_ikin_results                = 0;                          %either 0 or 1
+CONFIG.ikin_integration_step                 = 0.01;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% %%%%%%%%%%%%%%%%%%%%%%%%%% ADVANCED SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%% %%
+% ONLY FOR DEVELOPERS
+% tolerances for pseudoinverse and QP
+CONFIG.pinv_tol           = 1e-8;
+CONFIG.pinv_damp          = 5e-6;
+CONFIG.reg_HessianQP      = 1e-3;
+
+%% Verify the joint space linearization; stability analysis procedure
+% run the simulation with CONFIG.linearizationDebug = 1 to verify that the
+% joint space linearization is performed properly, and to check the
+% controlled system's stability
+CONFIG.linearizationDebug  = 1;                                            %either 0 or 1
+
+% enter in debug mode
+if CONFIG.linearizationDebug == 1
+
+    CONFIG.demo_movements  = 0;
+    CONFIG.use_QPsolver    = 0;
+end
+
+%% Forward dynamics integration setup
+% CONFIG.integrateWithFixedStep will use a Euler forward integrator instead
+% of ODE15s to integrate the forward dynamics.
+CONFIG.integrateWithFixedStep = 0;                                         %either 0 or 1
+
+% The fixed step integration needs a desingularization of system mass matrix
+% to converge to a solution
+if CONFIG.integrateWithFixedStep == 1
+
+    CONFIG.massCorr = 0.05;
+else
+    CONFIG.massCorr = 0;
+end
+
+% integration options
+CONFIG.options                   = odeset('RelTol',1e-6,'AbsTol',1e-6);
+
+%% Visualization setup
+% this script modifies the default MATLAB options for figures and graphics
+plot_set
+
+% this is the figure counter. It is used to automatically adapt the figure
+% number in case new figures are added
+CONFIG.figureCont                = 1;
+
+%% Initialize the robot model
+wbm_modelInitialise('icubGazeboSim');
+CONFIG.ndof  = 25;
+
+%% Initial joints position [deg]
+leftArmInit  = [ -20  30  0  45  0]';
+rightArmInit = [ -20  30  0  45  0]';
+torsoInit    = [ -10   0  0]';
+
+if       CONFIG.feet_on_ground(1) == 1 && CONFIG.feet_on_ground(2) == 1
+
+    % initial conditions for balancing on two feet
+    leftLegInit  = [  25.5   0.1   0  -18.5  -5.5  -0.1]';
+    rightLegInit = [  25.5   0.1   0  -18.5  -5.5  -0.1]';
+
+elseif   CONFIG.feet_on_ground(1) == 1 && CONFIG.feet_on_ground(2) == 0
+
+    % initial conditions for the robot standing on the left foot
+    leftLegInit  = [  25.5   15   0  -18.5  -5.5  -0.1]';
+    rightLegInit = [  25.5    5   0  -40    -5.5  -0.1]';
+
+elseif   CONFIG.feet_on_ground(1) == 0 && CONFIG.feet_on_ground(2) == 1
+
+    % initial conditions for the robot standing on the right foot
+    leftLegInit  = [  25.5    5   0  -40    -5.5  -0.1]';
+    rightLegInit = [  25.5   15   0  -18.5  -5.5  -0.1]';
+end
+
+% joint configuration [rad]
+CONFIG.qjInit = [torsoInit;leftArmInit;rightArmInit;leftLegInit;rightLegInit]*(pi/180);
+
+%% %%%%%%%%%%%%%%%%%%%%% FORWARD DYNAMICS INTEGRATION %%%%%%%%%%%%%%%%%% %%
+addpath('./src');
+addpath('./../../utilityMatlabFunctions');
+addpath('./../../utilityMatlabFunctions/LinearizationAndGainTuning');
+addpath('./../../utilityMatlabFunctions/RobotFunctions');
+addpath('./../../utilityMatlabFunctions/InverseKinematics');
+addpath('./../../utilityMatlabFunctions/Visualization');
+initForwardDynamics(CONFIG);

controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m

@@ -0,0 +1,102 @@
+function [dchi,visualization] = forwardDynamics(t,chi,CONFIG)
+%FORWARDDYNAMICS is the function that will be integrated in the forward
+%                dynamics integrator.
+%
+%             [dchi,visualization] = FORWARDDYNAMICS(t,chi,config) takes
+%             as input the current time step, T; the robot state, CHI
+%             [13+2ndof x 1]; the structure CONFIG which contains the
+%             user-defined parameters.
+%             The output are the vector to be integrated, DCHI [13+2ndof x1]
+%             and the structure VISUALIZATION which contains all the parameters
+%             used to generate the plots in the visualizer.
+%
+% Author : Gabriele Nava (gabriele.nava@iit.it)
+% Genova, May 2016
+
+% ------------Initialization----------------
+% waitbar
+waitbar(t/CONFIG.tEnd,CONFIG.wait)
+
+%% Robot Configuration
+ndof                  = CONFIG.ndof;
+gain                  = CONFIG.gain;
+qjInit                = CONFIG.qjInit;
+xCoMRef               = CONFIG.xCoMRef;
+
+%% Robot State
+STATE                 = robotState(chi,CONFIG);
+RotBase               = STATE.RotBase;
+omegaBaseWorld        = STATE.omegaBaseWorld;
+quatBase              = STATE.quatBase;
+VelBase               = STATE.VelBase;
+dqj                   = STATE.dqj;
+qj                    = STATE.qj;
+PosBase               = STATE.PosBase;
+
+%% Set the robot state (for wbm functions)
+wbm_setWorldFrame(RotBase,PosBase,[0 0 -9.81]')
+wbm_updateState(qj,dqj,[VelBase;omegaBaseWorld]);
+
+%% Robot Dynamics
+DYNAMICS              = robotDynamics(STATE,CONFIG);
+Jc                    = DYNAMICS.Jc;
+M                     = DYNAMICS.M;
+h                     = DYNAMICS.h;
+H                     = DYNAMICS.H;
+m                     = M(1,1);
+
+%% Robot Forward kinematics
+FORKINEMATICS         = robotForKinematics(STATE,DYNAMICS);
+RFootPoseEul          = FORKINEMATICS.RFootPoseEul;
+LFootPoseEul          = FORKINEMATICS.LFootPoseEul;
+xCoM                  = FORKINEMATICS.xCoM;
+
+%% Joint limits check
+% jointLimitsCheck(qj,t);
+
+%% Interpolation for joint reference trajectory with ikin
+if CONFIG.jointRef_with_ikin == 1
+    trajectory.JointReferences         = interpInverseKinematics(t,CONFIG.ikin);
+else
+    trajectory.JointReferences.ddqjRef = zeros(ndof,1);
+    trajectory.JointReferences.dqjRef  = zeros(ndof,1);
+    trajectory.JointReferences.qjRef   = qjInit;
+end
+
+%% CoM trajectory generator
+trajectory.desired_x_dx_ddx_CoM    = trajectoryGenerator(xCoMRef,t,CONFIG);
+
+%% %%%%%%%%%%%%% LINEARIZATION DEBUG AND STABILITY ANALYSIS %%%%%%%%%%%% %%
+if CONFIG.linearizationDebug  == 1
+    % linearized joint accelerations
+    visualization.ddqjLin    = trajectory.JointReferences.ddqjRef-CONFIG.linearization.KS*(qj-trajectory.JointReferences.qjRef)...
+                              -CONFIG.linearization.KD*(dqj-trajectory.JointReferences.dqjRef);
+end
+
+%% Balancing controller
+controlParam    =  runController(gain,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE);
+tau             =  controlParam.tau;
+fc              =  controlParam.fc;
+
+%% State derivative computation
+omegaWorldBase  = transpose(RotBase)*omegaBaseWorld;
+dquatBase       = quaternionDerivative(omegaWorldBase,quatBase);
+NuQuat          = [VelBase;dquatBase;dqj];
+dNu             = M\(Jc'*fc + [zeros(6,1); tau]-h);
+% state derivative
+dchi            = [NuQuat;dNu];
+
+%% Parameters for visualization
+visualization.ddqjNonLin  = dNu(7:end);
+visualization.dqj         = dqj;
+visualization.qj          = qj;
+visualization.JointRef    = trajectory.JointReferences;
+visualization.xCoM        = xCoM;
+visualization.poseFeet    = [LFootPoseEul;RFootPoseEul];
+visualization.H           = H;
+visualization.HRef        = [m*trajectory.desired_x_dx_ddx_CoM(:,2);zeros(3,1)];
+visualization.fc          = fc;
+visualization.f0          = controlParam.f0;
+visualization.tau         = tau;
+
+end