diff --git a/.startup_mexWholeBodyModel.m.in b/.startup_mexWholeBodyModel.m.in new file mode 100644 index 0000000..c5a0e2b --- /dev/null +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index cc893e1..b58246e 100644 --- a/CMakeLists.txt +++ b/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) + + + diff --git a/README.md b/README.md index 060fdbb..1a4879f 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,8 @@ 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 @@ -13,21 +14,14 @@ To execute scripts that use `mex-wholebodymodel`, make sure that this directorie 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 diff --git a/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m b/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m new file mode 100644 index 0000000..0fdff5a --- /dev/null +++ b/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); diff --git a/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m b/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m new file mode 100644 index 0000000..8a753bd --- /dev/null +++ b/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 diff --git a/controllers/experiments/torqueBalancingGainTuning/src/gains.m b/controllers/experiments/torqueBalancingGainTuning/src/gains.m new file mode 100644 index 0000000..92b964f --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/gains.m @@ -0,0 +1,77 @@ +function gainsInit = gains(CONFIG) +%GAINS generates the initial gains matrices 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 utility parameters, and the structure DYNAMICS +% which contains the robot dynamics. 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 the ground +if sum(CONFIG.feet_on_ground) == 2 + + gainsPCoM = diag([45 50 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 35.6 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 the ground +if sum(CONFIG.feet_on_ground) == 1 + + gainsPCoM = diag([40 45 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 53 16 33]; + impArms = [ 25 20 25 20 25]; + + if CONFIG.feet_on_ground(1) == 1 + + impLeftLeg = [ 70 70 65 30 10 10]; + impRightLeg = [ 20 20 20 10 10 10]; + else + impLeftLeg = [ 20 20 20 10 10 10]; + impRightLeg = [ 30 30 25 30 10 10]; + 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]; + +% Desired shape for the state matrix of the linearized system, for gains tuning procedure +gainsInit.KSdes = gainsInit.impedances; +gainsInit.KDdes = 2*sqrt(gainsInit.KSdes); + +% Gains for feet correction to avoid numerical errors +gainsInit.CorrPosFeet = 5; + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/initForwardDynamics.m b/controllers/experiments/torqueBalancingGainTuning/src/initForwardDynamics.m new file mode 100644 index 0000000..8d5202a --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/initForwardDynamics.m @@ -0,0 +1,131 @@ +function [] = initForwardDynamics(CONFIG) +%INITFORWARDDYNAMICS setup the forward dynamics integration of robot iCub +% in MATLAB. +% +% [] = INITFORWARDDYNAMICS(config) takes as input the structure +% CONFIG containing all the configuration parameters. It has no +% output. The forward dynamics integration will be performed +% following the options the user specifies in the initialization +% file. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +qjInit = CONFIG.qjInit; + +%% Contact constraints definition +if feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + 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); + +%% Initial conditions +dqjInit = zeros(ndof,1); +VelBaseInit = zeros(3,1); +omegaBaseWorldInit = zeros(3,1); + +% Update the initial position +wbm_updateState(qjInit,dqjInit,[VelBaseInit;omegaBaseWorldInit]); + +% Fixing the world reference frame w.r.t. the foot on ground position +if feet_on_ground(1) == 1 + + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('l_sole',qjInit); +else + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('r_sole',qjInit); +end + +wbm_setWorldFrame(RotBaseInit,PosBaseInit,[0 0 -9.81]') + +% Initial base pose; initial robot state +[~,BasePoseInit,~,~] = wbm_getState(); +chiInit = [BasePoseInit; qjInit; VelBaseInit; omegaBaseWorldInit; dqjInit]; +CONFIG.initState = robotState(chiInit,CONFIG); + +%% Initial gains +CONFIG.gainsInit = gains(CONFIG); + +%% Joint references with inverse kinematics +if CONFIG.jointRef_with_ikin == 1 + + [CONFIG.ikin,chiInit,CONFIG.figureCont] = initInverseKinematics(CONFIG); + CONFIG.initState = robotState(chiInit,CONFIG); +end + +%% Initial dynamics and forward kinematics +% Initial dynamics +CONFIG.initDynamics = robotDynamics(CONFIG.initState,CONFIG); +% Initial forward kinematics +CONFIG.initForKinematics = robotForKinematics(CONFIG.initState,CONFIG.initDynamics); +CONFIG.xCoMRef = CONFIG.initForKinematics.xCoM; + +%% %%%%%%%%%%%%% LINEARIZATION DEBUG AND STABILITY ANALYSIS %%%%%%%%%%%% %% +if CONFIG.linearizationDebug == 1 + + % the initial configuration is changed by a small delta, but the references + % are not updated. In this way the robot will move to the reference position + qjInit(1:13) = qjInit(1:13)-1*pi/180; + + wbm_updateState(qjInit,dqjInit,[VelBaseInit;omegaBaseWorldInit]); + + if feet_on_ground(1) == 1 + + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('l_sole',qjInit); + else + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('r_sole',qjInit); + end + + wbm_setWorldFrame(RotBaseInit,PosBaseInit,[0 0 -9.81]') + [~,BasePoseInit,~,~] = wbm_getState(); + chiInit = [BasePoseInit; qjInit; VelBaseInit; omegaBaseWorldInit; dqjInit]; + CONFIG.initState = robotState(chiInit,CONFIG); + CONFIG.initDynamics = robotDynamics(CONFIG.initState,CONFIG); + CONFIG.initForKinematics = robotForKinematics(CONFIG.initState,CONFIG.initDynamics); + +end + +% the system is linearized around the initial position, to verify the stability +CONFIG.linearization = jointSpaceLinearization(CONFIG,qjInit); + +%% Gains tuning procedure +if CONFIG.gains_tuning == 1 + + [gainsKron,CONFIG.linearization.KS,CONFIG.linearization.KD] = gainsTuning(CONFIG.linearization,CONFIG); + CONFIG.gain = gainsConstraint(gainsKron,CONFIG.linearization,CONFIG); +else + CONFIG.gain = CONFIG.gainsInit; +end + +%% 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) + +%% VISUALIZATION +CONFIG.figureCont = initVisualizer(t,chi,CONFIG); + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/initVisualizer.m b/controllers/experiments/torqueBalancingGainTuning/src/initVisualizer.m new file mode 100644 index 0000000..9ee3c64 --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/initVisualizer.m @@ -0,0 +1,130 @@ +function figureCont = initVisualizer(t,chi,CONFIG) +%INITVISUALIZER is the main function for visualize the results of iCub forward +% dynamics integration in MATLAB. +% +% INITVISUALIZER visualizes some outputs from the forward dynamics +% integration (e.g. the robot state, contact forces, control torques...). +% Also, for the "stack of task" controller, it is possible to visualize +% the linearization results (both soundness of linearization and gains tuning). +% +% figureCont = INITVISUALIZER(t,chi,config) takes as input the integration +% time T, the robot state CHI and the structure CONFIG containing all the +% utility parameters. The output is a counter for the automatic correction +% of figures numbers in case a new figure is added. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +ndof = CONFIG.ndof; +initState = CONFIG.initState; + +%% ROBOT SIMULATOR +if CONFIG.visualize_robot_simulator == 1 + + CONFIG.figureCont = visualizeSimulation(t,chi,CONFIG); +end + +%% FORWARD DYNAMICS (basic parameters) +if CONFIG.visualize_integration_results == 1 || CONFIG.visualize_joints_dynamics == 1 || CONFIG.gains_tuning == 1 || CONFIG.linearizationDebug == 1 + + CONFIG.wait = waitbar(0,'Generating the plots...'); + set(0,'DefaultFigureWindowStyle','Docked'); + + % joints + qj = zeros(ndof,length(t)); + qjInit = zeros(ndof,length(t)); + dqj = zeros(ndof,length(t)); + qjRef = zeros(ndof,length(t)); + dqjRef = zeros(ndof,length(t)); + ddqjRef = zeros(ndof,length(t)); + ddqjNonLin = zeros(ndof,length(t)); + ddqjLin = zeros(ndof,length(t)); + + % contact forces and torques + fc = zeros(6*CONFIG.numConstraints,length(t)); + f0 = zeros(6*CONFIG.numConstraints,length(t)); + tau = zeros(ndof,length(t)); + normTau = zeros(length(t),1); + + % forward kinematics + xCoM = zeros(3,length(t)); + poseFeet = zeros(12,length(t)); + CoP = zeros(4,length(t)); + H = zeros(6,length(t)); + HRef = zeros(6,length(t)); + + % generate the vectors from forward dynamics + + for time = 1:length(t) + + [~,visual] = forwardDynamics(t(time), chi(time,:)', CONFIG); + + % joints + qj(:,time) = visual.qj; + qjInit(:,time) = initState.qj; + dqj(:,time) = visual.dqj; + qjRef(:,time) = visual.JointRef.qjRef; + dqjRef(:,time) = visual.JointRef.dqjRef; + ddqjRef(:,time) = visual.JointRef.ddqjRef; + ddqjNonLin(:,time) = visual.ddqjNonLin; + + % linearization debug + if CONFIG.linearizationDebug == 1 + ddqjLin(:,time) = visual.ddqjLin; + end + + %% Other parameters + % contact forces and torques + fc(:,time) = visual.fc; + f0(:,time) = visual.f0; + tau(:,time) = visual.tau; + normTau(time) = norm(visual.tau); + + % forward kinematics + xCoM(:,time) = visual.xCoM; + poseFeet(:,time) = visual.poseFeet; + H(:,time) = visual.H; + HRef(:,time) = visual.HRef; + + % centers of pressure at feet + CoP(1,time) = -visual.fc(5)/visual.fc(3); + CoP(2,time) = visual.fc(4)/visual.fc(3); + + if CONFIG.numConstraints == 2 + + CoP(3,time) = -visual.fc(11)/visual.fc(9); + CoP(4,time) = visual.fc(10)/visual.fc(9); + end + + end + + delete(CONFIG.wait) + % composed parameters + HErr = H-HRef; + + %% Basic visualization (forward dynamics integration results) + if CONFIG.visualize_integration_results == 1 + + CONFIG.figureCont = visualizeForwardDyn(t,CONFIG,xCoM,poseFeet,fc,f0,normTau,CoP,HErr); + end + + %% Joints positions and position error + if CONFIG.visualize_joints_dynamics == 1 + + tAss = 4./(sqrt(diag(CONFIG.linearization.KSdes))); + CONFIG.figureCont = visualizeJointDynamicsTuning(t,CONFIG,qj,qjRef,tAss); + end + + %% Linearization results (soundness of linearization and gains tuning) + if CONFIG.linearizationDebug == 1 + + CONFIG.figureCont = visualizeLinearizAndGains(t,CONFIG,ddqjNonLin,ddqjLin); + end + + figureCont = CONFIG.figureCont; + set(0,'DefaultFigureWindowStyle','Normal'); + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/runController.m b/controllers/experiments/torqueBalancingGainTuning/src/runController.m new file mode 100644 index 0000000..3ab0556 --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/runController.m @@ -0,0 +1,73 @@ +function controlParam = runController(gains,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE) +%RUNCONTROLLER is the initialization function for iCub balancing controllers +% in Matlab. +% +% controlParam = RUNCONTROLLER(gains,trajectory,dynamics, +% forKinematics,config,state) takes as input the control gains, +% the joint reference trajectory, all the configuration parameters +% and the robot dynamics, forward kinematics and state. The output +% is the structure CONTROLPARAM which contains the control torques +% TAU, the contact forces FC and other parameters for visualization. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Feet correction gains +KCorrPos = gains.CorrPosFeet; +KCorrVel = 2*sqrt(KCorrPos); + +%% Config parameters +ndof = CONFIG.ndof; +feet_on_ground = CONFIG.feet_on_ground; +use_QPsolver = CONFIG.use_QPsolver; +initForKinematics = CONFIG.initForKinematics; +S = [zeros(6,ndof); + eye(ndof,ndof)]; + +%% Dynamics +dJcNu = DYNAMICS.dJcNu; +M = DYNAMICS.M; +Jc = DYNAMICS.Jc; +h = DYNAMICS.h; +JcMinv = Jc/M; +JcMinvS = JcMinv*S; + +%% Forward Kinematics +VelFeet = FORKINEMATICS.VelFeet; +TLfoot = FORKINEMATICS.TLfoot; +TRfoot = FORKINEMATICS.TRfoot; +RFootPoseEul = FORKINEMATICS.RFootPoseEul; +LFootPoseEul = FORKINEMATICS.LFootPoseEul; +DeltaPoseRFoot = TRfoot*(RFootPoseEul-initForKinematics.RFootPoseEul); +DeltaPoseLFoot = TLfoot*(LFootPoseEul-initForKinematics.LFootPoseEul); + +%% BALANCING CONTROLLER +controlParam = stackOfTaskController(CONFIG,gains,trajectory,DYNAMICS,FORKINEMATICS,STATE); + +if use_QPsolver == 1 + % Quadratic programming solver for the nullspace of contact forces + controlParam.fcDes = QPSolver(controlParam,CONFIG,FORKINEMATICS); +end + +controlParam.tau = controlParam.tauModel + controlParam.Sigma*controlParam.fcDes; + +%% Feet pose correction +% this will avoid numerical errors during the forward dynamics integration +if feet_on_ground(1) == 1 && feet_on_ground(2) == 0 + + DeltaPoseFeet = DeltaPoseLFoot; + +elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1 + + DeltaPoseFeet = DeltaPoseRFoot; + +elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + DeltaPoseFeet = [DeltaPoseLFoot;DeltaPoseRFoot]; +end + +%% REAL CONTACT FORCES COMPUTATION +controlParam.fc = (JcMinv*transpose(Jc))\(JcMinv*h -JcMinvS*controlParam.tau -dJcNu -KCorrVel.*VelFeet-KCorrPos.*DeltaPoseFeet); + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m b/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m new file mode 100644 index 0000000..4c78553 --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m @@ -0,0 +1,159 @@ +function controlParam = stackOfTaskController(CONFIG,gains,trajectory,DYNAMICS,FORKINEMATICS,STATE) +%STACKOFTASKCONTROLLER implements a momentum-based control algorithm to +% control the robot iCub. +% +% STACKOFTASKCONTROLLER computes the desired control torques at joints +% using a task-based approach. The first task is the control of robot's +% momentum, while the second task is a postural task. +% +% controlParam = STACKOFTASKCONTROLLER(config, gains, trajectory, +% dynamics, forKinematics, state) takes as input the structure CONFIG, +% which contains all the configuration parameters, while the other +% structures contain the control gains, the desired trajectory, the robot +% dynamics, forward kinematics and state. +% The output is the structure CONTROLPARAM which contains the desired +% control torques, the desired contact forces and others parameters used +% for visualization and QP solver. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +pinv_tol = CONFIG.pinv_tol; +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +% pinv_damp = CONFIG.pinv_damp; + +%% Gains +impedances = gains.impedances; +dampings = gains.dampings; +MomentumGains = gains.MomentumGains; +intMomentumGains = gains.intMomentumGains; + +%% Dynamics +M = DYNAMICS.M; +g = DYNAMICS.g; +CNu = DYNAMICS.CNu; +dJcNu = DYNAMICS.dJcNu; +H = DYNAMICS.H; +Jc = DYNAMICS.Jc; +h = g + CNu; +m = M(1,1); +Mb = M(1:6,1:6); +Mbj = M(1:6,7:end); +Mj = M(7:end,7:end); +Mjb = M(7:end,1:6); +Mbar = Mj - Mjb/Mb*Mbj; +% The centroidal momentum jacobian is reduced to the joint velocity. This +% is then used to compute the approximation of the angular momentum integral +JH = DYNAMICS.JH; +JG = JH(:,7:end) -JH(:,1:6)*(eye(6)/Jc(1:6,1:6))*Jc(1:6,7:end); + +%% Forward kinematics +xCoM = FORKINEMATICS.xCoM; +dxCoM = FORKINEMATICS.dxCoM; +RFootPoseEul = FORKINEMATICS.RFootPoseEul; +LFootPoseEul = FORKINEMATICS.LFootPoseEul; + +%% Robot State +qj = STATE.qj; +dqj = STATE.dqj; + +% Trajectory +x_dx_ddx_CoMDes = trajectory.desired_x_dx_ddx_CoM; +ddqjRef = trajectory.JointReferences.ddqjRef; +dqjRef = trajectory.JointReferences.dqjRef; +qjRef = trajectory.JointReferences.qjRef; +qjTilde = qj-qjRef; +dqjTilde = dqj-dqjRef; + +% General parameters +gravAcc = 9.81; +S = [zeros(6,ndof); + eye(ndof,ndof)]; +f_grav = [zeros(2,1); + -m*gravAcc; + zeros(3,1)]; + +%% Multiplier of contact wrenches at CoM +posRFoot = RFootPoseEul(1:3); +posLFoot = LFootPoseEul(1:3); +distRF = posRFoot - xCoM; % Application point of the contact force on the right foot w.r.t. CoM +distLF = posLFoot - xCoM; % Application point of the contact force on the left foot w.r.t. CoM +AL = [eye(3), zeros(3); + skew(distLF),eye(3)]; +AR = [eye(3), zeros(3); + skew(distRF),eye(3)]; + +% One foot or two feet on ground selector +if sum(feet_on_ground) == 2 + + A = [AL,AR]; + pinvA = pinv(A,pinv_tol); +else + if feet_on_ground(1) == 1 + + A = AL; + elseif feet_on_ground(2) == 1 + + A = AR; + end + pinvA = eye(6)/A; +end + +%% Desired momentum derivative +% closing the loop on angular momentum integral +deltaPhi = JG(4:end,:)*(qj-qjRef); +accDesired = [m.*x_dx_ddx_CoMDes(:,3); zeros(3,1)]; +velDesired = -MomentumGains*[m.*(dxCoM-x_dx_ddx_CoMDes(:,2)); H(4:end)]; +posDesired = -intMomentumGains*[m.*(xCoM-x_dx_ddx_CoMDes(:,1)); deltaPhi]; +HDotDes = accDesired + velDesired + posDesired; + +%% Control torques equation +JcMinv = Jc/M; +Lambda = JcMinv*S; +JcMinvJct = JcMinv*transpose(Jc); +pinvLambda = pinv(Lambda,pinv_tol); +%pinvLambda = Lambda'/(Lambda*Lambda' + pinv_damp*eye(size(Lambda,1))); + +% multiplier of contact wrenches in tau0 +JBar = transpose(Jc(:,7:end)) - Mbj'/Mb*transpose(Jc(:,1:6)); + +% nullspaces +Nullfc = eye(6*CONFIG.numConstraints)-pinvA*A; +NullLambda = eye(ndof)-pinvLambda*Lambda; + +Sigma = -(pinvLambda*JcMinvJct + NullLambda*JBar); +SigmaNA = Sigma*Nullfc; + +% Postural task correction +posturalCorr = CONFIG.linearization.BNull; + +tauModel = pinvLambda*(JcMinv*h - dJcNu) + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); + +%% Desired contact forces computation +fcHDot = pinvA*(HDotDes - f_grav); + +% Desired f0 without Quadratic Programming +f0 = zeros(6,1); + +if sum(feet_on_ground) == 2 + + f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*fcHDot); +end + +fcDes = fcHDot + Nullfc*f0; + +%% Output parameters +controlParam.fcHDot = fcHDot; +controlParam.fcDes = fcDes; +controlParam.tauModel = tauModel; +controlParam.Sigma = Sigma; +controlParam.f0 = f0; +controlParam.Nullfc = Nullfc; +controlParam.A = A; + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m b/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m new file mode 100644 index 0000000..2a14bf0 --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m @@ -0,0 +1,59 @@ +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 = GENERTRAJ(xCoMInit,t,config) takes as an input +% the initial CoM position, XCOMINIT, the current time T and the structure +% CONFIG which contains all the user-defined parameters. +% +% The output DESIRED_X_DX_DDX_COM is a matrix [3x3] which contains the +% 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 + +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 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]; + +end diff --git a/controllers/experiments/torqueBalancingGainTuning/src/visualizeJointDynamicsTuning.m b/controllers/experiments/torqueBalancingGainTuning/src/visualizeJointDynamicsTuning.m new file mode 100644 index 0000000..a33392b --- /dev/null +++ b/controllers/experiments/torqueBalancingGainTuning/src/visualizeJointDynamicsTuning.m @@ -0,0 +1,119 @@ +function figureCont = visualizeJointDynamicsTuning(t,CONFIG,qj,qjRef,tAss) +%VISUALIZEJOINTDYNAMICS visualizes the joint space dynamics of the iCub robot +% from forward dynamics integration. +% +% figureCont = VISUALIZEJOINTDYNAMICS(t,config,qj,qjRef) takes as inputs the +% integration time T, a structure CONFIG which contains all the utility +% parameters, the joint position QJ and the desired joint position QJREF. +% The output is a counter for the automatic correction of figures numbers +% in case a new figure is added. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% setup parameters +figureCont = CONFIG.figureCont; + +for k = 1:length(tAss) + index(k) = sum(t>tAss(k))+1; +end + +qj = 180/pi*qj; +qjRef = 180/pi*qjRef; +step = qjRef(:,end)-qj(:,1); + +%% Joints dynamics +for k=1:5 + + % LEFT ARM + figure(figureCont) + subplot(3,2,k) + plot(t,qj(k+3,:)) + hold on + plot(tAss(k+3),qj(k+3,end-index(k+3)),'ok') + plot(t,qjRef(k+3,:),'k') + plot(t,qjRef(k+3,:)+0.05*step(k+3),'--g') + plot(t,qjRef(k+3,:)-0.05*step(k+3),'--g') + grid on + xlabel('Time [s]') + ylabel('Angle [deg]') + name = whatname('left_arm',k); + title(name) + legend('qj','qjRef') + + % RIGHT ARM + figure(figureCont+1) + subplot(3,2,k) + plot(t,qj(k+3+5,:)) + hold on + plot(tAss(k+3+5),qj(k+3+5,end-index(k+3+5)),'ok') + plot(t,qjRef(k+3+5,:),'k') + plot(t,qjRef(k+3+5,:)+0.05*step(k+3+5),'--g') + plot(t,qjRef(k+3+5,:)-0.05*step(k+3+5),'--g') + grid on + xlabel('Time [s]') + ylabel('Angle [deg]') + name = whatname('right_arm',k); + title(name) + legend('qj','qjRef') +end + +figureCont = figureCont +2; + +for k=1:6 + + % LEFT LEG + figure(figureCont) + subplot(3,2,k) + plot(t,qj(k+13,:)) + hold on + plot(tAss(k+13),qj(k+13,end-index(k+13)),'ok') + plot(t,qjRef(k+13,:),'k') + grid on + xlabel('Time [s]') + ylabel('Angle [deg]') + name = whatname('left_leg',k); + title(name) + legend('qj','qjRef') + + % RIGHT LEG + figure(figureCont+1) + subplot(3,2,k) + plot(t,qj(k+13+6,:)) + hold on + plot(tAss(k+13+6),qj(k+13+6,end-index(k+13+6)),'ok') + plot(t,qjRef(k+13+6,:),'k') + grid on + xlabel('Time [s]') + ylabel('Angle [deg]') + name = whatname('right_leg',k); + title(name) + legend('qj','qjRef') +end + +figureCont = figureCont +2; + +for k=1:3 + + % TORSO + figure(figureCont) + subplot(3,1,k) + plot(t,qj(k,:)) + hold on + plot(tAss(k),qj(k,end-index(k)),'ok') + plot(t,qjRef(k,:),'k') + plot(t,(qjRef(k,:)+0.05*step(k)),'--g') + plot(t,(qjRef(k,:)-0.05.*step(k)),'--g') + grid on + xlabel('Time [s]') + ylabel('Angle [deg]') + name = whatname('torso',k); + title(name) + legend('qj','qjRef') +end + +figureCont = figureCont +1; + +end diff --git a/controllers/experiments/torqueBalancingJointControl/initializeTorqueBalancing.m b/controllers/experiments/torqueBalancingJointControl/initializeTorqueBalancing.m new file mode 100644 index 0000000..9687c53 --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/initializeTorqueBalancing.m @@ -0,0 +1,94 @@ +%% 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 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. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +clear +close all +clc +%% %%%%%%%%%%%%%%%%%%%%%%%%%%% BASIC SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% +%% Configure the simulation +CONFIG.demo_movements = 1; %either 0 or 1 +CONFIG.feet_on_ground = [1,0]; %either 0 or 1; [left,right] + +%% Visualization setup +% robot simulator +CONFIG.visualize_robot_simulator = 1; %either 0 or 1 +% forward dynamics integration results +CONFIG.visualize_integration_results = 1; %either 0 or 1 +CONFIG.visualize_joints_dynamics = 1; %either 0 or 1 + +%% Integration time [s] +CONFIG.tStart = 0; +CONFIG.tEnd = 10; +CONFIG.sim_step = 0.01; + +%% Visualize the inverse kinematics results +CONFIG.visualize_ikin_results = 1; %either 0 or 1 +CONFIG.ikin_integration_step = 0.01; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%% ADVANCED SETUP %%%%%%%%%%%%%%%%%%%%%%%%%%% %% +% ONLY FOR DEVELOPERS +% tolerances for pseudoinverse, mass matrix and postural task correction +CONFIG.pinv_tol = 1e-8; +CONFIG.pinv_damp = 5e-6; +CONFIG.reg_HessianQP = 1e-3; +CONFIG.massCorr = 0; + +%% Forward dynamics integration setup +if CONFIG.demo_movements == 0 + + CONFIG.options = odeset('RelTol',1e-3,'AbsTol',1e-4); +else + CONFIG.options = odeset('RelTol',1e-6,'AbsTol',1e-6); +end + +%% Initialize the robot model +wbm_modelInitialise('icubGazeboSim'); +plot_set +CONFIG.figureCont = 1; +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/RobotFunctions'); +addpath('./../../utilityMatlabFunctions/Visualization'); +addpath('./../../utilityMatlabFunctions/InverseKinematics'); +addpath('./../../utilityMatlabFunctions/CentroidalTransformation'); +initForwardDynamics(CONFIG); + diff --git a/controllers/experiments/torqueBalancingJointControl/src/forwardDynamics.m b/controllers/experiments/torqueBalancingJointControl/src/forwardDynamics.m new file mode 100644 index 0000000..3f5ba0c --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/forwardDynamics.m @@ -0,0 +1,86 @@ +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.gainsInit; +% 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 of the joint reference trajectory with ikin +trajectory.JointReferences = interpInverseKinematics(t,CONFIG.ikin); + +%% CoM trajectory generator +trajectory.desired_x_dx_ddx_CoM = trajectoryGenerator(xCoMRef,t,CONFIG); + +%% 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.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.tau = tau; + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/gains.m b/controllers/experiments/torqueBalancingJointControl/src/gains.m new file mode 100644 index 0000000..987150c --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/gains.m @@ -0,0 +1,73 @@ +function gainsInit = gains(CONFIG) +%GAINS generates the initial gains matrices 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 utility parameters, and the structure DYNAMICS +% which contains the robot dynamics. 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 the ground +if sum(CONFIG.feet_on_ground) == 2 + + gainsPCoM = diag([45 50 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 40 40 40]; + 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 the ground +if sum(CONFIG.feet_on_ground) == 1 + + gainsPCoM = diag([40 45 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 20 20 20]; + impArms = [ 15 15 45 5 5]; + + if CONFIG.feet_on_ground(1) == 1 + + impLeftLeg = [ 70 70 65 30 10 10]; + impRightLeg = [ 20 20 20 10 10 10]; + else + impLeftLeg = [ 20 20 20 10 10 10]; + impRightLeg = [ 70 70 65 30 10 10]; + 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 errors +gainsInit.CorrPosFeet = 5; + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/initForwardDynamics.m b/controllers/experiments/torqueBalancingJointControl/src/initForwardDynamics.m new file mode 100644 index 0000000..5584c0a --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/initForwardDynamics.m @@ -0,0 +1,85 @@ +function [] = initForwardDynamics(CONFIG) +%INITFORWARDDYNAMICS setup the forward dynamics integration of robot iCub +% in MATLAB. +% +% [] = INITFORWARDDYNAMICS(CONFIG) takes as input the structure +% CONFIG containing all the configuration parameters. It has no +% output. The forward dynamics integration will be performed +% following the options the user specifies in the initialization +% file. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +qjInit = CONFIG.qjInit; + +%% Contact constraints definition +if feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + 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); + +%% Initial conditions +dqjInit = zeros(ndof,1); +VelBaseInit = zeros(3,1); +omegaBaseWorldInit = zeros(3,1); + +% Update the initial position +wbm_updateState(qjInit,dqjInit,[VelBaseInit;omegaBaseWorldInit]); + +% Fixing the world reference frame w.r.t. the foot on ground position +if feet_on_ground(1) == 1 + + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('l_sole',qjInit); +else + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('r_sole',qjInit); +end + +wbm_setWorldFrame(RotBaseInit,PosBaseInit,[0 0 -9.81]') + +% Initial base pose; initial robot state +[~,BasePoseInit,~,~] = wbm_getState(); +chiInit = [BasePoseInit; qjInit; VelBaseInit; omegaBaseWorldInit; dqjInit]; +CONFIG.initState = robotState(chiInit,CONFIG); + +%% Initial gains +CONFIG.gainsInit = gains(CONFIG); + +%% Joint references with inverse kinematics +[CONFIG.ikin,chiInit,CONFIG.figureCont] = initInverseKinematics(CONFIG); + +%% Initial dynamics and forward kinematics +CONFIG.initState = robotState(chiInit,CONFIG); +% Initial dynamics +CONFIG.initDynamics = robotDynamics(CONFIG.initState,CONFIG); +% Initial forward kinematics +CONFIG.initForKinematics = robotForKinematics(CONFIG.initState,CONFIG.initDynamics); +CONFIG.xCoMRef = CONFIG.initForKinematics.xCoM; + +%% FORWARD DYNAMICS INTEGRATION +CONFIG.wait = waitbar(0,'Forward dynamics integration...'); +forwardDynFunc = @(t,chi)forwardDynamics(t,chi,CONFIG); + +[t,chi] = ode15s(forwardDynFunc,CONFIG.tStart:CONFIG.sim_step:CONFIG.tEnd,chiInit,CONFIG.options); + +delete(CONFIG.wait) + +%% VISUALIZATION +CONFIG.figureCont = initVisualizer(t,chi,CONFIG); + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/initVisualizer.m b/controllers/experiments/torqueBalancingJointControl/src/initVisualizer.m new file mode 100644 index 0000000..7c119e7 --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/initVisualizer.m @@ -0,0 +1,108 @@ +function figureCont = initVisualizer(t,chi,CONFIG) +%INITVISUALIZER is the main function for visualize the results of iCub forward +% dynamics integration in MATLAB. +% +% INITVISUALIZER visualizes some outputs from the forward dynamics +% integration (e.g. the robot state, contact forces, control torques...). +% Also, for the "stack of task" controller, it is possible to visualize +% the linearization results (both soundness of linearization and gains tuning). +% +% figureCont = INITVISUALIZER(t,chi,config) takes as input the integration +% time T, the robot state CHI and the structure CONFIG containing all the +% utility parameters. The output is a counter for the automatic correction +% of figures numbers in case a new figure is added. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +ndof = CONFIG.ndof; +initState = CONFIG.initState; + +%% ROBOT SIMULATOR +if CONFIG.visualize_robot_simulator == 1 + + CONFIG.figureCont = visualizeSimulation(t,chi,CONFIG); +end + +%% FORWARD DYNAMICS (basic parameters) +if CONFIG.visualize_integration_results == 1 || CONFIG.visualize_joints_dynamics == 1 + + CONFIG.wait = waitbar(0,'Generating the plots...'); + set(0,'DefaultFigureWindowStyle','Docked'); + + % joints + qj = zeros(ndof,length(t)); + qjInit = zeros(ndof,length(t)); + qjRef = zeros(ndof,length(t)); + + % contact forces and torques + fc = zeros(6*CONFIG.numConstraints,length(t)); + f0 = zeros(6*CONFIG.numConstraints,length(t)); + tau = zeros(ndof,length(t)); + normTau = zeros(length(t),1); + + % forward kinematics + xCoM = zeros(3,length(t)); + poseFeet = zeros(12,length(t)); + CoP = zeros(4,length(t)); + H = zeros(6,length(t)); + HRef = zeros(6,length(t)); + + % generate the vectors from forward dynamics + + for time = 1:length(t) + + [~,visual] = forwardDynamics(t(time), chi(time,:)', CONFIG); + + % joints + qj(:,time) = visual.qj; + qjInit(:,time) = initState.qj; + qjRef(:,time) = visual.JointRef.qjRef; + + %% Other parameters + % contact forces and torques + fc(:,time) = visual.fc; + tau(:,time) = visual.tau; + normTau(time) = norm(visual.tau); + + % forward kinematics + xCoM(:,time) = visual.xCoM; + poseFeet(:,time) = visual.poseFeet; + H(:,time) = visual.H; + HRef(:,time) = visual.HRef; + + % centers of pressure at feet + CoP(1,time) = -visual.fc(5)/visual.fc(3); + CoP(2,time) = visual.fc(4)/visual.fc(3); + + if CONFIG.numConstraints == 2 + + CoP(3,time) = -visual.fc(11)/visual.fc(9); + CoP(4,time) = visual.fc(10)/visual.fc(9); + end + + end + + delete(CONFIG.wait) + % composed parameters + HErr = H-HRef; + + %% Basic visualization (forward dynamics integration results) + if CONFIG.visualize_integration_results == 1 + + CONFIG.figureCont = visualizeForwardDyn(t,CONFIG,xCoM,poseFeet,fc,f0,normTau,CoP,HErr); + end + + %% Joints positions and position error + if CONFIG.visualize_joints_dynamics == 1 + + CONFIG.figureCont = visualizeJointDynamics(t,CONFIG,qj,qjRef); + end + + figureCont = CONFIG.figureCont; + set(0,'DefaultFigureWindowStyle','Normal'); + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/jointSpaceController.m b/controllers/experiments/torqueBalancingJointControl/src/jointSpaceController.m new file mode 100644 index 0000000..a1f201b --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/jointSpaceController.m @@ -0,0 +1,82 @@ +function controlParam = jointSpaceController(CONFIG,gains,trajectory,DYNAMICS,STATE) +%JOINTSPACECONTROLLER implemements a joint space balancing controller for +% the robot iCub. +% +% JOINTSPACECONTROLLER computes the control torques at joints reducing the +% system to the joint space. It compensates for the gravity terms and +% defines the desired control torques at joints oin order to follow a joint +% reference trajectory. +% +% controlParam = JOINTSPACECONTROLLER(config, gains, trajectory, dynamics, +% state) takes as input the structure CONFIG, which contains all the +% configuration parameters, the desired control gains, the reference +% trajectory, the robot dynamics and state. +% The output is the structure CONTROLPARAM which contains the desired +% control torques and other parameters for visualization. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +pinv_damp = CONFIG.pinv_damp; +ndof = CONFIG.ndof; +feet_on_ground = CONFIG.feet_on_ground; +%pinv_tol = CONFIG.pinv_tol; + +%% Dynamics +M = DYNAMICS.M; +Jc = DYNAMICS.Jc; +dqj = DYNAMICS.Nu(7:end); +g = DYNAMICS.g; +CNu = DYNAMICS.CNu; +dJcNu = DYNAMICS.dJcNu; +Mj = M(7:end,7:end); + +%% Gains and references +impedances = gains.impedances; +dampings = gains.dampings; +ddqjRef = trajectory.JointReferences.ddqjRef; +dqjRef = trajectory.JointReferences.dqjRef; +qjRef = trajectory.JointReferences.qjRef; + +%% Robot state +qj = STATE.qj; +qjTilde = qj-qjRef; +dqjTilde = dqj-dqjRef; + +%% Project the equations in the joint space +Jct = transpose(Jc); +JcMinv = Jc/M; +JcMinvJct = JcMinv*Jct; +Lambda = (JcMinvJct\JcMinv); + +% new dynamics parameters in the equations of motion after substituting the +% contact forces from the constraints equations. +gTilde = -Jct*Lambda*g; +CNuTilde = -Jct*Lambda*CNu; +dJcNuTilde = Jct*(JcMinvJct\dJcNu); +TauMatrix = (eye(ndof+6) - Jct*Lambda); + +% the dynamics is projected into the joint space +gTilde = gTilde(7:end); +CNuTilde = CNuTilde(7:end); +dJcNuTilde = dJcNuTilde(7:end); +TauMatrix = TauMatrix(7:end,7:end); + +%% Joint space controller +if sum(feet_on_ground) == 1 + + tau = TauMatrix\(CNuTilde + gTilde + dJcNuTilde + Mj*ddqjRef -impedances*qjTilde -dampings*dqjTilde); + +elseif sum(feet_on_ground) == 2 + + pinvTauMatrix = TauMatrix'/(TauMatrix*TauMatrix' + pinv_damp*eye(size(TauMatrix,1))); + %pinvTauMatrix = pinv(TauMatrix,pinv_tol); + tau = pinvTauMatrix*(CNuTilde + gTilde + dJcNuTilde + Mj*ddqjRef -impedances*qjTilde -dampings*dqjTilde); +end + +controlParam.tau = tau; + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/runController.m b/controllers/experiments/torqueBalancingJointControl/src/runController.m new file mode 100644 index 0000000..b41579f --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/runController.m @@ -0,0 +1,67 @@ +function controlParam = runController(gains,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE) +%RUNCONTROLLER is the initialization function for iCub balancing controllers +% in Matlab. +% +% controlParam = RUNCONTROLLER(gains,trajectory,dynamics, +% forKinematics,config,state) takes as input the control gains, +% the joint reference trajectory, all the configuration parameters +% and the robot dynamics, forward kinematics and state. The output +% is the structure CONTROLPARAM which contains the control torques +% TAU, the contact forces FC and other parameters for visualization. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Feet correction gains +KCorrPos = gains.CorrPosFeet; +KCorrVel = 2*sqrt(KCorrPos); + +%% Config parameters +ndof = CONFIG.ndof; +feet_on_ground = CONFIG.feet_on_ground; +initForKinematics = CONFIG.initForKinematics; +S = [zeros(6,ndof); + eye(ndof,ndof)]; + +%% Dynamics +dJcNu = DYNAMICS.dJcNu; +M = DYNAMICS.M; +Jc = DYNAMICS.Jc; +h = DYNAMICS.h; +JcMinv = Jc/M; +JcMinvS = JcMinv*S; + +%% Forward Kinematics +VelFeet = FORKINEMATICS.VelFeet; +TLfoot = FORKINEMATICS.TLfoot; +TRfoot = FORKINEMATICS.TRfoot; +RFootPoseEul = FORKINEMATICS.RFootPoseEul; +LFootPoseEul = FORKINEMATICS.LFootPoseEul; +DeltaPoseRFoot = TRfoot*(RFootPoseEul-initForKinematics.RFootPoseEul); +DeltaPoseLFoot = TLfoot*(LFootPoseEul-initForKinematics.LFootPoseEul); + +%% BALANCING CONTROLLERS +% Centroidal coordinates transformation +centroidalDynamics = centroidalConversion(DYNAMICS,FORKINEMATICS,STATE); +controlParam = jointSpaceController(CONFIG,gains,trajectory,centroidalDynamics,STATE); + +%% Feet pose correction +% this will avoid numerical errors during the forward dynamics integration +if feet_on_ground(1) == 1 && feet_on_ground(2) == 0 + + DeltaPoseFeet = DeltaPoseLFoot; + +elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1 + + DeltaPoseFeet = DeltaPoseRFoot; + +elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + DeltaPoseFeet = [DeltaPoseLFoot;DeltaPoseRFoot]; +end + +%% REAL CONTACT FORCES COMPUTATION +controlParam.fc = (JcMinv*transpose(Jc))\(JcMinv*h -JcMinvS*controlParam.tau -dJcNu -KCorrVel.*VelFeet-KCorrPos.*DeltaPoseFeet); + +end diff --git a/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m b/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m new file mode 100644 index 0000000..2a14bf0 --- /dev/null +++ b/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m @@ -0,0 +1,59 @@ +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 = GENERTRAJ(xCoMInit,t,config) takes as an input +% the initial CoM position, XCOMINIT, the current time T and the structure +% CONFIG which contains all the user-defined parameters. +% +% The output DESIRED_X_DX_DDX_COM is a matrix [3x3] which contains the +% 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 + +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 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]; + +end diff --git a/controllers/experiments/visualizerNew/visualizeForwardDynamics.m b/controllers/experiments/visualizerNew/visualizeForwardDynamics.m new file mode 100644 index 0000000..41b7e81 --- /dev/null +++ b/controllers/experiments/visualizerNew/visualizeForwardDynamics.m @@ -0,0 +1,545 @@ +function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) +%% visualize_forwardDyn +% Visualize the simulation results obtained from integration +% of the forward dynamics of the iCub. +% +% visualize_forwardDyn(XOUT,PARAMETERS) visualizes the motion +% of the robot. XOUT is the output vector of the integration carried +% out in the forward dynamics part, containing the position and the +% orientation of the base and the joint positions in the first 32 +% elements of its row vectors along a time span. PARAMETERS is the +% struct variable which contains constant parameters related to the +% simulation environment, robot, controller etc. +% + +text.color = [1 1 1]; +text.fontSize = 22; + +[xout,tSpan,comDes,jetsIntensities] = resizeData(xout,tSpan,squeeze(references(:,1,:))',jetsIntensities,config); + + +config.plotThusts = false; +if nargin == 5 + config.plotThusts = true; + % the list of link names that are used for identifying the jets positions + config.jets.frames = cell(4,1); + config.jets.frames{1} = 'l_hand_dh_frame'; + config.jets.frames{2} = 'r_hand_dh_frame'; + config.jets.frames{3} = 'l_foot_dh_frame'; + config.jets.frames{4} = 'r_foot_dh_frame'; +end + +robotColor = [1 0.4 1]; +thrustColor = [1 0 0]; + +n = size(xout,1); % number of instances of the simulation results +q = xout(:,1:7); % first 3 elements provide the position and next 4 elements provide the orientation of the base + +q_noBpos = q; +q_noBpos(:,1:3) = repmat([0 0 0.7],size(q(:,1:3),1),1); + +qj = xout(:,8:32); % joint positions + +% time span vector of the simulation +%tSpan = linspace( config.sim_start_time, config.sim_start_time+config.sim_duration, config.sim_duration/config.sim_step); + +[~,qjInit,~,~] = stateDemux(config.state0,config); + +[R,p] = wbm_getWorldFrameFromFixedLink('l_sole',qjInit); +wbm_setWorldFrame(R,p,[ 0,0,-9.81]'); +wbm_updateState(qjInit,zeros(config.ndof,1),zeros(6,1)); + +% the list of link/joint names that are used to construct the robot in the visualizer +L = cell(15,1); +L{1} = 'root_link' ; +L{2} = 'r_hip_1' ; +L{3} = 'r_lower_leg' ; +L{4} = 'r_sole' ; +L{5} = 'l_hip_1' ; +L{6} = 'l_lower_leg' ; +L{7} = 'l_sole' ; +L{8} = 'neck_1' ; +L{9} = 'r_shoulder_1'; +L{10} = 'r_elbow_1' ; +L{11} = 'r_gripper' ; +L{12} = 'l_shoulder_1'; +L{13} = 'l_elbow_1' ; +L{14} = 'l_gripper' ; +L{15} = 'com' ; + + +% RELATED TO WORLD REFERENCE FRAME ISSUE +% since for now the world reference frame is that of the codyco_balancing_world, +% the z-axis is the vertical axis for the robot. +xaxis = 'xdata'; +yaxis = 'ydata'; +zaxis = 'zdata'; + +n_plot = 15; % number of points to be plotted (virtual joints) +n_lin = 13; % number of lines to be plotted (virtual links) + +kin = zeros(size(xout,1),7,n_plot); +kin_noBpos = zeros(size(xout,1),7,n_plot); + +if config.visualiser.computeKinematics + for jj=1:n_plot + for ii=1:n % at each instance + [pB,R] = frame2posrot(squeeze(q(ii,:)')); + [pB_noBpos,R_noBpos] = frame2posrot(squeeze(q_noBpos(ii,:)')); + kin(ii,:,jj) = (wbm_forwardKinematics(R,pB,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links + kin_noBpos(ii,:,jj) = (wbm_forwardKinematics(R_noBpos,pB_noBpos,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links + end + end +else + load(config.fileName,'kin','kin_noBpos'); +end +if config.visualiser.saveKinematics + save([config.fileName '_kinematics'],'kin','kin_noBpos'); +end + +kin(:,:,1)= q; %use base data instead of fwdkin rootlink + +% clear and reset the plots +for ii=2:-1:1 + axes(config.plot_main(ii)); + grid on + axis([-0.5 0.5 -0.42 0.58 0 1]); + set(gca,'XGrid','off','YGrid','off','ZGrid','off','XTick', [],'YTick', [],'ZTick', []); + set(gca,'LineWidth',5); + set(gca, 'drawmode', 'fast'); + cla; + drawnow +end + +axes(config.plot_main(1)); + +%% INITIAL PLOTS + +% allocate memory +x = zeros(1,n_plot); +y = zeros(1,n_plot); +z = zeros(1,n_plot); +pos = zeros(1,n_plot); +xyzpairs = zeros(n_lin,6); + +% plot the position of the center of mass +jj = n_plot; +comPosition = kin(:,1:3,jj); + +% Change axis of the inertial frame visualization differently +% from that of the body frame +axes(config.plot_main(1)); +minimumAxisVariation = [1 1 1]; +maxComPos = max(comPosition)+minimumAxisVariation/2; +minComPos = min(comPosition)-minimumAxisVariation/2; + +axesInertial = zeros(3,1); +for indexSetAxis = 1:3 + if abs(maxComPos(indexSetAxis) - minComPos(indexSetAxis)) < minimumAxisVariation(indexSetAxis) + axesInertial(2*(indexSetAxis -1)+1) = minComPos(indexSetAxis) - minimumAxisVariation(indexSetAxis)/2; + axesInertial(2*(indexSetAxis -1)+2) = minComPos(indexSetAxis) + minimumAxisVariation(indexSetAxis)/2; + else + axesInertial(2*(indexSetAxis -1)+1) = minComPos(indexSetAxis); + axesInertial(2*(indexSetAxis -1)+2) = maxComPos(indexSetAxis); + end +end + +axis(axesInertial); +%% +% plot the base position +x(1)=kin(1,1,1); +y(1)=kin(1,2,1); +z(1)=kin(1,3,1); +pos(1)=plot3(x(1),y(1),z(1),'w*'); +% plot the joints and CoM +for jj=2:n_plot + [Ptemp,~] = frame2posrot(kin(1,:,jj)'); + x(jj) = Ptemp(1); + y(jj) = Ptemp(2); + z(jj) = Ptemp(3); + col = 'w*'; + if jj == n_plot + col = 'r*'; + end + pos(jj)=plot3(x(jj),y(jj),z(jj),col); +end + +%% RELATED TO WORLD REFERENCE FRAME ISSUE (see line 44) + +% define the pairs between the joints that will form the links +xyzpairs( 1,:) = [ x(1) x(8) y(1) y(8) z(1) z(8)]; +xyzpairs( 2,:) = [ x(1) x(2) y(1) y(2) z(1) z(2)]; +xyzpairs( 3,:) = [ x(3) x(2) y(3) y(2) z(3) z(2)]; +xyzpairs( 4,:) = [ x(3) x(4) y(3) y(4) z(3) z(4)]; +xyzpairs( 5,:) = [ x(1) x(5) y(1) y(5) z(1) z(5)]; +xyzpairs( 6,:) = [ x(6) x(5) y(6) y(5) z(6) z(5)]; +xyzpairs( 7,:) = [ x(6) x(7) y(6) y(7) z(6) z(7)]; +xyzpairs( 8,:) = [ x(8) x(9) y(8) y(9) z(8) z(9)]; +xyzpairs( 9,:) = [x(10) x(9) y(10) y(9) z(10) z(9)]; +xyzpairs(10,:) = [x(10) x(11) y(10) y(11) z(10) z(11)]; +xyzpairs(11,:) = [ x(8) x(12) y(8) y(12) z(8) z(12)]; +xyzpairs(12,:) = [x(13) x(12) y(13) y(12) z(13) z(12)]; +xyzpairs(13,:) = [x(13) x(14) y(13) y(14) z(13) z(14)]; + +% allocate memory +lin = zeros(1,n_lin); +lnkpatch = zeros(1,n_lin); +xyzpatch.vertices = zeros(8,3); +xyzpatch.faces = zeros(6,4); + +% constant multipliers related to the sizes of the patches around the links to form the robot figure +mult_patch = [0.07 , 0.03; + 0.04 , 0.02; + 0.03 , 0.02; + 0.025, 0.02; + 0.04 , 0.02; + 0.03 , 0.02; + 0.025, 0.02; + 0.03 , 0.02; + 0.025, 0.02; + 0.02 , 0.02; + 0.03 , 0.02; + 0.025, 0.02; + 0.02 , 0.02]; + +%% PLOT LINES DEPICTING LINKS +for jj=1:n_lin + + lin(jj) = line(xaxis,xyzpairs(jj,1:2),yaxis,xyzpairs(jj,3:4),zaxis,xyzpairs(jj,5:6),'erasemode','normal','linewidth',3,'color','red'); + + % for the patches (to determine the orientation of the patch to be applied to the links) + vectlnk = [xyzpairs(jj,2)-xyzpairs(jj,1),xyzpairs(jj,4)-xyzpairs(jj,3),xyzpairs(jj,6)-xyzpairs(jj,5)]; + orthlnk = null(vectlnk); + orthlnk1 = mult_patch(jj,1)*orthlnk(:,1); + orthlnk2 = mult_patch(jj,2)*orthlnk(:,2); + + % offsets in the direction orthogonal to the link + qq1 = orthlnk1+orthlnk2; + qq2 = -orthlnk1+orthlnk2; + qq3 = -orthlnk1-orthlnk2; + qq4 = orthlnk1-orthlnk2; + + % vertices for the patch + xyzpatch.vertices = [xyzpairs(jj,2)+qq1(1) , xyzpairs(jj,4)+qq1(2) , xyzpairs(jj,6)+qq1(3); + xyzpairs(jj,2)+qq2(1) , xyzpairs(jj,4)+qq2(2) , xyzpairs(jj,6)+qq2(3); + xyzpairs(jj,2)+qq3(1) , xyzpairs(jj,4)+qq3(2) , xyzpairs(jj,6)+qq3(3); + xyzpairs(jj,2)+qq4(1) , xyzpairs(jj,4)+qq4(2) , xyzpairs(jj,6)+qq4(3); + xyzpairs(jj,1)+qq1(1) , xyzpairs(jj,3)+qq1(2) , xyzpairs(jj,5)+qq1(3); + xyzpairs(jj,1)+qq2(1) , xyzpairs(jj,3)+qq2(2) , xyzpairs(jj,5)+qq2(3); + xyzpairs(jj,1)+qq3(1) , xyzpairs(jj,3)+qq3(2) , xyzpairs(jj,5)+qq3(3); + xyzpairs(jj,1)+qq4(1) , xyzpairs(jj,3)+qq4(2) , xyzpairs(jj,5)+qq4(3)]; + + % RELATED TO WORLD REFERENCE FRAME ISSUE (see line 44) + %temp = xyzpatch.vertices(:,1); + %xyzpatch.vertices(:,1) = xyzpatch.vertices(:,3); + %xyzpatch.vertices(:,3) = temp; + % + xyzpatch.faces = [ 1 2 3 4; + 1 4 8 5; + 5 8 7 6; + 7 3 2 6; + 2 6 5 1; + 3 7 8 4]; + + lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,... + 'FaceAlpha',0.2,'FaceColor',robotColor); +end + +%% FEET PATCHES +% right foot patch +jj=n_lin+1; + +orthlnk1 = [0 0.03 0]'; +orthlnk2 = [0 0 0.03]'; + +qq1 = orthlnk1+2*orthlnk2; +qq2 = -orthlnk1+2*orthlnk2; +qq3 = -orthlnk1-orthlnk2; +qq4 = orthlnk1-orthlnk2; + +xyzpatch.vertices = [xyzpairs(4,2)+qq1(1) , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); + xyzpairs(4,2)+qq2(1) , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); + xyzpairs(4,2)+qq3(1) , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); + xyzpairs(4,2)+qq4(1) , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3); + xyzpairs(4,2)+qq1(1)+0.03 , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); + xyzpairs(4,2)+qq2(1)+0.03 , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); + xyzpairs(4,2)+qq3(1)+0.03 , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); + xyzpairs(4,2)+qq4(1)+0.03 , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3)]; + + +lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,'FaceAlpha',0.2,... + 'FaceColor',robotColor); + +% left foot patch +jj=n_lin+2; + +qq1 = orthlnk1+2*orthlnk2; +qq2 = -orthlnk1+2*orthlnk2; +qq3 = -orthlnk1-orthlnk2; +qq4 = orthlnk1-orthlnk2; + +xyzpatch.vertices = [xyzpairs(7,2)+qq1(1) , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); + xyzpairs(7,2)+qq2(1) , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); + xyzpairs(7,2)+qq3(1) , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); + xyzpairs(7,2)+qq4(1) , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3); + xyzpairs(7,2)+qq1(1)+0.03 , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); + xyzpairs(7,2)+qq2(1)+0.03 , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); + xyzpairs(7,2)+qq3(1)+0.03 , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); + xyzpairs(7,2)+qq4(1)+0.03 , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3)]; + +lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,'FaceAlpha',0.2,... + 'FaceColor',robotColor); +%% PLOT THRUSTS +F = 0.01; +delta = 5*pi/180; +nT = 2; +tCy = 0:0.01:tan(delta); +[xT, yT, zT] = cylinder(1.2*F.*(1+tCy)); +Coord_Thrust = roty(-pi/2)*fromMesh2sortedVector(xT,yT,zT); +xT = sortedVector2mesh(Coord_Thrust(1,:),nT); +yT = sortedVector2mesh(Coord_Thrust(2,:),nT); +zT = sortedVector2mesh(Coord_Thrust(3,:),nT); + + +if config.plotThusts + handlerThrust(1) = surf(xT,yT,zT); + colormap(thrustColor) + set(handlerThrust(1),'EdgeColor',thrustColor,'LineStyle','none'); + + handlerThrust(2) = surf(xT,yT,zT); + set(handlerThrust(2),'EdgeColor',thrustColor,'LineStyle','none'); + handlerThrust(3) = surf(xT,yT,zT); + set(handlerThrust(3),'EdgeColor',thrustColor,'LineStyle','none'); + handlerThrust(4) = surf(xT,yT,zT); + set(handlerThrust(4),'EdgeColor',thrustColor,'LineStyle','none'); + %% + % store axes objects' handles to a vector + config.plot_objs{1}=[lnkpatch';lin';pos';handlerThrust']; + +else + config.plot_objs{1}=[lnkpatch';lin';pos']; +end + +if config.plotComTrajectories + DeltaI = 50; + stepI = 1; + temp = 1:stepI:DeltaI; + + colorComTrajectory = copper(length(temp)); + colorComDesTrajectory = bone(length(temp)); + + for j = 1: length(temp) + handlerTrajectoryCom(j) = plot3([0 0],[0 0],[0 0],'-','Color',colorComTrajectory(j,:),'Linewidth',1); + handlerTrajectoryComDes(j)= plot3([0 0],[0 0],[0 0],'-','Color',colorComDesTrajectory(j,:),'Linewidth',1); + end + config.plot_objs{1} = [config.plot_objs{1};handlerTrajectoryCom';handlerTrajectoryComDes']; +end + +% copy all objects to other axes with different views +axes(config.plot_main(2)); +config.plot_objs{2} = copyobj(config.plot_objs{1},config.plot_main(2)); + +%% WRITE INFO ON IMAGES +% text.time = text(0.5,0.4,['$t = 0 [s]$'],'Color',text.color ,'Interpreter','LaTex','FontSize',text.fontSize); + +%% SETTINGS FOR MAKING VIDEO +if config.visualiser.makeVideo + aviobj = VideoWriter(config.visualiser.video.filename);%,'fps',1000/di); + aviobj.Quality = 100; + open(aviobj) +else + aviobj = 0; +end + +%% UPDATING THE PLOTS +for ii=2:n % the visualization instance + %%The following for accounts for the two figures + tic; % visualizer step timer start (to setting the visualizer speed) + + for num_gigure=2:-1:1 + % Retrieving the handlers for the ithe figure + lnkpatch = config.plot_objs{num_gigure}(1:n_plot); + + lin = config.plot_objs{num_gigure}(1+n_plot:n_plot+n_lin); + + pos = config.plot_objs{num_gigure}(1+n_plot+n_lin:n_plot+n_lin+n_plot); + + %% UPDATING THRUST FORCES + if config.plotThusts + handlerThrust = config.plot_objs{num_gigure}(1+n_plot+n_lin+n_plot:size(config.jets.frames,1)+n_plot+n_lin+n_plot); + for ithJet = 1:size(config.jets.frames,1); + ith_axis = abs(config.jets.axes(ithJet)); + ith_dir = sign(config.jets.axes(ithJet)); + if num_gigure == 1 + [w_p_b,w_R_b] = frame2posrot(squeeze(q(ii,:)')); + else + [w_p_b,w_R_b] = frame2posrot(squeeze(q_noBpos(ii,:)')); + end + + kinTmp = (wbm_forwardKinematics(w_R_b,w_p_b,qj(ii,:)',config.jets.frames{ithJet}))'; % forward kinematics for the list of joints/links + + [w_p_link,w_R_link] = frame2posrot(kinTmp); + + [xT, yT, zT] = cylinder(1.3*F+jetsIntensities(ii,ithJet)*tCy); + CoordT_i = roty(pi/2)*fromMesh2sortedVector(xT,yT,zT); + + CoordT_i(ith_axis,:) = CoordT_i(ith_axis,:)*jetsIntensities(ii,ithJet) ; + CoordT_iC = -ith_dir*w_R_link*CoordT_i + repmat(w_p_link', 1, size(CoordT_i,2)); + + xTM = sortedVector2mesh(CoordT_iC(1,:),nT); + yTM = sortedVector2mesh(CoordT_iC(2,:),nT); + zTM = sortedVector2mesh(CoordT_iC(3,:),nT); + + set(handlerThrust(ithJet), 'xdata', xTM, 'ydata', yTM, 'zdata', zTM); + end + end + + %% UPDATING COM TRAJECTORIES + if config.plotComTrajectories + if num_gigure == 1 + handlerTrajectoryCom = config.plot_objs{num_gigure}(1+size(config.jets.frames,1)+n_plot+n_lin+n_plot:... + size(config.jets.frames,1)+n_plot+n_lin+n_plot+length(temp)); + handlerTrajectoryComDes = config.plot_objs{num_gigure}(1+size(config.jets.frames,1)+n_plot+n_lin+n_plot+length(temp):... + size(config.jets.frames,1)+n_plot+n_lin+n_plot+2*length(temp)); + j2 = 1; + for j = max(stepI+1,ii-DeltaI+1):stepI:ii + set(handlerTrajectoryCom(j2),'xdata',comPosition(j-stepI:j,1),... + 'ydata',comPosition(j-stepI:j,2),... + 'zdata',comPosition(j-stepI:j,3),'Color',colorComTrajectory(j2,:)); + set(handlerTrajectoryComDes(j2),'xdata',comDes(j-stepI:j,1),... + 'ydata',comDes(j-stepI:j,2),... + 'zdata',comDes(j-stepI:j,3),'Color',colorComDesTrajectory(j2,:)); + j2 = j2 +1; + end + end + end + + %% UPDATING MAIN BODY + % Update joint position (* in the plots) and CoM + for jj=1:n_plot + if num_gigure == 1 + kin_tmp = kin(ii,:,jj); + else + kin_tmp = kin_noBpos(ii,:,jj); + end + [Ptemp,~] = frame2posrot(kin_tmp); + x(jj) = Ptemp(1); + y(jj) = Ptemp(2); + z(jj) = Ptemp(3); + set(pos(jj),xaxis,x(jj),yaxis,y(jj),zaxis,z(jj)); + end + + + xyzpairs(1,:) = [ x(1) x(8) y(1) y(8) z(1) z(8)]; + xyzpairs(2,:) = [ x(1) x(2) y(1) y(2) z(1) z(2)]; + xyzpairs(3,:) = [ x(3) x(2) y(3) y(2) z(3) z(2)]; + xyzpairs(4,:) = [ x(3) x(4) y(3) y(4) z(3) z(4)]; + xyzpairs(5,:) = [ x(1) x(5) y(1) y(5) z(1) z(5)]; + xyzpairs(6,:) = [ x(5) x(6) y(5) y(6) z(5) z(6)]; + xyzpairs(7,:) = [ x(6) x(7) y(6) y(7) z(6) z(7)]; + xyzpairs(8,:) = [ x(9) x(8) y(9) y(8) z(9) z(8)]; + xyzpairs(9,:) = [ x(9) x(10) y(9) y(10) z(9) z(10)]; + xyzpairs(10,:) = [x(10) x(11) y(10) y(11) z(10) z(11)]; + xyzpairs(11,:) = [ x(8) x(12) y(8) y(12) z(8) z(12)]; + xyzpairs(12,:) = [x(13) x(12) y(13) y(12) z(13) z(12)]; + xyzpairs(13,:) = [x(13) x(14) y(13) y(14) z(13) z(14)]; + + % update the lines for the links wrt the new joint positions + for jj=1:n_lin + + set(lin(jj),xaxis,xyzpairs(jj,1:2),yaxis,xyzpairs(jj,3:4),zaxis,xyzpairs(jj,5:6)); + + vectlnk = [xyzpairs(jj,2)-xyzpairs(jj,1),xyzpairs(jj,4)-xyzpairs(jj,3),xyzpairs(jj,6)-xyzpairs(jj,5)]; + orthlnk = null(vectlnk); + orthlnk1 = mult_patch(jj,1)*orthlnk(:,1); + orthlnk2 = mult_patch(jj,2)*orthlnk(:,2); + qq1 = orthlnk1+orthlnk2; + qq2 = -orthlnk1+orthlnk2; + qq3 = -orthlnk1-orthlnk2; + qq4 = orthlnk1-orthlnk2; + + + xyzpatch.vertices = [xyzpairs(jj,2)+qq1(1) , xyzpairs(jj,4)+qq1(2) , xyzpairs(jj,6)+qq1(3); + xyzpairs(jj,2)+qq2(1) , xyzpairs(jj,4)+qq2(2) , xyzpairs(jj,6)+qq2(3); + xyzpairs(jj,2)+qq3(1) , xyzpairs(jj,4)+qq3(2) , xyzpairs(jj,6)+qq3(3); + xyzpairs(jj,2)+qq4(1) , xyzpairs(jj,4)+qq4(2) , xyzpairs(jj,6)+qq4(3); + xyzpairs(jj,1)+qq1(1) , xyzpairs(jj,3)+qq1(2) , xyzpairs(jj,5)+qq1(3); + xyzpairs(jj,1)+qq2(1) , xyzpairs(jj,3)+qq2(2) , xyzpairs(jj,5)+qq2(3); + xyzpairs(jj,1)+qq3(1) , xyzpairs(jj,3)+qq3(2) , xyzpairs(jj,5)+qq3(3); + xyzpairs(jj,1)+qq4(1) , xyzpairs(jj,3)+qq4(2) , xyzpairs(jj,5)+qq4(3)]; + + set(lnkpatch(jj),'vertices',xyzpatch.vertices); + end + + %% UPDATE FEET + % right foot + jj=n_lin+1; + + vectlnk = [xyzpairs(4,2)-xyzpairs(4,1),xyzpairs(4,4)-xyzpairs(4,3),xyzpairs(4,6)-xyzpairs(4,5)]; + orthlnk = null(vectlnk); + + orthlnk1 = mult_patch(4,1)*orthlnk(:,1); + orthlnk2 = mult_patch(4,2)*orthlnk(:,2); + + qq1 = orthlnk1+2*orthlnk2; + qq2 = -orthlnk1+2*orthlnk2; + qq3 = -orthlnk1-orthlnk2; + qq4 = orthlnk1-orthlnk2; + + xyzpatch.vertices = [xyzpairs(4,2)+qq1(1) , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); + xyzpairs(4,2)+qq2(1) , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); + xyzpairs(4,2)+qq3(1) , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); + xyzpairs(4,2)+qq4(1) , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3); + xyzpairs(4,2)+qq1(1)-0.04 , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); + xyzpairs(4,2)+qq2(1)-0.04 , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); + xyzpairs(4,2)+qq3(1)-0.04 , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); + xyzpairs(4,2)+qq4(1)-0.04 , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3)]; + + set(lnkpatch(jj),'vertices',xyzpatch.vertices); + + % left foot + jj=n_lin+2; + + vectlnk = [xyzpairs(7,2)-xyzpairs(7,1),xyzpairs(7,4)-xyzpairs(7,3),xyzpairs(7,6)-xyzpairs(7,5)]; + orthlnk = null(vectlnk); + + orthlnk1 = mult_patch(7,1)*orthlnk(:,1); + orthlnk2 = mult_patch(7,2)*orthlnk(:,2); + qq1 = orthlnk1+2*orthlnk2; + qq2 = -orthlnk1+2*orthlnk2; + qq3 = -orthlnk1-orthlnk2; + qq4 = orthlnk1-orthlnk2; + + xyzpatch.vertices = [xyzpairs(7,2)+qq1(1) , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); + xyzpairs(7,2)+qq2(1) , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); + xyzpairs(7,2)+qq3(1) , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); + xyzpairs(7,2)+qq4(1) , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3); + xyzpairs(7,2)+qq1(1)-0.04 , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); + xyzpairs(7,2)+qq2(1)-0.04 , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); + xyzpairs(7,2)+qq3(1)-0.04 , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); + xyzpairs(7,2)+qq4(1)-0.04 , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3)]; + + + set(lnkpatch(jj),'vertices',xyzpatch.vertices); + + %% UPDATE TEXT INFO ON IMAGES + + drawnow; + end + % to update the visualizer speed to keep it close to real simulation time + if toc() < config.visualiser.timeStep && toc() > 0.01 + pause(config.visualiser.timeStep-toc()); + end + if config.visualiser.makeVideo == 1 && ii > 2 + saveFramesForVideo(config.visualiser.video.filename,config.figure_main,aviobj,1); + end +end +if config.visualiser.makeVideo == 1 + saveFramesForVideo(config.visualiser.video.filename,config.figure_main,aviobj,90); + close(aviobj); +end + +end + + + diff --git a/controllers/experiments/visualizerNew/visualizer_demo.m b/controllers/experiments/visualizerNew/visualizer_demo.m new file mode 100644 index 0000000..519b2f4 --- /dev/null +++ b/controllers/experiments/visualizerNew/visualizer_demo.m @@ -0,0 +1,70 @@ +function [] = visualizer_demo(t,state,config,references,jetsIntensitiesByWeight) + +useSavedData = true; +if useSavedData + close all; + clear all; + clc; + % load('helicoidalShort.mat'); + % load('helicoidal.mat'); + fileName = 'helicoidal.mat'; + load(fileName); + config.fileName = fileName; + t = time.Data; + state = state.Data; + references = desired_x_dx_ddx_dddx_CoM; +end +config.visualiser.makeVideo = true; +if config.visualiser.makeVideo + config.visualiser.video.filename = 'helicoidal'; +end + +config.visualiser.computeKinematics = true; +config.visualiser.saveKinematics = false; + +config.visualiser.timeStep = 0.05; +% config.visualiser.wait = waitbar(0,'Wait:'); +config.plotComTrajectories = true; + +addpath([getenv('CODYCO_SUPERBUILD_ROOT') '/main/mexWholeBodyModel/mex-wholebodymodel/matlab/wrappers']) + +wbm_modelInitialise('icubGazeboSim'); + +BackGroundColor = [0 0 0]; +GridColor = [1 1 1]; +figure_main = figure('Name', 'iCub Simulator', 'NumberTitle', 'off',... + 'Position', [500,800,1200,650],'Color',BackGroundColor); + +%% ADAPT FIGURE DIMENSION DEPENDING ON SCREEN SIZE +sizeFig = get(0, 'MonitorPositions'); +sizeFig = 2*sizeFig/3; +sizeFig(1:2) = sizeFig(3:4)/10; +set(gcf, 'position', sizeFig); +config.figure_main = figure_main; + +config.plot_main = zeros(1,4); + +plot_pos = [0.51,0.05,0.45,1; + 0.01,0.05,0.45,1]; +for ii=1:2 + config.plot_main(ii) = subplot('Position', plot_pos(ii,:)); + config.plot_objs{ii} = plot3(0,0,0,'.'); + hold on; + set(gca,'Color',BackGroundColor,'Xcolor',GridColor,'Ycolor',GridColor,'Zcolor',GridColor); + set(gcf, 'MenuBar', 'None') +end + +axes(config.plot_main(1)); + +% root link trajectory +config.demux.baseOrientationType = 1; +robotConfiguration_t = zeros(length(t),7+size(config.ndofM,1)); +for i = 1:length(t) + [basePosei,jointAnglesi,~,~] = stateDemux(state(i,:),config); + robotConfiguration_t(i,:) = [basePosei(1:3,4)',basePosei(:,1)',jointAnglesi]; +end + +visualizeForwardDynamics(robotConfiguration_t,t,config,references.Data,jetsIntensitiesByWeight.Data); + + +end diff --git a/controllers/torqueBalancing/README.md b/controllers/torqueBalancing/README.md new file mode 100644 index 0000000..90bfc78 --- /dev/null +++ b/controllers/torqueBalancing/README.md @@ -0,0 +1,16 @@ +## torqueBalancing controller in MATLAB + +This is the controller currently implemented on the humanoid robot iCub for balancing. The control strategy is `momentum-based`. +The control objective is the achievement of two different tasks: the primary task is a desired centroidal momentum dynamics. The secondary task is the stability of the zero dynamics. + +### Run a simulation + +To run a balancing simulation, open the script `initializeTorqueBalancing.m`. The user can set all the simulation parameters as balancing on one foot or two feet, enable robot movements, and set the integration options. The control gains and the desired center of mass trajectory are defined in the functions `gains.m` and `trajectoryGenerator.m` in the [src](src) folder. + +### Visualize the result + +It is currently available a MATLAB iCub simulator to visualize the robot movements. Furthermore, the user can plot the forward dynamics integration results such that contact forces, control torques, the joint dynamics and CoP positions. + +### Utility Functions + +Most of the functions required to run the simulation are inside the `utilityMatlabFunction` folder. This folder is divided into subfolders according to the specific usage of the functions, e.g. all the functions related to robot dynamics, forward kinematics and state are inside the `utilityMatlabFunction/RobotFunctions` folder. diff --git a/controllers/torqueBalancing/initializeTorqueBalancing.m b/controllers/torqueBalancing/initializeTorqueBalancing.m new file mode 100644 index 0000000..f8b4406 --- /dev/null +++ b/controllers/torqueBalancing/initializeTorqueBalancing.m @@ -0,0 +1,111 @@ +%% 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 = 1; %either 0 or 1 +CONFIG.feet_on_ground = [1,1]; %either 0 or 1; [left,right] +CONFIG.use_QPsolver = 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 = 1; %either 0 or 1 +CONFIG.visualize_joints_dynamics = 1; %either 0 or 1 + +%% Integration time [s] +CONFIG.tStart = 0; +CONFIG.tEnd = 10; +CONFIG.sim_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; + +%% Forward dynamics integration setup +% CONFIG.integrateWithFixedStep will use a Euler forward integrator instead +% of ODE15s to integrate the forward dynamics. Use it only for debug +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 +if CONFIG.demo_movements == 0 + CONFIG.options = odeset('RelTol',1e-3,'AbsTol',1e-3); +else + CONFIG.options = odeset('RelTol',1e-6,'AbsTol',1e-6); +end + +%% 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/RobotFunctions'); +addpath('./../utilityMatlabFunctions/Visualization'); +initForwardDynamics(CONFIG); diff --git a/controllers/torqueBalancing/src/forwardDynamics.m b/controllers/torqueBalancing/src/forwardDynamics.m new file mode 100644 index 0000000..b761fed --- /dev/null +++ b/controllers/torqueBalancing/src/forwardDynamics.m @@ -0,0 +1,87 @@ +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.gainsInit; +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); + +%% CoM and joint trajectory generator +trajectory.JointReferences.ddqjRef = zeros(ndof,1); +trajectory.JointReferences.dqjRef = zeros(ndof,1); +trajectory.JointReferences.qjRef = qjInit; +trajectory.desired_x_dx_ddx_CoM = trajectoryGenerator(xCoMRef,t,CONFIG); + +%% 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.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 diff --git a/controllers/torqueBalancing/src/gains.m b/controllers/torqueBalancing/src/gains.m new file mode 100644 index 0000000..5f9e4c4 --- /dev/null +++ b/controllers/torqueBalancing/src/gains.m @@ -0,0 +1,73 @@ +function gainsInit = gains(CONFIG) +%GAINS generates the initial gains matrices 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 utility parameters, and the structure DYNAMICS +% which contains the robot dynamics. 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 the ground +if sum(CONFIG.feet_on_ground) == 2 + + gainsPCoM = diag([45 50 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 30 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 the ground +if sum(CONFIG.feet_on_ground) == 1 + + gainsPCoM = diag([40 45 40]); + gainsDCoM = 2*sqrt(gainsPCoM); + gainsPAngMom = diag([5 10 5]); + gainsDAngMom = 2*sqrt(gainsPAngMom); + + % impedances acting in the null space of the desired contact forces + impTorso = [ 12 10 12]; + impArms = [ 7 5 7 5 7]; + + if CONFIG.feet_on_ground(1) == 1 + + impLeftLeg = [ 15 15 12 15 5 5]; + impRightLeg = [ 10 10 12 15 5 5]; + else + impLeftLeg = [ 10 10 12 15 5 5]; + impRightLeg = [ 15 15 12 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 errors +gainsInit.CorrPosFeet = 5; + +end diff --git a/controllers/torqueBalancing/src/initForwardDynamics.m b/controllers/torqueBalancing/src/initForwardDynamics.m new file mode 100644 index 0000000..71e52ae --- /dev/null +++ b/controllers/torqueBalancing/src/initForwardDynamics.m @@ -0,0 +1,87 @@ +function [] = initForwardDynamics(CONFIG) +%INITFORWARDDYNAMICS setup the forward dynamics integration of robot iCub +% in MATLAB. +% +% [] = INITFORWARDDYNAMICS(CONFIG) takes as input the structure +% CONFIG containing all the configuration parameters. It has no +% output. The forward dynamics integration will be performed +% following the options the user specified in the initialization +% file. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +qjInit = CONFIG.qjInit; + +%% Contact constraints definition +if feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + 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); + +%% Initial conditions +dqjInit = zeros(ndof,1); +VelBaseInit = zeros(3,1); +omegaBaseWorldInit = zeros(3,1); + +% Update the initial position +wbm_updateState(qjInit,dqjInit,[VelBaseInit;omegaBaseWorldInit]); + +% Fixing the world reference frame w.r.t. the foot on ground position +if feet_on_ground(1) == 1 + + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('l_sole',qjInit); +else + [RotBaseInit,PosBaseInit] = wbm_getWorldFrameFromFixedLink('r_sole',qjInit); +end + +wbm_setWorldFrame(RotBaseInit,PosBaseInit,[0 0 -9.81]') + +% Initial base pose; initial robot state +[~,BasePoseInit,~,~] = wbm_getState(); +chiInit = [BasePoseInit; qjInit; VelBaseInit; omegaBaseWorldInit; dqjInit]; +CONFIG.initState = robotState(chiInit,CONFIG); + +%% Initial gains +CONFIG.gainsInit = gains(CONFIG); + +%% Initial dynamics and forward kinematics +% Initial dynamics +CONFIG.initDynamics = robotDynamics(CONFIG.initState,CONFIG); +% Initial forward kinematics +CONFIG.initForKinematics = robotForKinematics(CONFIG.initState,CONFIG.initDynamics); +CONFIG.xCoMRef = CONFIG.initForKinematics.xCoM; + +%% 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) + +%% VISUALIZATION +CONFIG.figureCont = initVisualizer(t,chi,CONFIG); + +end diff --git a/controllers/torqueBalancing/src/initVisualizer.m b/controllers/torqueBalancing/src/initVisualizer.m new file mode 100644 index 0000000..3da081d --- /dev/null +++ b/controllers/torqueBalancing/src/initVisualizer.m @@ -0,0 +1,107 @@ +function figureCont = initVisualizer(t,chi,CONFIG) +%INITVISUALIZER is the main function for visualize the results of iCub forward +% dynamics integration in MATLAB. +% +% INITVISUALIZER visualizes some outputs from the forward dynamics +% integration (e.g. the robot state, contact forces, control torques...). +% +% figureCont = INITVISUALIZER(t,chi,config) takes as input the integration +% time T, the robot state CHI and the structure CONFIG containing all the +% utility parameters. The output is a counter for the automatic correction +% of figures numbers in case a new figure is added. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +ndof = CONFIG.ndof; +initState = CONFIG.initState; + +%% ROBOT SIMULATOR +if CONFIG.visualize_robot_simulator == 1 + + CONFIG.figureCont = visualizeSimulation(t,chi,CONFIG); +end + +%% FORWARD DYNAMICS (basic parameters) +if CONFIG.visualize_integration_results == 1 || CONFIG.visualize_joints_dynamics == 1 + + CONFIG.wait = waitbar(0,'Generating the plots...'); + set(0,'DefaultFigureWindowStyle','Docked'); + + % joints + qj = zeros(ndof,length(t)); + qjInit = zeros(ndof,length(t)); + qjRef = zeros(ndof,length(t)); + + % contact forces and torques + fc = zeros(6*CONFIG.numConstraints,length(t)); + f0 = zeros(6*CONFIG.numConstraints,length(t)); + tau = zeros(ndof,length(t)); + normTau = zeros(length(t),1); + + % forward kinematics + xCoM = zeros(3,length(t)); + poseFeet = zeros(12,length(t)); + CoP = zeros(4,length(t)); + H = zeros(6,length(t)); + HRef = zeros(6,length(t)); + + % generate the vectors from forward dynamics + + for time = 1:length(t) + + [~,visual] = forwardDynamics(t(time), chi(time,:)', CONFIG); + + % joints + qj(:,time) = visual.qj; + qjInit(:,time) = initState.qj; + qjRef(:,time) = visual.JointRef.qjRef; + + %% Other parameters + % contact forces and torques + fc(:,time) = visual.fc; + f0(:,time) = visual.f0; + tau(:,time) = visual.tau; + normTau(time) = norm(visual.tau); + + % forward kinematics + xCoM(:,time) = visual.xCoM; + poseFeet(:,time) = visual.poseFeet; + H(:,time) = visual.H; + HRef(:,time) = visual.HRef; + + % centers of pressure at feet + CoP(1,time) = -visual.fc(5)/visual.fc(3); + CoP(2,time) = visual.fc(4)/visual.fc(3); + + if CONFIG.numConstraints == 2 + + CoP(3,time) = -visual.fc(11)/visual.fc(9); + CoP(4,time) = visual.fc(10)/visual.fc(9); + end + + end + + delete(CONFIG.wait) + % other parameters + HErr = H-HRef; + + %% Basic visualization (forward dynamics integration results) + if CONFIG.visualize_integration_results == 1 + + CONFIG.figureCont = visualizeForwardDyn(t,CONFIG,xCoM,poseFeet,fc,f0,normTau,CoP,HErr); + end + + %% Joints positions and position error + if CONFIG.visualize_joints_dynamics == 1 + + CONFIG.figureCont = visualizeJointDynamics(t,CONFIG,qj,qjRef); + end + + figureCont = CONFIG.figureCont; + set(0,'DefaultFigureWindowStyle','Normal'); + +end diff --git a/controllers/torqueBalancing/src/runController.m b/controllers/torqueBalancing/src/runController.m new file mode 100644 index 0000000..0d71527 --- /dev/null +++ b/controllers/torqueBalancing/src/runController.m @@ -0,0 +1,73 @@ +function controlParam = runController(gain,trajectory,DYNAMICS,FORKINEMATICS,CONFIG,STATE) +%RUNCONTROLLER is the initialization function for iCub balancing controllers +% in Matlab. +% +% controlParam = RUNCONTROLLER(gains,trajectory,dynamics, +% forKinematics,config,state) takes as input the control gains, +% the joint reference trajectory, all the configuration parameters +% and the robot dynamics, forward kinematics and state. The output +% is the structure CONTROLPARAM which contains the control torques +% TAU, the contact forces FC and other parameters for visualization. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Feet correction gains +KCorrPos = gain.CorrPosFeet; +KCorrVel = 2*sqrt(KCorrPos); + +%% Config parameters +ndof = CONFIG.ndof; +feet_on_ground = CONFIG.feet_on_ground; +use_QPsolver = CONFIG.use_QPsolver; +initForKinematics = CONFIG.initForKinematics; +S = [zeros(6,ndof); + eye(ndof,ndof)]; + +%% Dynamics +dJcNu = DYNAMICS.dJcNu; +M = DYNAMICS.M; +Jc = DYNAMICS.Jc; +h = DYNAMICS.h; +JcMinv = Jc/M; +JcMinvS = JcMinv*S; + +%% Forward Kinematics +VelFeet = FORKINEMATICS.VelFeet; +TLfoot = FORKINEMATICS.TLfoot; +TRfoot = FORKINEMATICS.TRfoot; +RFootPoseEul = FORKINEMATICS.RFootPoseEul; +LFootPoseEul = FORKINEMATICS.LFootPoseEul; +DeltaPoseRFoot = TRfoot*(RFootPoseEul-initForKinematics.RFootPoseEul); +DeltaPoseLFoot = TLfoot*(LFootPoseEul-initForKinematics.LFootPoseEul); + +%% BALANCING CONTROLLER +controlParam = stackOfTaskController(CONFIG,gain,trajectory,DYNAMICS,FORKINEMATICS,STATE); + +if use_QPsolver == 1 + % Quadratic programming solver for the nullspace of contact forces + controlParam.fcDes = QPSolver(controlParam,CONFIG,FORKINEMATICS); +end + +controlParam.tau = controlParam.tauModel + controlParam.Sigma*controlParam.fcDes; + +%% Feet pose correction +% this will avoid numerical errors during the forward dynamics integration +if feet_on_ground(1) == 1 && feet_on_ground(2) == 0 + + DeltaPoseFeet = DeltaPoseLFoot; + +elseif feet_on_ground(1) == 0 && feet_on_ground(2) == 1 + + DeltaPoseFeet = DeltaPoseRFoot; + +elseif feet_on_ground(1) == 1 && feet_on_ground(2) == 1 + + DeltaPoseFeet = [DeltaPoseLFoot;DeltaPoseRFoot]; +end + +%% REAL CONTACT FORCES COMPUTATION +controlParam.fc = (JcMinv*transpose(Jc))\(JcMinv*h -JcMinvS*controlParam.tau -dJcNu -KCorrVel.*VelFeet-KCorrPos.*DeltaPoseFeet); + +end diff --git a/controllers/torqueBalancing/src/stackOfTaskController.m b/controllers/torqueBalancing/src/stackOfTaskController.m new file mode 100644 index 0000000..866b1d7 --- /dev/null +++ b/controllers/torqueBalancing/src/stackOfTaskController.m @@ -0,0 +1,164 @@ +function controlParam = stackOfTaskController(CONFIG,gain,trajectory,DYNAMICS,FORKINEMATICS,STATE) +%STACKOFTASKCONTROLLER implements a momentum-based control algorithm to +% control the robot iCub. +% +% STACKOFTASKCONTROLLER computes the desired control torques at joints +% using a task-based approach. The first task is the control of robot's +% momentum, while the second task is a postural task. +% +% controlParam = STACKOFTASKCONTROLLER(config, gains, trajectory, +% dynamics, forKinematics, state) takes as input the structure CONFIG, +% which contains all the configuration parameters, while the other +% structures contain the control gains, the desired trajectory, the robot +% dynamics, forward kinematics and state. +% The output is the structure CONTROLPARAM which contains the desired +% control torques, the desired contact forces and others parameters used +% for visualization and QP solver. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Config parameters +pinv_tol = CONFIG.pinv_tol; +pinv_damp = CONFIG.pinv_damp; +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; + +%% Gains +impedances = gain.impedances; +dampings = gain.dampings; +MomentumGains = gain.MomentumGains; +intMomentumGains = gain.intMomentumGains; + +%% Dynamics +M = DYNAMICS.M; +g = DYNAMICS.g; +CNu = DYNAMICS.CNu; +dJcNu = DYNAMICS.dJcNu; +H = DYNAMICS.H; +Jc = DYNAMICS.Jc; +h = g + CNu; +m = M(1,1); +Mb = M(1:6,1:6); +Mbj = M(1:6,7:end); +Mj = M(7:end,7:end); +Mjb = M(7:end,1:6); +Mbar = Mj - Mjb/Mb*Mbj; +% The centroidal momentum jacobian is reduced to the joint velocity. This +% is then used to compute the approximation of the angular momentum integral +JH = DYNAMICS.JH; +JG = JH(:,7:end) -JH(:,1:6)*(eye(6)/Jc(1:6,1:6))*Jc(1:6,7:end); + +%% Forward kinematics +xCoM = FORKINEMATICS.xCoM; +dxCoM = FORKINEMATICS.dxCoM; +RFootPoseEul = FORKINEMATICS.RFootPoseEul; +LFootPoseEul = FORKINEMATICS.LFootPoseEul; + +%% Robot State +qj = STATE.qj; +dqj = STATE.dqj; + +% Trajectory +x_dx_ddx_CoMDes = trajectory.desired_x_dx_ddx_CoM; +ddqjRef = trajectory.JointReferences.ddqjRef; +dqjRef = trajectory.JointReferences.dqjRef; +qjRef = trajectory.JointReferences.qjRef; +qjTilde = qj-qjRef; +dqjTilde = dqj-dqjRef; + +% General parameters +gravAcc = 9.81; +S = [zeros(6,ndof); + eye(ndof,ndof)]; +f_grav = [zeros(2,1); + -m*gravAcc; + zeros(3,1)]; + +%% Multiplier of contact wrenches at CoM +posRFoot = RFootPoseEul(1:3); +posLFoot = LFootPoseEul(1:3); +distRF = posRFoot - xCoM; % Application point of the contact force on the right foot w.r.t. CoM +distLF = posLFoot - xCoM; % Application point of the contact force on the left foot w.r.t. CoM +AL = [eye(3), zeros(3); + skew(distLF),eye(3)]; +AR = [eye(3), zeros(3); + skew(distRF),eye(3)]; + +% One foot or two feet on ground selector +if sum(feet_on_ground) == 2 + + A = [AL,AR]; + pinvA = pinv(A,pinv_tol); +else + if feet_on_ground(1) == 1 + + A = AL; + elseif feet_on_ground(2) == 1 + + A = AR; + end + pinvA = eye(6)/A; +end + +%% Desired momentum derivative +% closing the loop on angular momentum integral +deltaPhi = JG(4:end,:)*(qj-qjRef); +accDesired = [m.*x_dx_ddx_CoMDes(:,3); zeros(3,1)]; +velDesired = -MomentumGains*[m.*(dxCoM-x_dx_ddx_CoMDes(:,2)); H(4:end)]; +posDesired = -intMomentumGains*[m.*(xCoM-x_dx_ddx_CoMDes(:,1)); deltaPhi]; +HDotDes = accDesired + velDesired + posDesired; + +%% Control torques equation +JcMinv = Jc/M; +Lambda = JcMinv*S; +JcMinvJct = JcMinv*transpose(Jc); +pinvLambda = pinv(Lambda,pinv_tol); + +% multiplier of contact wrenches in tau0 +JBar = transpose(Jc(:,7:end)) - Mbj'/Mb*transpose(Jc(:,1:6)); + +% nullspaces +Nullfc = eye(6*CONFIG.numConstraints)-pinvA*A; +NullLambda = eye(ndof)-pinvLambda*Lambda; + +Sigma = -(pinvLambda*JcMinvJct + NullLambda*JBar); +SigmaNA = Sigma*Nullfc; + +% Postural task correction +pinvLambdaDamp = Lambda'/(Lambda*Lambda' + pinv_damp*eye(size(Lambda,1))); +NullLambdaDamp = eye(ndof)-pinvLambdaDamp*Lambda; +posturalCorr = NullLambdaDamp*Mbar; +impedances = impedances*pinv(posturalCorr,pinv_tol) + 0.01*eye(ndof); +dampings = dampings*pinv(posturalCorr,pinv_tol) + 0.01*eye(ndof); + +tauModel = pinvLambda*(JcMinv*h - dJcNu) + NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); + +%% Desired contact forces computation +fcHDot = pinvA*(HDotDes - f_grav); + +% Desired f0 without Quadratic Programming +f0 = zeros(6,1); + +if sum(feet_on_ground) == 2 + + f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*fcHDot); +end + +fcDes = fcHDot + Nullfc*f0; + +%% Output parameters +controlParam.fcHDot = fcHDot; +controlParam.HDotDes = HDotDes; +controlParam.fcDes = fcDes; +controlParam.f_grav = f_grav; +controlParam.tauModel = tauModel; +controlParam.Sigma = Sigma; +controlParam.f0 = f0; +controlParam.Nullfc = Nullfc; +controlParam.A = A; + +end diff --git a/controllers/torqueBalancing/src/trajectoryGenerator.m b/controllers/torqueBalancing/src/trajectoryGenerator.m new file mode 100644 index 0000000..2a14bf0 --- /dev/null +++ b/controllers/torqueBalancing/src/trajectoryGenerator.m @@ -0,0 +1,59 @@ +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 = GENERTRAJ(xCoMInit,t,config) takes as an input +% the initial CoM position, XCOMINIT, the current time T and the structure +% CONFIG which contains all the user-defined parameters. +% +% The output DESIRED_X_DX_DDX_COM is a matrix [3x3] which contains the +% 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 + +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 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]; + +end diff --git a/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalConversion.m b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalConversion.m new file mode 100644 index 0000000..cf44bed --- /dev/null +++ b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalConversion.m @@ -0,0 +1,44 @@ +function centroidalDyn = centroidalConversion(DYNAMICS,FORKINEMATICS,STATE) +%CENTROIDALCONVERSION applies the centroidal coordinates tranformation to the +% system, i.e. the base dynamics now coincides with the +% CoM dynamics. +% +% centroidal = CENTROIDALCONVERSION(dynamics,forKinematics,state) +% takes as an input the structures containing the robot dynamics, +% forward kinematics and state. +% The output is the structure CENTROIDALDYN which contains the robot +% dynamics converted into the centroidal coordinates. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Dynamics +M = DYNAMICS.M; +h = DYNAMICS.h; +g = DYNAMICS.g; +Jc = DYNAMICS.Jc; +dJcNu = DYNAMICS.dJcNu; + +%% Forward kinematics +xCoM = FORKINEMATICS.xCoM; +dxCoM = FORKINEMATICS.dxCoM; + +%% Robot state +Nu = STATE.Nu; +VelBase = STATE.VelBase; +PosBase = STATE.PosBase; + +%% CENTROIDAL TRANSFORMATION +[T,dT] = centroidalTransformationT_TDot(xCoM,PosBase,dxCoM,VelBase,M); +[M_c, CNu_c, g_c, Jc_c, dJcNu_c, Nu_c] = fromFloatingToCentroidalDynamics(M, h, g, Jc, dJcNu, Nu, T, dT); + +centroidalDyn.M = M_c; +centroidalDyn.g = g_c; +centroidalDyn.CNu = CNu_c; +centroidalDyn.dJcNu = dJcNu_c; +centroidalDyn.Nu = Nu_c; +centroidalDyn.Jc = Jc_c; + +end + diff --git a/mex-wholebodymodel/matlab/utilities/centroidalTransformationT_TDot.m b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m similarity index 76% rename from mex-wholebodymodel/matlab/utilities/centroidalTransformationT_TDot.m rename to controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m index 396ec0b..af368f8 100644 --- a/mex-wholebodymodel/matlab/utilities/centroidalTransformationT_TDot.m +++ b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m @@ -1,13 +1,13 @@ function [T, dT] = centroidalTransformationT_TDot(xCoM,x_b,dxCoM,dx_b,M) % centroidalTransformationT_TDot -% converts the normal floating base frame of reference to +% converts the normal floating base frame of reference to % the centroidal frame of reference %% T calculation r = xCoM - x_b; X = [eye(3),skew(r)'; - zeros(3),eye(3)]; - + zeros(3),eye(3)]; + Mbj = M(1:6,7:end); Mb = M(1:6,1:6); Js = X*(Mb\Mbj); @@ -15,8 +15,8 @@ ndof = size(Mbj,2); T = [X,Js; - zeros(ndof,6),eye(ndof)]; - + zeros(ndof,6),eye(ndof)]; + %% time derivative of T dr = dxCoM - dx_b; mdr = M(1,1)*dr; @@ -30,8 +30,8 @@ inv_dMb = -Mb\dMb/Mb; dJs = dX*(Mb\Mbj) + X*inv_dMb*Mbj; - + dT = [dX,dJs; - zeros(ndof,6),zeros(ndof)]; - + zeros(ndof,6),zeros(ndof)]; + end diff --git a/mex-wholebodymodel/matlab/utilities/fromFloatingToCentroidalDynamics.m b/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m similarity index 90% rename from mex-wholebodymodel/matlab/utilities/fromFloatingToCentroidalDynamics.m rename to controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m index 1295957..bb3e5c2 100644 --- a/mex-wholebodymodel/matlab/utilities/fromFloatingToCentroidalDynamics.m +++ b/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m @@ -35,12 +35,12 @@ C_cNu_c_dT = invTt*CNu - M_c*dT*Nu; -C_cNu_c = [ zeros(3,1); - C_cNu_c_dT(4:6); +C_cNu_c = [ zeros(3,1); + C_cNu_c_dT(4:6); CNu_j-(Mbj')*(Mb\CNu_b)]; %new dT*Nu computation for Jacobian -dTNu = M_c\(CNu-C_cNu_c); +dTNu = M_c\(CNu-C_cNu_c); Jc_c = Jc*invT; dJcNu_c = dJcNu - Jc*invT*dTNu; diff --git a/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m b/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m new file mode 100644 index 0000000..0913397 --- /dev/null +++ b/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m @@ -0,0 +1,83 @@ +function [ikin,ChiInit,figureCont] = initInverseKinematics(CONFIG) +%INITINVERSEKINEMATICS generates the initial condition for inverse kinematics +% integration of robot iCub in MATLAB. +% INITINVERSEKINEMATICS evaluates the desired Momentum trajectory for the +% robot and generates proper joint references. The joints accelerations are +% calculated using a task-based structure: the first task is to guarantee +% the contact constraints, the second task to follow a desired Momentum +% trajectory while the third task is to keep the initial posture of the +% robot. Positions and velocities are obtained by means of a double fixed +% step integrator. +% +% [ikin, ChiInit, ContFig] = INITINVERSEKINEMATICS(config) takes +% as input the structure CONFIG containing all the utility parameters. +% The output are the initial conditions for the forward dynamics integration, +% CHIINIT [13+2ndof x 1], the structure IKIN which contains the joint +% references and the figures number counter, CONTFIG. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% Config parameters +pinv_tol = CONFIG.pinv_tol; +figureCont = CONFIG.figureCont; +ndof = CONFIG.ndof; +STATE = CONFIG.initState; + +% State parameters +BasePose = STATE.basePose; +qj = STATE.qj; + +% Dynamics parameters +DYNAMICS = robotDynamics(STATE,CONFIG); +m = DYNAMICS.M(1,1); +Jc = DYNAMICS.Jc; +JH = DYNAMICS.JH; +CONFIG.initDynamics = DYNAMICS; + +% Forward kinematics +FORKINEMATICS = robotForKinematics(STATE,DYNAMICS); +xCoM = FORKINEMATICS.xCoM; +CONFIG.initForKin = FORKINEMATICS; + +%% Generate the CoM reference fot the time t = 0 +desired_x_dx_ddx_CoM = trajectoryGenerator(xCoM,0,CONFIG); + +%% INITIAL CONDITIONS FOR INVERSE KINEMATICS INTEGRATION +% Define the first task for state velocities: not break the contact constraints +pinvJc = pinv(Jc,pinv_tol); +feetErrorDynamics = zeros(6*CONFIG.numConstraints,1); +Nullfeet = eye(ndof+6) - pinvJc*Jc; + +% Define the second task for state velocities: follow a desired momentum dynamics +pinvJH = pinv(JH*Nullfeet,pinv_tol); +CentroidalErrDynamics = [m*(desired_x_dx_ddx_CoM(:,3)+desired_x_dx_ddx_CoM(:,2)-(xCoM-desired_x_dx_ddx_CoM(:,1)));... + zeros(3,1)] - JH*pinvJc*feetErrorDynamics; +NullH = eye(ndof+6) - pinvJH*(JH*Nullfeet); + +% Define the third task for joints velocities: follow a desired joints trajectory +JPost = [zeros(ndof,6) eye(ndof)]; +JointErrDynamics = zeros(ndof,1)-JPost*(pinvJc*feetErrorDynamics+Nullfeet*pinvJH*CentroidalErrDynamics); +pinvJPost = pinv(JPost*Nullfeet*NullH,pinv_tol); + +%% TASK-BASED INITIAL STATE VELOCITY +NuThirdTask = pinvJPost*JointErrDynamics; +NuSecondTask = pinvJH*CentroidalErrDynamics + NullH*NuThirdTask; +NuFirstTask = pinvJc*feetErrorDynamics + Nullfeet*NuSecondTask; + +% initial condition for state integration +ChiInit = [BasePose; qj; NuFirstTask]; + +%% Inverse kinematics integrator +CONFIG.wait = waitbar(0,'Inverse kinematics integration...'); +ikin = integrateInverseKinematics(CONFIG,ChiInit); +delete(CONFIG.wait) + +%% Visualize the results of the inverse kinematics +if CONFIG.visualize_ikin_results == 1 + + figureCont = visualizeInverseKin(CONFIG,ikin); +end + diff --git a/controllers/utilityMatlabFunctions/InverseKinematics/integrateInverseKinematics.m b/controllers/utilityMatlabFunctions/InverseKinematics/integrateInverseKinematics.m new file mode 100644 index 0000000..58cce3a --- /dev/null +++ b/controllers/utilityMatlabFunctions/InverseKinematics/integrateInverseKinematics.m @@ -0,0 +1,81 @@ +function ikin = integrateInverseKinematics(CONFIG,ChiInit) +%INTEGRATEINVERSEKINEMATICS integrates the inverse kinematics of the robot iCub. +% It uses a fixed step integrator. +% +% ikinParam = INTEGRATEINVERSEKINEMATICS(config,ChiInit) takes as input the +% structure CONFIG which contains all the utility parameters, and the +% initial state of the robot CHIINIT. The output is the structure IKIN, +% which contains the joint reference trajectory and visualization parameters. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% setup initial parameters +tStep = CONFIG.ikin_integration_step; +t = transpose(CONFIG.tStart:tStep:CONFIG.tEnd); +ndof = CONFIG.ndof; + +%% Initial conditions for inverse dynamics integration +dimTime = length(t); +dimState = length(ChiInit); +qj = zeros(ndof,dimTime); +dqj = qj; +ddqj = dqj; +ChiIkin = zeros(dimState,dimTime); +CoMTrajectoryError = zeros(18,dimTime); +momentumError = zeros(6,dimTime); + +if CONFIG.numConstraints == 2 + + feetError = zeros(12,dimTime); + +elseif CONFIG.numConstraints == 1 + + feetError = zeros(6,dimTime); +end + +[dChiInit,visualizeIkinParam] = inverseKinematics(t(1),ChiInit,CONFIG); + +ChiIkin(:,1) = ChiInit; +qj(:,1) = ChiInit(8:7+ndof); +dqj(:,1) = ChiInit(14+ndof:end); +ddqj(:,1) = dChiInit(14+ndof:end); +CoMTrajectoryError(:,1) = visualizeIkinParam.CoMTrajectoryError; +feetError(:,1) = visualizeIkinParam.feetError; +momentumError(:,1) = visualizeIkinParam.momentumError; + +%% Function to be integrated +integratedFunction = @(t,Chi) inverseKinematics(t,Chi,CONFIG); + +%% Euler forward integrator (fixed step) +for kk = 2:dimTime + + % state at step k + ChiIkin(:,kk) = ChiIkin(:,kk-1) + tStep.*integratedFunction(t(kk-1),ChiIkin(:,kk-1)); + + % joint references and visualization parameters + [dChiIkin,visualizeIkinParam] = inverseKinematics(t(kk),ChiIkin(:,kk),CONFIG); + + qj(:,kk) = ChiIkin(8:7+ndof,kk); + dqj(:,kk) = ChiIkin(14+ndof:end,kk); + ddqj(:,kk) = dChiIkin(14+ndof:end); + + CoMTrajectoryError(:,kk) = visualizeIkinParam.CoMTrajectoryError; + feetError(:,kk) = visualizeIkinParam.feetError; + momentumError(:,kk) = visualizeIkinParam.momentumError; +end + +%% Joint trajectory and visualization parameters +ikin.ChiIkin = ChiIkin; +ikin.qj = qj; +ikin.dqj = dqj; +ikin.ddqj = ddqj; +ikin.t = t; +ikin.feetError = feetError; +ikin.CoMTrajectoryError = CoMTrajectoryError; +ikin.momentumError = momentumError; + +end + diff --git a/controllers/utilityMatlabFunctions/InverseKinematics/interpInverseKinematics.m b/controllers/utilityMatlabFunctions/InverseKinematics/interpInverseKinematics.m new file mode 100644 index 0000000..e1984bc --- /dev/null +++ b/controllers/utilityMatlabFunctions/InverseKinematics/interpInverseKinematics.m @@ -0,0 +1,53 @@ +function JointReferences = interpInverseKinematics(t,ikin) +%INTERPINVERSEKINEMATICS interpolates the joint reference trajectory obtained +% from inverse kinematics (which uses a fixed step integrator) +% to make the reference compatible with the variable step +% integrator used in the forward dynamics integrations. +% +% JointReferences = INTERPINVERSEKINEMATICS(t,ikin) performs a linear +% interpolation. The inputs are the forward dynamics integration time t +% and the structure ikin which contains all the parameters from +% inverse kinematics integration. The output are the Joint References. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +% initial parameters +time = ikin.t; + +% find in the vector "time" the step which is before the current time t +logic_t = time0 + + flag(1) = 1; +end + +logicNewEig = eigAStateDes0 + + flag(2) = 1; +end + +if flag(1) == 1 + + disp('Warning: the new linearized state dynamics after gains tuning is NOT asymptotically stable') + +elseif flag(2) == 1 + + disp('Warning: your desired linearized state dynamics is NOT asymptotically stable') + +elseif sum(flag) == 0 + + disp('The linearized state dynamics after gains tuning is asymptotically stable') +end + +%% New gains verification +% verify the symmetry +SymmKpx = (Kpx+Kpx')/2; +SymmKdx = (Kdx+Kdx')/2; +SymmKpn = (Kpn+Kpn')/2; +SymmKdn = (Kdn+Kdn')/2; + +tolSymm = 1e-7; +flag = [0;0;0;0]; + +logicSymm = abs(SymmKpx-Kpx)>tolSymm; + +if sum(sum(logicSymm))>0 + + flag(1) = 1; +end + +logicSymm = abs(SymmKdx-Kdx)>tolSymm; + +if sum(sum(logicSymm))>0 + + flag(2) = 1; +end + +logicSymm = abs(SymmKpn-Kpn)>tolSymm; + +if sum(sum(logicSymm))>0 + + flag(3) = 1; +end + +logicSymm = abs(SymmKdn-Kdn)>tolSymm; + +if sum(sum(logicSymm))>0 + + flag(4) = 1; +end + +if sum(flag) == 0 + + disp('All the gains matrices are symmetric') +end + +if flag(1) == 1 + + disp('Warning: the gains matrix on centroidal momentum pose is not symmetric') +end + +if flag(2) == 1 + + disp('Warning: the gains matrix on centroidal momentum velocity is not symmetric') +end + +if flag(3) == 1 + + disp('Warning: the gains matrix on joint position is not symmetric') +end + +if flag(4) == 1 + + disp('Warning: the gains matrix on joint velocity is not symmetric') +end + +% verify the positive definiteness +eigKpx = real(eig((Kpx+Kpx')/2)); +eigKdx = real(eig((Kdx+Kdx')/2)); +eigKpn = real(eig((Kpn+Kpn')/2)); +eigKdn = real(eig((Kdn+Kdn')/2)); + +toleig = 1e-5; +flag = [0;0;0;0]; + +logicEig = eigKpx0 + + flag(1) = 1; +end + +logicEig = eigKdx0 + + flag(2) = 1; +end + +logicEig = eigKpn0 + + flag(3) = 1; +end + +logicEig = eigKdn0 + + flag(4) = 1; +end + +if sum(flag) == 0 + + disp('All the gains matrices are positive definite') +end + +if flag(1) == 1 + + disp('Warning: the gains matrix on centroidal momentum pose is not positive definite') +end + +if flag(2) == 1 + + disp('Warning: the gains matrix on centroidal momentum velocity is not positive definite') +end + +if flag(3) == 1 + + disp('Warning: the gains matrix on joint position is not positive definite') +end + +if flag(4) == 1 + + disp('Warning: the gains matrix on joint velocity is not positive definite') +end + +disp('Tuned system eigenvalues; Desired eigenvalues') +disp([eig(AStateNew) eig(AStateDes)]) + +%% Gains matrices after optimization +gainsOpt.impedances = Kpn; +gainsOpt.dampings = Kdn; +gainsOpt.MomentumGains = Kdx; +gainsOpt.intMomentumGains = Kpx; +gainsOpt.CorrPosFeet = gainsInit.CorrPosFeet; +gainsOpt.KSdes = KSdes; +gainsOpt.KDdes = KDdes; +gainsOpt.KSn = KSn; +gainsOpt.KDn = KDn; + +end + diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/integrateGains.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/integrateGains.m new file mode 100644 index 0000000..1f147dd --- /dev/null +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/integrateGains.m @@ -0,0 +1,22 @@ +function vectorOfGains = integrateGains(initialConditions,gainsKronecker,CONFIG) +%% Initial conditions +tStep = 0.01; +CONFIG.tEndGain = 50; +t = transpose(0:tStep:CONFIG.tEndGain); +dimTime = length(t); +dimState = length(initialConditions); + +% variable to be integrated +vectorOfGains = zeros(dimState,dimTime); +vectorOfGains(:,1) = initialConditions; + +%% Function to be integrated +integratedFunction = @(t,gainsVect) gainsDynamics(t,gainsVect,gainsKronecker,CONFIG); + +%% Euler forward integrator (fixed step) +for kk = 2:dimTime + % state at step k + vectorOfGains(:,kk) = vectorOfGains(:,kk-1) + tStep.*integratedFunction(t(kk-1),vectorOfGains(:,kk-1)); +end + +end diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m new file mode 100644 index 0000000..9e8b242 --- /dev/null +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m @@ -0,0 +1,158 @@ +function linearization = jointSpaceLinearization(CONFIG,qjConfig) +%JOINTSPACELINEARIZATION linearizes the joint space dynamics of the iCub robot +% around an equilibrium point. +% JOINTSPACELINEARIZATION assumes that the robot is balancing on one foot +% or two feet, and the feedback controller is momentum-based. The solution is +% analytical, i.e. it is not necessary to compute numerical derivatives. +% +% linearization = JOINTSPACELINEARIZATION(config,qjConfig,mode) takes as an input the +% robot dynamics through the structure CONFIG, and the joint configuration +% around which the system is linearized, qjConfig. The variable MODE is a +% string, and can be either 'normal' mode or 'stability'. The only +% difference is that the 'stability' mode verifies the eigenvalues of the +% state matrix and returns a message about system's stability properties. +% It returns the structure LINEARIZATION which contains all the parameters +% related to the linearized system (i.e. the stiffness and damping +% matrix). +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Config parameters +gainsInit = CONFIG.gainsInit; +pinv_tol = CONFIG.pinv_tol; +feet_on_ground = CONFIG.feet_on_ground; +ndof = CONFIG.ndof; +pinv_damp = CONFIG.pinv_damp; + +%% Initialize the robot configuration +if feet_on_ground(1) == 1 + + [RotBase,PosBase] = wbm_getWorldFrameFromFixedLink('l_sole',qjConfig); +else + [RotBase,PosBase] = wbm_getWorldFrameFromFixedLink('r_sole',qjConfig); +end + +wbm_setWorldFrame(RotBase,PosBase,[0 0 -9.81]') + +[~,BasePose,~,~] = wbm_getState(); +chi = [BasePose; qjConfig; zeros(6,1); zeros(ndof,1)]; + +% robot state, dynamics and forward kinematics +STATE = robotState(chi,CONFIG); +DYNAMICS = robotDynamics(STATE,CONFIG); +FORKINEMATICS = robotForKinematics(STATE,DYNAMICS); + +%% Dynamics parameters +Jc = DYNAMICS.Jc; +JH = DYNAMICS.JH; +M = DYNAMICS.M; +Mb = M(1:6,1:6); +Mbj = M(1:6,7:end); +Mjb = M(7:end,1:6); +Mj = M(7:end,7:end); +Jb = Jc(:,1:6); +Jj = Jc(:,7:end); +Mbar = Mj - Mjb/Mb*Mbj; +invMbar = eye(ndof)/Mbar; +%invMbar = Mbar'/(Mbar*Mbar' + pinv_damp*eye(size(Mbar,1))); + +%% Forward kinematics parameters +posLFoot = FORKINEMATICS.LFootPoseQuat(1:3); +posRFoot = FORKINEMATICS.RFootPoseQuat(1:3); +xCoM = FORKINEMATICS.xCoM; + +if sum(feet_on_ground) == 1 + + if feet_on_ground(1) == 1 + posFoot = posLFoot; + else + posFoot = posRFoot; + end + + r = posFoot - xCoM; + A = [eye(3) zeros(3); + skew(r) eye(3) ]; + pinvA = eye(6)/A; + +else + Pr = posRFoot - xCoM; + Pl = posLFoot - xCoM; + + AL = [ eye(3), zeros(3); + skew(Pl), eye(3)]; + AR = [ eye(3), zeros(3); + skew(Pr), eye(3)]; + A = [AL, AR]; + pinvA = pinv(A,pinv_tol); +end + +%% Initial gains (before optimization) +impedances = gainsInit.impedances; +dampings = gainsInit.dampings; +MomentumGains = gainsInit.MomentumGains; +intMomentumGains = gainsInit.intMomentumGains; + +%% DEFINE THE LINEARIZED JOINT SPACE DYNAMICS +Lambda = (Jj - Jb/Mb*Mbj)*invMbar; +MultFirstTask = Jb/Mb*transpose(Jb)*pinvA; +% pinvLambda = pinv(Lambda,pinv_tol); +pinvLambda = Lambda'/(Lambda*Lambda' + pinv_damp*eye(size(Lambda,1))); +NullLambda = eye(ndof) - pinvLambda*Lambda; +JG = JH(:,7:end)-JH(:,1:6)*(eye(6)/Jb(1:6,1:6))*Jj(1:6,:); + +% Postural task correction +pinvLambdaDamp = Lambda'/(Lambda*Lambda' + 0.5*eye(size(Lambda,1))); +NullLambdaDamp = eye(ndof) - pinvLambdaDamp*Lambda; +posturalCorr = NullLambdaDamp*Mbar; + +%% Stiffness matrix +KS = invMbar*(-pinvLambda*MultFirstTask*intMomentumGains*JG + NullLambda*impedances*posturalCorr); + +%% Damping matrix +KD = invMbar*(-pinvLambda*MultFirstTask*MomentumGains*JG + NullLambda*dampings*posturalCorr); + +%% Verify the state matrix +AStateOld = [zeros(ndof) eye(ndof); + -KS -KD]; + +eigAStateOld = -real(eig(AStateOld)); + +toleig = 1e-5; +flag = 0; +logicEigOld = eigAStateOld0 + + flag = 1; +end + +if flag == 1 + + disp('Warning: the linearized state dynamics is NOT asymptotically stable') +else + disp('The linearized state dynamics is asymptotically stable') +end + +%% Two feet correction +if sum(feet_on_ground) == 2 + + JjBar = (eye(size(Jb,1))-Jb*pinv(Jb,pinv_tol))*Jj; + NullJjBar = eye(ndof)-pinv(JjBar,pinv_tol)*JjBar; + + gainsInit.KSdes= NullJjBar*gainsInit.KSdes; + gainsInit.KDdes= NullJjBar*gainsInit.KDdes; +end + +%% Parameters for visualization and gains tuning +linearization.KS = KS; +linearization.KD = KD; +linearization.KDdes = gainsInit.KDdes; +linearization.KSdes = gainsInit.KSdes; +linearization.ACartesian = -invMbar*pinvLambda*MultFirstTask; +linearization.BCartesian = JG; +linearization.ANull = invMbar*NullLambda; +linearization.BNull = posturalCorr; + +end \ No newline at end of file diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/kronVectorization.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/kronVectorization.m new file mode 100644 index 0000000..96a384d --- /dev/null +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/kronVectorization.m @@ -0,0 +1,121 @@ +function [Kx,Kn] = kronVectorization(Ax,Bx,An,Bn,Kdes,CONFIG) +%KRONVECTORIZATION is a gains optimization for the iCub linearized joint +% space dynamics through vectorization and Kronecher product. +% KRONVECTORIZATION solves the least square problem: x = pinv(M)*xdes +% where x is a vectorization of the feedback control gains, xdes is the +% vectorization of the optimization objective and M is a proper matrix. +% +% KRONVECTORIZATION also takes into account the following constraints on +% the gains matrices: Kx should be block diagonal; Kx(1:3,1:3) should be +% diagonal; Kx(4:6,4:6) should be symmetric. Kn should be symmetric. The +% positive definiteness constraint is only verified a-posteriori. in +% particular, the matrix Kn is enforced to be positive definite with the +% addition of a regulation term. +% +% [Kx,Kn] = KRONVECTORIZATION(Ax,Bx,An,Bn,Kdes,config) takes as inputs +% the pre and post multiplier of the gains matrices in the linearized +% system dynamics, i.e. AX,BX,AN,BN. KDES is the objective matrix. CONFIG +% is a structure containing all the utility parameters. +% +% The output are the optimized gains matrices, Kx [6x6] and Kn [ndof x +% ndof] +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +% setup parameters + +ndof = CONFIG.ndof; +%pinv_toll = CONFIG.pinv_tol; +pinv_damp = 0.5; + +kronMom = kron(Bx',Ax); +kronNull = kron(Bn',An); + +indexDiag = 1:36; +indexDiagMatr = reshape(indexDiag,[6,6]); +diagonalIndex = sort(diag(indexDiagMatr)); +kronMom = kronMom(:,diagonalIndex); + +MKron = [kronMom kronNull]; +xdes = Kdes(:); + +%% First task generator +indexTotal = 1:ndof^2; +indexMatrix = reshape(indexTotal,[ndof,ndof]); + +if sum(CONFIG.feet_on_ground) == 2 + + numberofJoints = [1 2 3 4 5 6 7 8 9 10 11 12 13]; + otherJoints = [14 15 16 17 18 19 20 21 22 23 24 25]; + + indexBlock1 = indexMatrix(numberofJoints,numberofJoints); + indexBlock2 = indexMatrix(numberofJoints,otherJoints); + indexBlock3 = indexMatrix(otherJoints,numberofJoints); + indexBlock4 = indexMatrix(otherJoints,otherJoints); + + indexFirstTask = sort([indexBlock1(:);indexBlock2(:);indexBlock3(:)]); + indexNull = sort(indexBlock4(:)); + +elseif CONFIG.feet_on_ground(1) == 1 && sum(CONFIG.feet_on_ground) == 1 + + numberofJoints = [1 2 3 4 5 6 7 8 9 10 11 12 13 20 21 22 23 24 25]; + otherJoints = [14 15 16 17 18 19]; + + indexBlock1 = indexMatrix(numberofJoints,numberofJoints); + indexBlock2 = indexMatrix(numberofJoints,otherJoints); + indexBlock3 = indexMatrix(otherJoints,numberofJoints); + indexBlock4 = indexMatrix(otherJoints,otherJoints); + + indexFirstTask = sort([indexBlock2(:);indexBlock1(:);indexBlock3(:)]); + indexNull = sort(indexBlock4(:)); + +elseif CONFIG.feet_on_ground(1) == 0 && sum(CONFIG.feet_on_ground) == 1 + + numberofJoints = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19]; + otherJoints = [20 21 22 23 24 25]; + + indexBlock1 = indexMatrix(numberofJoints,numberofJoints); + indexBlock2 = indexMatrix(numberofJoints,otherJoints); + indexBlock3 = indexMatrix(otherJoints,numberofJoints); + indexBlock4 = indexMatrix(otherJoints,otherJoints); + + indexFirstTask = sort([indexBlock2(:);indexBlock1(:);indexBlock3(:)]); + indexNull = sort(indexBlock4(:)); + +end + +xdesFirstTask = xdes(indexFirstTask); +MKronFirstTask = MKron(indexFirstTask,:); + +pinvMKronFirstTask = pinvDamped(MKronFirstTask,pinv_damp); +% pinvMKronFirstTask = pinv(MKronFirstTask,pinv_toll); +NullMKron = eye(size(MKronFirstTask,2))-pinvMKronFirstTask*MKronFirstTask; + +%% Null Space generator +% xdesNull = xdes(indexNull); +% MKronNull = MKron(indexNull,:); +% pinvMKronNull = pinvDamped(MKronNull,pinv_damp); +% % pinvMKronNull = pinv(MKronNull,pinv_toll); +% +% x0 = pinvMKronNull*xdesNull; + +if strcmp(CONFIG.matrixSelector,'position')==1 + x0 = [diag(CONFIG.gainsInit.intMomentumGains); CONFIG.gainsInit.impedances(:)]; +else + x0 = [diag(CONFIG.gainsInit.MomentumGains); CONFIG.gainsInit.dampings(:)]; +end + +%% Final vector +x = pinvMKronFirstTask*xdesFirstTask + NullMKron*x0; + +%% Gain matrices +vettKMom = x(1:6); +vettKn = x(7:end); +Kx = diag(vettKMom); +Kn = reshape(vettKn,[ndof,ndof]); + +end + + diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m new file mode 100644 index 0000000..1d7106c --- /dev/null +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m @@ -0,0 +1,193 @@ +function [dLMom,dRMom,dLNull,dRNull,XMomentum,XNull,V,Vdot] ... + = oneStepTuning(LMom,RMom,LNull,RNull,Xini,ROBOT_DOF_FOR_SIMULINK,KLMom,KOMom,KLNull,KONull,AMom,BMom,ANull,BNull) +%% config parameters +ndof = size(ROBOT_DOF_FOR_SIMULINK,1); +omegaNull = (ndof*(ndof-1))/2; +omegaMom = (6*(6-1))/2; +tol = 0.1; + +% multipliers of derivatives +normTrace = abs(trace(Xini'*Xini))+tol; +LambdaMatrMom = diag(LMom); +LambdaMatrNull = diag(LNull); +RMomt = transpose(RMom); +RNullt = transpose(RNull); + +%% postural gains time derivatives +% first term +A11 = -BNull'; +A22 = BNull'*RNullt*expm(LambdaMatrNull); +A33 = BNull'*RNullt*expm(LambdaMatrNull)*RNull; +A44 = -BNull'*RNullt*expm(LambdaMatrNull)*RNull*(ANull'*ANull); +A55 = BNull'*RNullt*expm(LambdaMatrNull)*RNull*(ANull'*ANull)*RNullt*expm(LambdaMatrNull); +A66 = BNull'*RNullt*expm(LambdaMatrNull)*RNull*(ANull'*ANull)*RNullt*expm(LambdaMatrNull)*RNull; + +B11 = RNullt*expm(LambdaMatrNull)*RNull*(ANull'*ANull)*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B22 = RNull*(ANull'*ANull)*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B33 = (ANull'*ANull)*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B44 = RNullt*expm(LambdaMatrNull)*RNull*BNull; +B55 = RNull*BNull; +B66 = BNull; + +% second term +A77 = 2*Xini*ANull; +A88 = -2*Xini*ANull*RNullt*expm(LambdaMatrNull); +A99 = -2*Xini*ANull*RNullt*expm(LambdaMatrNull); + +B77 = RNullt*expm(LambdaMatrNull)*RNull*BNull; +B88 = RNull*BNull; +B99 = BNull; + +%% momentum gains time derivatives +% first term +Aaa = -BMom'; +Abb = BMom'*RMomt*expm(LambdaMatrMom); +Acc = BMom'*RMomt*expm(LambdaMatrMom)*RMom; +Add = -BMom'*RMomt*expm(LambdaMatrMom)*RMom*(AMom'*AMom); +Aee = BMom'*RMomt*expm(LambdaMatrMom)*RMom*(AMom'*AMom)*RMomt*expm(LambdaMatrMom); +Aff = BMom'*RMomt*expm(LambdaMatrMom)*RMom*(AMom'*AMom)*RMomt*expm(LambdaMatrMom)*RMom; + +Baa = RMomt*expm(LambdaMatrMom)*RMom*(AMom'*AMom)*RMomt*expm(LambdaMatrMom)*RMom*BMom; +Bbb = RMom*(AMom'*AMom)*RMomt*expm(LambdaMatrMom)*RMom*BMom; +Bcc = (AMom'*AMom)*RMomt*expm(LambdaMatrMom)*RMom*BMom; +Bdd = RMomt*expm(LambdaMatrMom)*RMom*BMom; +Bee = RMom*BMom; +Bff = BMom; + +% second term +Agg = 2*Xini*AMom; +Ahh = -2*Xini*AMom*RMomt*expm(LambdaMatrMom); +Aii = -2*Xini*AMom*RMomt*expm(LambdaMatrMom); + +Bgg = RMomt*expm(LambdaMatrMom)*RMom*BMom; +Bhh = RMom*BMom; +Bii = BMom; + +%% mixed terms +% first term +A1a = -BMom'; +A2b = BMom'*RMomt*expm(LambdaMatrMom); +A3c = BMom'*RMomt*expm(LambdaMatrMom)*RMom; +A4d = -BMom'*RMomt*expm(LambdaMatrMom)*RMom*AMom'*ANull; +A5e = BMom'*RMomt*expm(LambdaMatrMom)*RMom*AMom'*ANull*RNullt*expm(LambdaMatrNull); +A6f = BMom'*RMomt*expm(LambdaMatrMom)*RMom*AMom'*ANull*RNullt*expm(LambdaMatrNull)*RNull; + +B1a = RMomt*expm(LambdaMatrMom)*RMom*AMom'*ANull*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B2b = RMom*AMom'*ANull*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B3c = AMom'*ANull*RNullt*expm(LambdaMatrNull)*RNull*BNull; +B4d = RNullt*expm(LambdaMatrNull)*RNull*BNull; +B5e = RNull*BNull; +B6f = BNull; + +%% Gain matrices +BTildeNull = (B11*A11+B33*A33+B44*A44+B66*A66+B77*A77+B99*A99+B4d*A4d+B6f*A6f)./normTrace; +ATildeNull = (B22*A22+B55*A55+B88*A88+B5e*A5e)./normTrace; + +BTildeMom = (Baa*Aaa+Bcc*Acc+Bdd*Add+Bff*Aff+Bgg*Agg+Bii*Aii+B1a*A1a+B3c*A3c)./normTrace; +ATildeMom = (Bbb*Abb+Bee*Aee+Bhh*Ahh+B2b*A2b)./normTrace; + +skewBMom = (BTildeMom-transpose(BTildeMom))/2; +skewBNull = (BTildeNull-transpose(BTildeNull))/2; + +%% Derivative of lambdaMom, lambdaNull +aMom = diag(ATildeMom); +aNull = diag(ATildeNull); + +dLMom = -KLMom.*aMom; +dLNull = -KLNull.*aNull; + +%% Omega for the skew-symm matrix on null space +g = 1; +uNull = zeros(omegaNull,1); + +for i = 1:ndof + + for j = 1:ndof + + if j l_max - tol; -res = sum(res); - -if res==0 - -else - - disp('Joint limits reached at time:') - disp(t) - error('joint limits reached '); - -end - -%% Feet correction to avoid numerical integration errors -Kcorr_pos = 2.5; -Kcorr_vel = 2*sqrt(Kcorr_pos); - -% feet position and orientation at time t -[x_rfoot,R_b_rfoot] = frame2posrot(rsole); -[x_lfoot,R_b_lfoot] = frame2posrot(lsole); - -[T_lfoot,phi_lfoot] = parametrization(R_b_lfoot); -[T_rfoot,phi_rfoot] = parametrization(R_b_rfoot); - -pos_leftFoot = [x_lfoot; phi_lfoot']; -pos_rightFoot = [x_rfoot; phi_rfoot']; - -% feet initial position and orientation -lfoot_ini = param.lfoot_ini; -rfoot_ini = param.rfoot_ini; - -[xInit_rfoot,R_bInit_rfoot] = frame2posrot(rfoot_ini); -[xInit_lfoot,R_bInit_lfoot] = frame2posrot(lfoot_ini); - -[~,phiInit_lfoot] = parametrization(R_bInit_lfoot); -[~,phiInit_rfoot] = parametrization(R_bInit_rfoot); - -lfoot_iniPosRot = [xInit_lfoot; phiInit_lfoot']; -rfoot_iniPosRot = [xInit_rfoot; phiInit_rfoot']; - -% feet error (corrected with angles time derivative) - if param.feet_on_ground(1) == 1 && param.feet_on_ground(2) == 0 - - T_tildeLeftfoot = [eye(3) zeros(3); - zeros(3) T_lfoot]; - - pos_feet_delta = T_tildeLeftfoot*(pos_leftFoot-lfoot_iniPosRot); - - elseif param.feet_on_ground(1) == 0 && param.feet_on_ground(2) == 1 - - T_tildeRightfoot = [eye(3) zeros(3); - zeros(3) T_rfoot]; - - pos_feet_delta = T_tildeRightfoot*(pos_rightFoot-rfoot_iniPosRot); - - elseif param.feet_on_ground(1) == 1 && param.feet_on_ground(2) == 1 - - T_tildeLeftfoot = [eye(3) zeros(3); - zeros(3) T_lfoot]; - - T_tildeRightfoot = [eye(3) zeros(3); - zeros(3) T_rfoot]; - - T_tilde_tot = [T_tildeLeftfoot zeros(6); - zeros(6) T_tildeRightfoot]; - - pos_feet_delta = T_tilde_tot*[(pos_leftFoot - lfoot_iniPosRot);... - (pos_rightFoot - rfoot_iniPosRot)]; - - end - -%% Control torques computation -% gains definition - gains = gainsAndConstraints_js(param); - -% ikin trajectory interpolation - [jointReferences, errorCoM] = interpolation(t,param,xCoM); - -% Joint space balancing controller - tau = JointSpaceController(param, Nu_c, M_c, g_c, CNu_c, Jc_c, dJcNu_c, gains, jointReferences); - -%% Real contact forces -Jct = transpose(Jc); -S = [zeros(6,ndof); eye(ndof)]; -Minv = eye(ndof+6)/M; -JcMinv = Jc*Minv; -JcMinvS = JcMinv*S; -invJcMinvJct = eye(6*param.numConstraints)/(JcMinv*Jct); - -fc = invJcMinvJct*(JcMinv*h -JcMinvS*tau -dJcNu -Kcorr_vel.*Jc*Nu -Kcorr_pos.*pos_feet_delta); - -%% State derivative -% need to calculate the quaternions time derivative -omega_b = transpose(R_b)*omega_w; -dqt_b = quaternionDerivative(omega_b, qt_b); - -dx = [dx_b; dqt_b; dqj]; -dNu = M\(Jc'*fc + [zeros(6,1); tau]-h); - -dchi = [dx;dNu]; - -%% Visualization parameters -% these are the variables that can be plotted by visualizer_js.m - visual_param.pos_feet = [pos_leftFoot; pos_rightFoot]; - visual_param.fc = fc; - visual_param.qj = qj; - visual_param.qjDes = jointReferences.qjDes; - visual_param.tau = tau; - visual_param.errorCoM = errorCoM; - -end - diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/gainsAndConstraints_js.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/gainsAndConstraints_js.m deleted file mode 100644 index 8bdc7cb..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/gainsAndConstraints_js.m +++ /dev/null @@ -1,68 +0,0 @@ -function gains = gainsAndConstraints_js (param) -%% gainsAndConstraints_js -% Generates the desired gains for both the joint and the CoM -% dynamics. -% The output is: -% -% gains this is a structure which contains both CoM and joints gains -% -%% Setup parameters -feet_on_ground = param.feet_on_ground; -ndof = param.ndof; - -%% Two feet impedances -if sum(feet_on_ground) == 2 - - impTorso = [ 50 50 50 - 0 0 0]; - - impArms = [ 10 10 10 10 20 - 0 0 0 0 0]; - - impLeftLeg = [ 35 50 5 30 5 10 - 0 0 0 0 0 0]; - - impRightLeg = [35 50 5 30 5 10 - 0 0 0 0 0 0]; -end - -%% One foot impedances -if sum(feet_on_ground) == 1 - - impTorso = [ 20 20 20 - 0 0 0]; - - impArms = [ 13 13 13 5 13 - 0 0 0 0 0 ]; - -if param.feet_on_ground(1) == 1 - - impLeftLeg = [ 70 70 65 30 10 10 - 0 0 0 0 0 0]; - - impRightLeg = [ 20 20 20 10 10 10 - 0 0 0 0 0 0]; - -else - - impRightLeg = [ 70 70 65 30 10 10 - 0 0 0 0 0 0]; - - impLeftLeg = [ 20 20 20 10 10 10 - 0 0 0 0 0 0]; - -end -end - -%% Dampings and impedances -gains.impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; - -gains.dampings = 0.5*ones(1,ndof); - -if (size(gains.impedances,2) ~= ndof) - - error('Dimension mismatch between ndof and dimension of the variable impedences. Check these variables in the file gains.m'); - -end - -end diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/generTraj_js.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/generTraj_js.m deleted file mode 100644 index b56ce58..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/generTraj_js.m +++ /dev/null @@ -1,33 +0,0 @@ -function desired_x_dx_ddx_CoM = generTraj_js(xCoM_ini, t, referenceParams, directionOfOscillation, noOscillationTime) -%% generTraj_js -% Generates a desired trajectory for robot's global CoM. It can be applied -% in every direction (X,Y,Z) -% Output -% -% desired_x_dx_ddx_CoM [3x3] it is a matrix wich contains the desired -% CoM position, velocity and acceleration -% -%% Time before the oscillation starts -if t >= noOscillationTime - - Amp = referenceParams(1); - -else - - Amp = 0; - -end - -%% Trajectory generator - freq = referenceParams(2); - - xCoMDes = xCoM_ini + Amp*sin(2*pi*freq*t)*directionOfOscillation; - - dxCoMDes = Amp*2*pi*freq*cos(2*pi*freq*t)*directionOfOscillation; - - ddxCoMDes = -Amp*(2*pi*freq)^2*sin(2*pi*freq*t)*directionOfOscillation; - - - desired_x_dx_ddx_CoM = [xCoMDes dxCoMDes ddxCoMDes]; - -end diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/integrateForwardDynamics_js.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/integrateForwardDynamics_js.m deleted file mode 100644 index 89210bd..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/integrateForwardDynamics_js.m +++ /dev/null @@ -1,227 +0,0 @@ -%% integrateForwardDynamics_js -% This is the main program for integrating the forward dynamics of the robot iCub in matlab. -% It integrates the robot state defined in forwardDynamics_js.m and the user can set -% how many feet are on the ground, decide if activate the robot's movements, -% plot forces, torques and joints variables, and activate a demo of the -% robot's movements. -clear all -close all -clc - -%% Setup path for inverse kinematics -addpath('./inverseKinematics'); - -%% Initialise the mexWholeBodyModel -wbm_modelInitialise('icubGazeboSim'); - -%% Setup robot parameters -% CoM follows a desired trajectory - params.demo_movements = 1; % either 0 or 1 - -% balancing on two feet or one foot - params.feet_on_ground = [1 1]; % either 0 or 1; [left;right] - -% visualize inverse kinematics graphics - params.visualizerIkin = 1; % either 0 or 1 - -% visualize state integration graphics - params.visualizerGraphics = 1; % either 0 or 1 - params.visualizerJoints = 0; % either 0 or 1 - -% visualize demo of robot's movements - params.visualizerDemo = 1; % either 0 or 1 - -%% Com trajectory reference -% if demo_movements = 1, these parameters define the desired CoM trajectory -% reference = [amplitude of oscillation (meters); frequency (Hz)] -if sum(params.feet_on_ground) == 2 - -params.noOscillationTime = 0; -params.direction = [0;1;0]; -params.reference = [0.035 0.35]; - -elseif sum(params.feet_on_ground) == 1 - -params.noOscillationTime = 0; -params.direction = [0;1;0]; -params.reference = [0.005 0.15]; - -end - -%% Setup integration initial conditions -% this is assuming a 25DoF iCub - params.ndof = 25; - -% initial conditions -params.leftArmInit = [ -20 30 0.0 45 0.0]'; -params.rightArmInit = [ -20 30 0.0 45 0.0]'; -params.torsoInit = [ -10.0 0.0 0.0]'; - -if params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 1 - -% initial conditions for balancing on two feet - params.leftLegInit = [ 25.5 0.1 0.0 -18.5 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 0.1 0.0 -18.5 -5.5 -0.1]'; - -elseif params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 0 - -% initial conditions for the robot standing on the left foot - params.leftLegInit = [ 25.5 15.0 0.0 -18.5 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 5.0 0.0 -40 -5.5 -0.1]'; - -elseif params.feet_on_ground(1) == 0 && params.feet_on_ground(2) == 1 - -% initial conditions for the robot standing on the right foot - params.leftLegInit = [ 25.5 5.0 0.0 -40 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 15.0 0.0 -18.5 -5.5 -0.1]'; - -end - - params.qjInit = [params.torsoInit;params.leftArmInit;params.rightArmInit;params.leftLegInit;params.rightLegInit]*(pi/180); - params.dqjInit = zeros(params.ndof,1); - -% initial floating base velocity - params.dx_bInit = zeros(3,1); - params.omega_bInit = zeros(3,1); - -%% Update the robot state with the initial conditions -wbm_updateState(params.qjInit,params.dqjInit,[params.dx_bInit;params.omega_bInit]); - -% fixing the world reference frame w.r.t. the foot on ground -if params.feet_on_ground(1) == 0 - -[rot,pos] = wbm_getWorldFrameFromFixedLink('r_sole',params.qjInit); - -else - -[rot,pos] = wbm_getWorldFrameFromFixedLink('l_sole',params.qjInit); - -end - -wbm_setWorldFrame(rot,pos,[0 0 -9.81]') -[~,T_b,~,~] = wbm_getState(); - -%% Contact constraints at feet -if sum(params.feet_on_ground) == 2 - - params.constraintLinkNames = {'l_sole','r_sole'}; - - elseif params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 0 - - params.constraintLinkNames = {'l_sole'}; - - elseif params.feet_on_ground(1) == 0 && params.feet_on_ground(2) == 1 - - params.constraintLinkNames = {'r_sole'}; - -end - - params.numConstraints = length(params.constraintLinkNames); - -%% Feet initial position and orientation and CoM initial position - params.lfoot_ini = wbm_forwardKinematics(rot, pos, params.qjInit,'l_sole'); - params.rfoot_ini = wbm_forwardKinematics(rot, pos, params.qjInit,'r_sole'); - params.CoM_ini = wbm_forwardKinematics(rot, pos, params.qjInit,'com'); - -%% Joints limits - [jl1,jl2] = wbm_jointLimits(); - limits = [jl1 jl2]; - params.limits = limits; - -%% Desired CoM trajectory at time t = 0 -directionOfOscillation = [0; 0; 0]; -referenceParams = [0 0]; - -if params.demo_movements == 1 - - directionOfOscillation = params.direction; - referenceParams = params.reference; - -end - -desired_x_dx_ddx_CoM = generTraj_js (params.CoM_ini(1:3), 0, referenceParams,... - directionOfOscillation, params.noOscillationTime); - -%% Velocity initial condition for inverse kinematics integration -% setup parameters -PINV_TOL = 1e-8; - -% Feet and CoM jacobians -for ii=1:params.numConstraints - - Jc_ikin(6*(ii-1)+1:6*ii,:) = wbm_jacobian(rot, pos, params.qjInit, params.constraintLinkNames{ii}); - -end - -JCoM_ikin = wbm_jacobian(rot, pos, params.qjInit,'com'); -JCoM_ikin_lin = JCoM_ikin(1:3,:); - -%% Desired state velocity -% higher priority: feet position and orientation -pinvJc_ikin = pinv(Jc_ikin,PINV_TOL); -feetPosAndOrient = zeros(6*params.numConstraints,1); -Nullfeet = eye(params.ndof+6) - pinvJc_ikin*Jc_ikin; - -% lower priority: CoM position -JCoM_low = JCoM_ikin_lin*Nullfeet; -CoMPos = desired_x_dx_ddx_CoM(:,2) - JCoM_ikin_lin*pinvJc_ikin*feetPosAndOrient; - -% desired state velocity -Nu_ikin0 = pinv(JCoM_low, PINV_TOL)*CoMPos; -Nu_ikin = pinvJc_ikin*feetPosAndOrient + Nullfeet*Nu_ikin0; - -% initial condition for inverse kinematics integrator -params.ikin_init = [T_b; params.qjInit; Nu_ikin]; - -% initial condition for state integration -params.chiInit = [T_b; params.qjInit; Nu_ikin]; - -%% Final and initial integration time -params.tStart = 0; -params.tEnd = 10; - -%% Inverse kinematics integrator -% integration step for inverse kinematics integrator -plot_set -params.euler_step = 0.01; - -% waitbar -params.wait = waitbar(0,'Inverse kinematics integration in progress...'); - -[params.t_kin, params.joints_traj] = ikin_integrator(params); - -delete(params.wait) - -% inverse kinematics visualization -ikin_graphics(params); - -%% Setup state integration -% ode tolerances - params.sim_step = 0.01; - - if params.demo_movements == 0 - - options = odeset('RelTol',1e-3,'AbsTol', 1e-4); - -else - - options = odeset('RelTol',1e-5,'AbsTol',1e-5); - - end - - params.wait = waitbar(0,'State integration in progress...'); - -% function that will be integrated - forwardDynFunc = @(t,chi)forwardDynamics_js(t,chi,params); - [t,chi] = ode15s(forwardDynFunc,params.tStart:params.sim_step:params.tEnd,params.chiInit,options); - - delete(params.wait) - -%% Visualizer -params.wait = waitbar(0,'Preparing the graphics...'); - -visualizer_js(t,chi,params); - -delete(params.wait) - - \ No newline at end of file diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_function.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_function.m deleted file mode 100644 index c626c0e..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_function.m +++ /dev/null @@ -1,43 +0,0 @@ -function [dikin, visual_kin_param] = ikin_function( t, ikin, param ) -%% ikin_function -% Calculates the inverse kinematics from a desired CoM -% trajectory to the corresponding joints pos, vel, acc, with respect of -% constraints at feet. -% The outputs are: -% -% dikin [ndof+6 x 1] which is the vector that will be integrated -% -% visual_kin_param which contains parameters for visualization - -waitbar(t/param.tEnd,param.wait) - -%% State definition -% ikin(1:7) is the quaternion associated with the desired trajectory and ikin(8:end) is the joints desired position -param.kin_total = ikin; - -%% Desired oscillation at CoM -directionOfOscillation = [0; 0; 0]; -referenceParams = [0 0]; - -if (param.demo_movements == 1) - - directionOfOscillation = param.direction; - referenceParams = param.reference; - -end - -%% CoM trajectory generator -desired_x_dx_ddx_CoM = generTraj_js(param.CoM_ini(1:3),t,referenceParams,directionOfOscillation,param.noOscillationTime); - -%% Desired acceleration and velocity at joints -[ddqjDes, dikin, traj_obt, Delta] = ikin_generator(desired_x_dx_ddx_CoM, param); - -%% errors visualization and stored trajectory -traj_des = [desired_x_dx_ddx_CoM(:,1); desired_x_dx_ddx_CoM(:,2); desired_x_dx_ddx_CoM(:,3)]; -traj = [traj_des; traj_obt]; - -visual_kin_param.traj = traj; -visual_kin_param.Delta = Delta; -visual_kin_param.ddqjDes = ddqjDes; - -end diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_generator.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_generator.m deleted file mode 100644 index 3eba491..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_generator.m +++ /dev/null @@ -1,171 +0,0 @@ -function [ddqjDes, dikin_total, traj_obt, delta] = ikin_generator(desired_x_dx_ddx_CoM, param) -%% ikin_generator -% Computes the inverse kinematics of the robot from a desired CoM position -% to the desired joints position. -% The outputs are: -% -% ddqjDes [ndofx1] which is the vector of desired joints accelerations -% -% d_ikin_total [ndof+6x1] which is the vector of desired robot state time -% derivative -% -% traj_obt [9x1] which contains the desired CoM linear position, -% velocity and acceleration -% -% delta [3+nfeet_on_groundx1] which contains the CoM and feet position errors - -%% Setup parameters -PINV_TOL = 1e-8; - -Kcorr_feet = 10; -Kcorr_CoM = 10; -Kcorr_qj = 10; -ndof = param.ndof; - -%% Terms coming from state integration -integr_terms = param.kin_total; - -qt_b = integr_terms(1:7); -qjDes = integr_terms(8:7+ndof); - -[x_b_Des,Rb_Des] = frame2posrot(qt_b); - -%% Jacobian at feet and CoM -for ii=1:param.numConstraints - - Jc_ikin(6*(ii-1)+1:6*ii,:) = wbm_jacobian(Rb_Des,x_b_Des,qjDes,param.constraintLinkNames{ii}); - -end - -JCoM_ikin_total = wbm_jacobian(Rb_Des,x_b_Des,qjDes,'com'); -JCoM_ikin = JCoM_ikin_total(1:3,:); - -%% CoM position -CoM_PosAndOrient = wbm_forwardKinematics(Rb_Des,x_b_Des,qjDes,'com'); -xCoM_real = CoM_PosAndOrient(1:3); - -%% Feet position and orientation -lsole = wbm_forwardKinematics(Rb_Des,x_b_Des,qjDes,'l_sole'); -rsole = wbm_forwardKinematics(Rb_Des,x_b_Des,qjDes,'r_sole'); - -[x_lfoot,Rb_lfoot] = frame2posrot(lsole); -[x_rfoot,Rb_rfoot] = frame2posrot(rsole); - -[T_rfoot,phi_rfoot] = parametrization(Rb_rfoot); -[T_lfoot,phi_lfoot] = parametrization(Rb_lfoot); - -pos_leftFoot = [x_lfoot; phi_lfoot.']; -pos_rightFoot = [x_rfoot; phi_rfoot.']; - -% initial feet position and orientation -lfoot_ini = param.lfoot_ini; -rfoot_ini = param.rfoot_ini; - -[xInit_rfoot,RbInit_rfoot] = frame2posrot(rfoot_ini); -[xInit_lfoot,RbInit_lfoot] = frame2posrot(lfoot_ini); - -[~,phiInit_lfoot] = parametrization(RbInit_lfoot); -[~,phiInit_rfoot] = parametrization(RbInit_rfoot); - -lfoot_initPosAndOrient = [xInit_lfoot; phiInit_lfoot.']; -rfoot_initPosAndOrient = [xInit_rfoot; phiInit_rfoot.']; - - if sum(param.feet_on_ground) == 1 && param.feet_on_ground(1) == 1 - - T_tildeLeftFoot = [eye(3) zeros(3); - zeros(3) T_lfoot]; - - feetPosAndOrient_real = T_tildeLeftFoot*(pos_leftFoot-lfoot_initPosAndOrient); - - elseif sum(param.feet_on_ground) == 1 && param.feet_on_ground(2) == 1 - - T_tildeRightFoot = [eye(3) zeros(3); - zeros(3) T_rfoot]; - - feetPosAndOrient_real = T_tildeRightFoot*(pos_rightFoot-rfoot_initPosAndOrient); - - elseif sum(param.feet_on_ground) == 2 - - T_tildeLeftFoot = [eye(3) zeros(3); - zeros(3) T_lfoot]; - - T_tildeRightFoot = [eye(3) zeros(3); - zeros(3) T_rfoot]; - - T_tildeTotal = [T_tildeLeftFoot zeros(6); - zeros(6) T_tildeRightFoot]; - - feetPosAndOrient_real = T_tildeTotal*[pos_leftFoot-lfoot_initPosAndOrient; - pos_rightFoot-rfoot_initPosAndOrient]; - - end - -%% CoM and feet correction terms -delta_pos_CoM = xCoM_real - desired_x_dx_ddx_CoM(:,1); -delta_pos_feet = feetPosAndOrient_real; - -%% Desired joints velocity -Nu_ikin = integr_terms(8+ndof:end); -dqjDes = Nu_ikin(7:end); - -Nu_ikin_base = Nu_ikin(1:6); -dqt_b = quaternionDerivative(transpose(Rb_Des)*Nu_ikin_base(4:end), qt_b(4:end)); - -%% Jacobian time derivative -for ii=1:param.numConstraints - - dJcNu(6*(ii-1)+1:6*ii,:) = wbm_djdq(Rb_Des,x_b_Des,qjDes,dqjDes,Nu_ikin_base,param.constraintLinkNames{ii}); - -end - -dJCoMNu_total = wbm_djdq(Rb_Des,x_b_Des,qjDes,dqjDes,Nu_ikin_base,'com'); -dJCoMNu = dJCoMNu_total(1:3); - -%% Feet and CoM corrections of velocities -Kcorr_feet_Nu = 2*sqrt(Kcorr_feet); -Kcorr_CoM_Nu = 2*sqrt(Kcorr_CoM); -Kcorr_dq = 2*sqrt(Kcorr_qj); - -delta_vel_feet = Jc_ikin*Nu_ikin; -delta_vel_CoM = JCoM_ikin*Nu_ikin - desired_x_dx_ddx_CoM(:,2); - -%% Desired joints accelerations -% first task: constraints at feet -feetPosAndOrient = -dJcNu -Kcorr_feet*delta_pos_feet -Kcorr_feet_Nu*delta_vel_feet; -NullFeet = eye(ndof+6) - pinv(Jc_ikin,PINV_TOL)*Jc_ikin; - -% second task: CoM trajectory -CoMPos = -dJCoMNu + desired_x_dx_ddx_CoM(:,3) - Kcorr_CoM*delta_pos_CoM -Kcorr_CoM_Nu*delta_vel_CoM... - -JCoM_ikin*pinv(Jc_ikin,PINV_TOL)*feetPosAndOrient; - -JCoM_0 = JCoM_ikin*NullFeet; -NullCoM = eye(ndof+6) - pinv(JCoM_0,PINV_TOL)*JCoM_0; - -% third task: posture -Jddqj_ikin = [zeros(ndof,6) eye(ndof)]; - -posturePos = -Kcorr_dq*(Nu_ikin(7:end)) -Kcorr_qj*(qjDes-param.qjInit)... - -Jddqj_ikin*pinv(Jc_ikin,PINV_TOL)*feetPosAndOrient -Jddqj_ikin*NullFeet*pinv(JCoM_0,PINV_TOL)*CoMPos; - -Jddqj_0 = Jddqj_ikin*NullFeet*NullCoM; - -% state acceleration -dNu_00 = pinv(Jddqj_0,PINV_TOL)*posturePos; -dNu_0 = pinv(JCoM_0, PINV_TOL)*CoMPos + NullCoM*dNu_00; -dNu_ikin = pinv(Jc_ikin, PINV_TOL)*feetPosAndOrient + NullFeet*dNu_0; - -% joints acceleration -ddqjDes = dNu_ikin(7:end); - -%% Vector for integration -dikin_total = [Nu_ikin_base(1:3); dqt_b; dqjDes; dNu_ikin]; - -%% Obtained trajectory at CoM -traj_obt = [xCoM_real; JCoM_ikin*Nu_ikin; (JCoM_ikin*dNu_ikin+dJCoMNu)]; - -%% CoM and feet position error -delta = [delta_pos_CoM; delta_pos_feet]; - -end - - diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_graphics.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_graphics.m deleted file mode 100644 index 72825dc..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_graphics.m +++ /dev/null @@ -1,125 +0,0 @@ -function [] = ikin_graphics(params) -% ikin_graphics -% visualizes inverse kinematics results -% comparison between desired CoM and obtained Com trajectory; -% CoM error between desired and obtained trajectory; -% Feet position and orientation error - -%% parameters definition -set(0,'DefaultFigureWindowStyle','Docked'); - -t = params.t_kin; -traj = params.joints_traj.traj; -delta = params.joints_traj.delta; - -%% visualizer - -if params.visualizerIkin == 1 - -title_graphics = {'X direction' 'Y direction' 'Z direction'}; -title2 = {'Error on desired x CoM trajectory' 'Error on desired y CoM trajectory' 'Error on desired z CoM trajectory'}; - -for k = 1:3 -% desired and obtained trajectory at CoM -% position -figure(1) -subplot(3,1,k) -plot(t,traj(k,:),'b') -hold on -grid on -plot(t,traj(k+9,:),'r') - -xlabel('s') -ylabel('m') -title(title_graphics(k)) -legend('desired CoM pos','obtained CoM pos') - -% velocity -figure(2) -subplot(3,1,k) -plot(t,traj(k+3,:),'b') -hold on -grid on -plot(t,traj(k+9+3,:),'r') - -xlabel('s') -ylabel('m/s') -title(title_graphics(k)) -legend('desired CoM vel','obtained CoM vel') - -% acceleration -figure(3) -subplot(3,1,k) -plot(t,traj(k+6,:),'b') -hold on -grid on -plot(t,traj(k+9+6,:),'r') - -xlabel('s') -ylabel('m/s^2') -title(title_graphics(k)) -legend('desired CoM acc','obtained CoM acc') - -% error on desired CoM trajectory -figure(4) -subplot(3,1,k) -plot(t,delta(k,:),'b') -hold on -grid on -title(title2(k)) -xlabel('s') -ylabel('m') - -end - -%% error on desired feet pos and orient -tit3 = {'Error on desired left foot pos along x axis','Error on desired left foot pos along y axis','Error on desired left foot pos along z axis',... - 'Error on desired left foot orient along x axis','Error on desired left foot orient along y axis','Error on desired left foot orient along z axis',... - 'Error on desired right foot pos along x axis','Error on desired right foot pos along y axis','Error on desired right foot pos along z axis',... - 'Error on desired right foot orient along x axis','Error on desired right foot orient along y axis','Error on desired right foot orient along z axis'}; - -tit4 = {'m' 'm' 'm' 'rad' 'rad' 'rad'}; - -if params.numConstraints == 1 - -for k=1:6 - -figure(5) -subplot(3,2,k) -plot(t,delta(k,:)) -grid on -title(tit3(k)) -xlabel('s') -ylabel(tit4(k)) - -end - -elseif params.numConstraints == 2 - -for k=1:6 - -figure(5) -subplot(3,2,k) -plot(t,delta(k,:)) -grid on -title(tit3(k)) -xlabel('s') -ylabel(tit4(k)) - -figure(6) -subplot(3,2,k) -plot(t,delta(k+6,:)) -grid on -title(tit3(k+6)) -xlabel('s') -ylabel(tit4(k)) - -end - -end - -end - -set(0,'DefaultFigureWindowStyle','Normal'); - -end diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_integrator.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_integrator.m deleted file mode 100644 index 0eb2ce2..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/inverseKinematics/ikin_integrator.m +++ /dev/null @@ -1,90 +0,0 @@ -function [t_kin,joints_traj] = ikin_integrator(params) -%% ikin_integrator -% Calculates the desired joints position and velocity by integrating the desired -% joints acceleration coming from inverse kinematics. It uses forward euler integration algorithm. -% The output are: -% -% t_kin which is the vector of integration time -% -% joints_traj which is a structure containing joint referece position, -% velocity and acceleration and CoM desired trajectory - -%% Define the integration time (fixed step) -t_kin = params.tStart:params.euler_step:params.tEnd; -t_kin = t_kin'; -step = params.euler_step; -ndof = params.ndof; - -%% Initial condition for joints position, velocity and acceleration -ikin_init = params.ikin_init; - -[~,ikinParam] = ikin_function(t_kin(1), ikin_init, params ); -ddqjDesInit = ikinParam.ddqjDes; - -% initial condition for errors visualization -ikinDeltaInit = ikinParam.Delta; -trajInit = ikinParam.traj; - -%% Setup integration -integratedFunction = @(t,ikin) ikin_function( t,ikin,params ); - -% setup matrix dimensions -dim_t = length(t_kin); -dim_qj = length(ikin_init); - -qj = zeros(ndof,dim_t); -dqj = qj; -ddqj = dqj; - -qState = zeros(dim_qj,dim_t); - -qState(:,1) = ikin_init; -qj(:,1) = ikin_init(8:7+ndof); -dqj(:,1) = ikin_init(14+ndof:end); -ddqj(:,1) = ddqjDesInit; - -if params.numConstraints == 2 - -ikinDelta = zeros(15,dim_t); - -elseif params.numConstraints == 1 - -ikinDelta = zeros(9,dim_t); - -end - -traj = zeros(18,dim_t); - -ikinDelta(:,1) = ikinDeltaInit; -traj(:,1) = trajInit; - -%% Euler forward integrator -for k = 2:dim_t - -% total state (floating base pose + joints position + Nu) - qState(:,k) = qState(:,k-1) + step.*integratedFunction(t_kin(k-1),qState(:,k-1)); - -% desired joints position, velocity and acceleration - [~,ikinParam] = ikin_function(t_kin(k), qState(:,k), params); - - qj(:,k) = qState(8:7+ndof,k); - dqj(:,k) = qState(14+ndof:end,k); - ddqj(:,k) = ikinParam.ddqjDes; - -% errors visualization - ikinDelta(:,k) = ikinParam.Delta; - traj(:,k) = ikinParam.traj; - -end - -%% Joints trajectory definition -joints_traj.qj = qj; -joints_traj.dqj = dqj; -joints_traj.ddqj = ddqj; - -%% Parameters for visualization -joints_traj.delta = ikinDelta; -joints_traj.traj = traj; - -end - diff --git a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/visualizer_js.m b/matlab-src/torqueBalancing_matlab/JointSpace_balancing/visualizer_js.m deleted file mode 100644 index 9dd11e2..0000000 --- a/matlab-src/torqueBalancing_matlab/JointSpace_balancing/visualizer_js.m +++ /dev/null @@ -1,341 +0,0 @@ -function [] = visualizer_js(t,chi,params) -%% visualizer_js -% Generates a demo of the robot state obtained using forward dynamics -% integration, and plot some graphics such as contact forces, CoM error, -% control torques, ZMP at feet. The user can decide what has been -% represented by selecting the corresponding variable in -% integrateForwardDynamics.m - -%% Robot demo -ndof = params.ndof; - -if params.visualizerDemo == 1 - -BackGroundColor = [0 0 0]; -GridColor = [1 1 1]; -figure_main = figure('Name', 'iCub Simulator', 'NumberTitle', 'off',... - 'Position', [500,800,1200,650],'Color',BackGroundColor); - - sizeFig = [10 26 800 600]; -% sizeFig = get(0, 'MonitorPositions'); -% sizeFig = 2*sizeFig/3; -% sizeFig(1:2) = sizeFig(3:4)/10 ; - - set(gcf, 'position', sizeFig); - - params.figure_main = figure_main; - - set(figure_main, 'MenuBar', 'none', 'BackingStore', 'off'); - set(figure_main, 'BackingStore', 'off'); - - params.plot_main = zeros(1,4); - - plot_pos = [0.51,0.05,0.45,1; - 0.01,0.05,0.45,1]; - - for ii=1:2 - - params.plot_main(ii) = subplot('Position', plot_pos(ii,:)); - params.plot_objs{ii} = plot3(0,0,0,'.'); - hold on; - set(gca,'Color',BackGroundColor,'Xcolor',GridColor,'Ycolor',GridColor,'Zcolor',GridColor); - view([45 25 25]) - end - - axes(params.plot_main(1)); - -%root link trajectory - params.demux.baseOrientationType = 1; - robotConfiguration_t = zeros(size(chi(:,1:8+params.ndof-1))); - - for i = 1:length(t) - - [basePosei,jointAnglesi,~,~] = stateDemux(chi(i,:)',params); - robotConfiguration_t(i,:) = [basePosei(1:3)',basePosei(4:7)',jointAnglesi']; - - end - - visualizeForwardDynamics(robotConfiguration_t,params); - -% plot root link position - x_b = chi(:,1:3); - - set(0,'DefaultFigureWindowStyle','Docked'); - - figure(8) - plot3(x_b(1:end,1),x_b(1:end,2),x_b(1:end,3)); - hold on - plot3(x_b(1,1),x_b(1,2),x_b(1,3),'ro'); - grid on; - title('Position of the root link') - - axis square; -%axis equal - - xlabel('X(m)'); - ylabel('Y(m)'); - zlabel('Z(m)'); - - end - -%% Graphics visualization -if params.visualizerGraphics == 1 - -dim_t = length(t); - -qj = zeros(ndof,dim_t); -tau = zeros(ndof,dim_t); -pos_feet = zeros(12,dim_t); - -fc = zeros(6*params.numConstraints,dim_t); -CoP = zeros(2*params.numConstraints,dim_t); - -error_CoM = zeros(3,dim_t); -qjDes = qj; - -t_kin = params.t_kin; -qjIkin = params.joints_traj.qj; -norm_tau = zeros(dim_t,1); - -for time = 1:length(t) - -[~,visual] = forwardDynamics_js(t(time), chi(time,:)', params); - -qj(:,time) = visual.qj; -pos_feet(:,time) = visual.pos_feet; -fc(:,time) = visual.fc; - -error_CoM(:,time) = visual.errorCoM; -tau(:,time) = visual.tau; - -qjDes(:,time) = visual.qjDes; - -norm_tau(time) = norm(visual.tau); - -% CoP at feet -CoP(1) = -visual.fc(5)/visual.fc(3); -CoP(2) = visual.fc(4)/visual.fc(3); - -if sum(params.feet_on_ground) == 2 - -CoP(3) = -visual.fc(11)/visual.fc(9); -CoP(4) = visual.fc(10)/visual.fc(9); - -end - -end - -%% Feet position and orientation -for k=1:3 - -if params.feet_on_ground(1) == 1 - -figure(9) -subplot(1,2,1) -hold all -grid on -plot(t,pos_feet(k,:)) -title('Left foot position') -xlabel('s') -ylabel('m') - -figure(9) -subplot(1,2,2) -hold all -grid on -plot(t,pos_feet(k+3,:)) -title('Left foot orientation') -xlabel('s') -ylabel('rad') - -else - -figure(9) -subplot(1,2,1) -hold all -grid on -plot(t,pos_feet(k+6,:)) -title('Right foot position') -xlabel('s') -ylabel('m') - -figure(9) -subplot(1,2,2) -hold all -grid on -plot(t,pos_feet(k+9,:)) -title('Right foot orientation') -xlabel('s') -ylabel('rad') - -end - -end - -%% Contact forces -for k=1:6*params.numConstraints - -figure(10) -hold all -grid on -plot(t,fc(k,:)) -title('Contact forces and moments') -xlabel('s') -ylabel('N,Nm') - -end - -%% CoM error -for k=1:3 - -figure(11) -hold all -grid on -plot(t,error_CoM(k,:)) -title('CoM error') -xlabel('s') -ylabel('m') - -end - -%% Norm of joints torques -figure(12) -hold on -grid on -plot(t,norm_tau) -title('Square norm of joints torques') -xlabel('s') -ylabel('Nm') - -%% Joints variables visualization -if params.visualizerJoints == 1 - -for k=1:5 - -% Arms position -figure(13) -subplot(3,2,k) -plot(t,qj(k+3,:),t,qjDes(k+3,:),'r',t_kin,qjIkin(k+3,:),'k') -hold on -grid on -xlabel('s') -ylabel('rad') - -name = whatname('left_arm',k); -title(name) -legend('Real pos','Ikin pos','Desired pos') - -figure(14) -subplot(3,2,k) -plot(t,qj(k+3+5,:),t,qjDes(k+3+5,:),'r',t_kin,qjIkin(k+3+5,:),'k') -hold on -grid on -xlabel('s') -ylabel('rad') - -name = whatname('right_arm',k); -title(name) -legend('Real pos','Ikin pos','Desired pos') - -end - -for k=1:6 - -% Legs position -figure(15) -subplot(3,2,k) -plot(t,qj(k+13,:),t,qjDes(k+13,:),'r',t_kin,qjIkin(k+13,:),'k') -hold on -grid on -xlabel('s') -ylabel('rad') - -name = whatname('left_leg',k); -title(name) -legend('Real pos','Ikin pos','Desired pos') - -figure(16) -subplot(3,2,k) -plot(t,qj(k+13+6,:),t,qjDes(k+13+6,:),'r',t_kin,qjIkin(k+13+6,:),'k') -hold on -grid on -xlabel('s') -ylabel('rad') - -name = whatname('right_leg',k); -title(name) -legend('Real pos','Ikin pos','Desired pos') - -end - -for k=1:3 - -% Torso position -figure(17) -subplot(3,1,k) -plot(t,qj(k,:),t,qjDes(k,:),'r',t_kin,qjIkin(k,:),'k') -hold on -grid on -xlabel('s') -ylabel('rad') - -name = whatname('torso',k); -title(name) -legend('Real pos','Ikin pos','Desired pos') - -end - -end - -%% CoP at feet -for k=1:2 - -if sum(params.feet_on_ground) == 2 - -figure(18) -subplot(1,2,1) -plot(CoP(1,:),CoP(2,:)) -hold on -grid on -subplot(1,2,1) -plot(CoP(1,1),CoP(2,1),'or') -title('Left foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -figure(18) -subplot(1,2,2) -plot(CoP(3,:),CoP(4,:)) -hold on -grid on -subplot(1,2,2) -plot(CoP(3,1),CoP(4,1),'or') -title('Right foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -else - -figure(18) -plot(CoP(1,:),CoP(2,:)) -hold on -grid on -plot(CoP(1,1),CoP(2,1),'or') -title('Foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -end - -end - -end - -set(0,'DefaultFigureWindowStyle','Normal'); - -end - - \ No newline at end of file diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/README.md b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/README.md deleted file mode 100644 index a2bb3bc..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/README.md +++ /dev/null @@ -1,25 +0,0 @@ -## Stack of task balancing - -iCub balancing control implemented using a 'momentum-based' controller. - -### Start the simulation - --open 'integrateForwardDynamics_SoT.m' - --at the top of the script, the user can define different types of simulations, for example 1 foot or 2 feet balancing, activate QP solver, - activate the CoM movements, change the robot configuration. at the bottom of the script it is possible to modify the simulation time and the - integration time step. - -### Modify gains and CoM movements - --all these parameters can be modified opening the Matlab function 'gainsAndConstraints_SoT.m' - -### Visualize more graphics - --open 'forwardDynamics_SoT.m' - --at the bottom of the script, add to the structure 'visual_param' the parameters you need to visualize - --open 'visualizer_SoT.m' - --add a 'figure' and 'plot' the parameters you seleced previously \ No newline at end of file diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/forwardDynamics_SoT.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/forwardDynamics_SoT.m deleted file mode 100644 index d37c405..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/forwardDynamics_SoT.m +++ /dev/null @@ -1,184 +0,0 @@ -function [dchi,visual_param]=forwardDynamics_SoT(t,chi,param) -%% forwardDynamics_SoT -% This is the forward dynamics of the model loaded in the -% wholeBodyInterface from the URDF description. The dynamic model is -% described as an explicit ordinary differential equation of the form: -% -% dchi = forwardDynamics(t,chi) -% -% where chi is the variable to be integrated. For a floating base -% articulated chain, the variable chi contains the following -% subvariables: -% -% x_b: the cartesian position of the base (R^3) -% qt_b: the quaternion describing the orientation of the base (global parametrization of SO(3)) -% qj: the joint positions (R^ndof) -% dx_b: the cartesian velocity of the base (R^3) -% omega_b: the velocity describing the orientation of the base (SO(3)) -% dqj: the joint velocities (R^ndof) - - waitbar(t/param.tEnd,param.wait) - ndof = param.ndof; -% disp(t) - -%% Extraction of state -param.demux.baseOrientationType = 1; % sets the base orientation in stateDemux.m as positions + quaternions (1) or transformation matrix (0) -[basePose,qj,baseVelocity,dqj] = stateDemux(chi,param); - -% position and orientation -x_b = basePose(1:3,:); - -% normalize quaternions to avoid numerical errors -% qt_b = qt_b/norm(qt_b); - -qt_b = basePose(4:7,:); - -% linear and angular velocity -dx_b = baseVelocity(1:3,:); -omega_w = baseVelocity(4:6,:); - -Nu = [dx_b;omega_w;dqj]; - -% Obtaining the rotation matrix from root link to world frame -qT = [x_b;qt_b]; -[~,R_b] = frame2posrot(qT); - -%% MexWholeBodyModel functions -% dynamics -M = wbm_massMatrix(R_b,x_b,qj); -h = wbm_generalisedBiasForces(R_b,x_b,qj,dqj,[dx_b;omega_w]); -H = wbm_centroidalMomentum(R_b,x_b,qj,dqj,[dx_b;omega_w]); - -% forward kinematics -l_sole = wbm_forwardKinematics(R_b,x_b,qj,'l_sole'); -r_sole = wbm_forwardKinematics(R_b,x_b,qj,'r_sole'); -CoM = wbm_forwardKinematics(R_b,x_b,qj,'com'); - -%% Building up jacobians and dJNu -% contact jacobians -Jc = zeros(6*param.numConstraints,6+ndof); -dJcNu = zeros(6*param.numConstraints,1); - -for i=1:param.numConstraints - - Jc(6*(i-1)+1:6*i,:) = wbm_jacobian(R_b,x_b,qj,param.constraintLinkNames{i}); - dJcNu(6*(i-1)+1:6*i,:) = wbm_djdq(R_b,x_b,qj,dqj,[dx_b;omega_w],param.constraintLinkNames{i}); - -end - -% CoM jacobian -J_CoM = wbm_jacobian(R_b,x_b,qj,'com'); - -%% Joints limits check -limits = param.limits; -l_min = limits(:,1); -l_max = limits(:,2); -tol = 0.01; - -res = qj < l_min + tol | qj > l_max - tol; -res = sum(res); - -if res==0 - -else - - disp('Joint limits reached at time:') - disp(t) - error('Joint limits reached '); - -end - -%% Feet correction to avoid numerical integration errors -% feet correction gain -K_corr_pos = 2.5; -K_corr_vel = 2*sqrt(K_corr_pos); - -% feet current position and orientation -[x_lfoot,R_b_lfoot] = frame2posrot(l_sole); -[x_rfoot,R_b_rfoot] = frame2posrot(r_sole); - -% orientation is parametrized with euler angles -[~,phi_lfoot] = parametrization(R_b_lfoot); -[~,phi_rfoot] = parametrization(R_b_rfoot); - -pos_leftFoot = [x_lfoot; phi_lfoot']; -pos_rightFoot = [x_rfoot; phi_rfoot']; - -% feet original position and orientation -lsole_ini = param.lfoot_ini; -rsole_ini = param.rfoot_ini; - -[xi_lfoot,R_bi_lfoot] = frame2posrot(lsole_ini); -[xi_rfoot,R_bi_rfoot] = frame2posrot(rsole_ini); - -[~,phi_rfoot_ini] = parametrization(R_bi_rfoot); -[~,phi_lfoot_ini] = parametrization(R_bi_lfoot); - -lfoot_ini_tot = [xi_lfoot; phi_lfoot_ini']; -rfoot_ini_tot = [xi_rfoot; phi_rfoot_ini']; - -% error between original and current feet position and orientation -if param.feet_on_ground(1) == 1 && param.feet_on_ground(2) == 0 - - pos_feet_delta = pos_leftFoot-lfoot_ini_tot; - -elseif param.feet_on_ground(1) == 0 && param.feet_on_ground(2) == 1 - - pos_feet_delta = pos_rightFoot-rfoot_ini_tot; - -elseif param.feet_on_ground(1) == 1 && param.feet_on_ground(2) == 1 - - pos_feet_delta = [(pos_leftFoot-lfoot_ini_tot);... - (pos_rightFoot-rfoot_ini_tot)]; -end - -% parameters for controller -feet.l_sole = l_sole; -feet.r_sole = r_sole; - -%% Control torques calculation -xCoM = CoM(1:3); -xCoMDes = param.CoM_ini(1:3); -param.qj = qj; - -% gains and friction cones definition -[gains,constraints,trajectory] = gainsAndConstraints_SoT(param); - -% CoM trajectory generator -desired_x_dx_ddx_CoM = generTraj_SoT(xCoMDes,t,trajectory); - -% balancing controller -[tau,errorCoM,f0] = stackOfTaskController(param, constraints, feet, gains, Nu, M, h, H, Jc, dJcNu, xCoM, J_CoM, desired_x_dx_ddx_CoM); - -% Real contact forces computation -S = [ zeros(6,ndof); - eye(ndof,ndof)]; -JcMinv = Jc/M; -JcMinvS = JcMinv*S; - -fc = (JcMinv*transpose(Jc))\(JcMinv*h -JcMinvS*tau -dJcNu -K_corr_vel.*Jc*Nu -K_corr_pos.*pos_feet_delta); - -%% State derivative computation -% Need to calculate the quaternions derivative -omega_b = transpose(R_b)*omega_w; -dqt_b = quaternionDerivative(omega_b,qt_b); - -dx = [dx_b;dqt_b;dqj]; -dNu = M\(Jc'*fc + [zeros(6,1); tau]-h); - -dchi = [dx;dNu]; - -%% Visualization -% These are the variables that can be plotted by the visualizer.m -% function - visual_param.Href = [M(1,1)*desired_x_dx_ddx_CoM(:,2);zeros(3,1)]; - visual_param.H = H; - visual_param.pos_feet = [l_sole;r_sole]; - visual_param.fc = fc; - visual_param.tau = tau; - visual_param.qj = qj; - visual_param.error_com = errorCoM; - visual_param.f0 = f0; - -end - diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/gainsAndConstraints_SoT.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/gainsAndConstraints_SoT.m deleted file mode 100644 index fbc9377..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/gainsAndConstraints_SoT.m +++ /dev/null @@ -1,169 +0,0 @@ -function [gains, constraints, trajectory] = gainsAndConstraints_SoT(param) -%% gainsAndConstraints_SoT -% Generates the desired gains for both the joint and the CoM -% dynamics. It also generates the friction cones at feet. -% The output are: -% -% gains this is a structure which contains both CoM and joints gains -% -% constraints this is also a structure and contains the constraints for QP -% solver -% -% trajectory this structure contains all the parameters needed for -% CoM trajectory definition -% -%% General parameters -ndof = param.ndof; -Contact_constraints = param.numConstraints; -demo_movements = param.demo_movements; - -trajectory.directionOfOscillation = [0;0;0]; -trajectory.referenceParams = [0.0 0.0]; %referenceParams(1) = amplitude of ascillations in meters - %referenceParams(2) = frequency of ascillations in Hertz - -trajectory.noOscillationTime = 0; % If DEMO_LEFT_AND_RIGHT = 1, the variable noOscillationTime is the time, in seconds, - % that the robot waits before starting the left-and-righ - -qTildeMax = 20*pi/180; - -%% Pameters for 2 feet on the ground -if Contact_constraints == 2 - - gains.gainsPCoM = diag([ 40 40 40]); - gains.gainsDCoM = 2*sqrt(gains.gainsPCoM); - gains.gainPhi = 5; - gains.gainMomentum = 2*sqrt(gains.gainPhi); - -% impedances acting in the null space of the desired contact forces - impTorso = [ 40 40 40 - 0 0 0]; - - impArms = [ 10 10 10 5 5 - 0 0 0 0 0]; - - impLeftLeg = [ 35 40 10 30 5 10 - 0 0 0 0 0 0]; - - impRightLeg = [35 40 10 30 5 10 - 0 0 0 0 0 0]; - - if (demo_movements == 1) - - trajectory.directionOfOscillation = [0;1;0]; - trajectory.referenceParams = [0.035 0.35]; %referenceParams(1) = amplitude of ascillations in meters - - end - -end - -%% Parameters for 1 foot on the ground -if Contact_constraints == 1 - - gains.gainsPCoM = diag([40 45 40]); - gains.gainsDCoM = 2*sqrt(gains.gainsPCoM); - gains.gainPhi = 5; - gains.gainMomentum = 2*sqrt(gains.gainPhi); - -% impedances acting in the null space of the desired contact forces - impTorso = [ 20 20 20 - 0 0 0]; - - impArms = [ 15 15 15 5 5 - 0 0 0 0 0 ]; - -if param.feet_on_ground(1) == 1 - - impLeftLeg = [ 70 70 65 30 10 10 - 0 0 0 0 0 0]; - - impRightLeg = [ 20 20 20 10 10 10 - 0 0 0 0 0 0]; - -else - - impRightLeg = [ 70 70 65 30 10 10 - 0 0 0 0 0 0]; - - impLeftLeg = [ 20 20 20 10 10 10 - 0 0 0 0 0 0]; - -end - - if (demo_movements == 1) - - trajectory.directionOfOscillation = [0;1;0]; - trajectory.referenceParams = [0.005 0.25]; %referenceParams(1) = amplitude of ascillations in meters - - end - -end - -%% Definition of impedances and dampings vectors - - impedances = [impTorso(1,:),impArms(1,:),impArms(1,:),impLeftLeg(1,:),impRightLeg(1,:)]; - - gains.dampings = 0.5*ones(1,ndof); - - increasingRatesImp = [impTorso(2,:),impArms(2,:),impArms(2,:),impLeftLeg(2,:),impRightLeg(2,:)]; - -if (size(impedances,2) ~= ndof) - - error('Dimension mismatch between ndof and dimension of the variable impedences. Check these variables in the file gains.m'); - -end - -%% Constraints for QP for balancing on both feet - friction cone - z-moment - in terms of f (not f0) -% Friction cone parameters -numberOfPoints = 4; % 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 - -forceFrictionCoefficient = 1; -torsionalFrictionCoefficient = 2/150; - -footSize = [ -0.1 0.1; % xMin, xMax - -0.1 0.1 ]; % yMin, yMax - -fZmin = 10; - -% The QP solver will search a solution f0 that satisfies the inequality Aineq_f F(fo) < bineq_f -[constraints.ConstraintsMatrix,constraints.bVectorConstraints] = frictionCones(forceFrictionCoefficient,numberOfPoints,... - torsionalFrictionCoefficient,footSize,fZmin); -constraints.footSize = footSize; - -%% Impedances correction with a nonlinear term -gains.impedances = nonLinImp(param.qjInit,param.qj,impedances,increasingRatesImp,qTildeMax); - -%% New postural task - -% old postural -gains.impedances_old = gains.impedances; -gains.dampings_old = gains.dampings; - -% parameters definition -M = param.M0; -Jc = param.Jc0; -pinv_tol = 1e-8; - -S = [ zeros(6,ndof); - eye(ndof,ndof)]; - -JcMinv = Jc/M; -JcMinvS = JcMinv*S; - -% damped null space -Mbar = M(7:end,7:end)-M(7:end,1:6)/M(1:6,1:6)*M(1:6,7:end); -reg = 5e-6; -Pinv_JcMinvS_damp = JcMinvS'/(JcMinvS*JcMinvS' + reg*eye(size(JcMinvS,1))); -NullLambda_damp = eye(ndof)-Pinv_JcMinvS_damp*JcMinvS; - -% new postural task -impedances = diag(gains.impedances)*pinv(NullLambda_damp*Mbar,pinv_tol); -dampings = diag(gains.dampings)*pinv(NullLambda_damp*Mbar,pinv_tol); - -gains.impedances = impedances +0.1*eye(ndof); -gains.dampings = dampings +0.1*eye(ndof); - -gains.GainCorr_imp = NullLambda_damp*Mbar; -gains.GainCorr_damp = NullLambda_damp*Mbar; - -end diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/generTraj_SoT.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/generTraj_SoT.m deleted file mode 100644 index 35fc23e..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/generTraj_SoT.m +++ /dev/null @@ -1,30 +0,0 @@ -function desired_x_dx_ddx_CoM = generTraj_SoT (xCoM_0,t,trajectory) -%% generTraj_SoT -% Generates a desired trajectory for robot's global CoM. It can be applied -% in every direction (X,Y,Z) -% Output: -% -% desired_x_dx_ddx_CoM [3x3] it is a matrix wich contains the desired -% CoM position, velocity and acceleration -% -%% Trajectory generation -if t > trajectory.noOscillationTime - - Ampl = trajectory.referenceParams(1); - -else - - Ampl = 0; - -end - - freq = trajectory.referenceParams(2); - -% Desired trajectory - xCoMDes = xCoM_0 + Ampl*sin(2*pi*freq*t)*trajectory.directionOfOscillation; - dxCoMDes = Ampl*2*pi*freq*cos(2*pi*freq*t)*trajectory.directionOfOscillation; - ddxCoMDes = -Ampl*(2*pi*freq)^2*sin(2*pi*freq*t)*trajectory.directionOfOscillation; - - desired_x_dx_ddx_CoM = [xCoMDes dxCoMDes ddxCoMDes]; - -end \ No newline at end of file diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/integrateForwardDynamics_SoT.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/integrateForwardDynamics_SoT.m deleted file mode 100644 index d6868cc..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/integrateForwardDynamics_SoT.m +++ /dev/null @@ -1,182 +0,0 @@ -%% integrateForwardDynamics_SoT -% This is the main program for integrating the forward dynamics of the robot iCub in matlab. -% It integrates the robot state defined in forwardDynamics_SoT.m and the user can set -% how many feet are on the ground, decide if activate the robot's movements, -% plot forces, torques and joints variables, and activate a demo of the -% robot's movements. -clear all -close all -clc - -%% Initialize the mexWholeBodyModel -wbm_modelInitialise('icubGazeboSim'); - -%% Setup params for balancing controller -params.demo_movements = 1; %either 0 or 1; only for two feet on the ground -params.use_QPsolver = 0; %either 0 or 1 -params.use_Orientation = 1; %either 0 or 1 - -% balancing on two feet or one foot -params.feet_on_ground = [1,1]; %either 0 or 1; [left,right] - -% allows the visualization of torques, forces and other user-defined graphics -params.visualizer_graphics = 1; %either 0 or 1 -params.visualizer_demo = 1; %either 0 or 1 -params.visualizer_jointsPos = 0; %either 0 or 1; only if visualizer_graphics = 1 - -%% Setup general params -% this is assuming a 25DoF iCub -params.ndof = 25; - -% initial conditions -params.leftArmInit = [ -20 30 0.0 45 0.0]'; -params.rightArmInit = [ -20 30 0.0 45 0.0]'; -params.torsoInit = [ -10.0 0.0 0.0]'; - -if params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 1 - -% initial conditions for balancing on two feet - params.leftLegInit = [ 25.5 0.1 0.0 -18.5 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 0.1 0.0 -18.5 -5.5 -0.1]'; - -elseif params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 0 - -% initial conditions for the robot standing on the left foot - params.leftLegInit = [ 25.5 15.0 0.0 -18.5 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 5.0 0.0 -40 -5.5 -0.1]'; - -elseif params.feet_on_ground(1) == 0 && params.feet_on_ground(2) == 1 - -% initial conditions for the robot standing on the right foot - params.leftLegInit = [ 25.5 5.0 0.0 -40 -5.5 -0.1]'; - params.rightLegInit = [ 25.5 15.0 0.0 -18.5 -5.5 -0.1]'; - -end - -params.qjInit = [params.torsoInit;params.leftArmInit;params.rightArmInit;params.leftLegInit;params.rightLegInit]*(pi/180); -params.dqjInit = zeros(params.ndof,1); - -params.dx_bInit = zeros(3,1); -params.omega_bInit = zeros(3,1); - -%% Updating the robot position -wbm_updateState(params.qjInit,params.dqjInit,[params.dx_bInit;params.omega_bInit]); - -% fixing the world reference frame w.r.t. the foot on ground position -if params.feet_on_ground(1) == 1 - -[R_b0,x_b0] = wbm_getWorldFrameFromFixedLink('l_sole',params.qjInit); - -else - -[R_b0,x_b0] = wbm_getWorldFrameFromFixedLink('r_sole',params.qjInit); - -end - -wbm_setWorldFrame(R_b0,x_b0,[0 0 -9.81]') - -[~,T_b,~,~] = wbm_getState(); - -params.chiInit = [T_b; params.qjInit; params.dx_bInit; params.omega_bInit; params.dqjInit]; - -%% Contact constraints definition -if params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 1 - -% contact constraints for 2 feet on ground - params.constraintLinkNames = {'l_sole','r_sole'}; - -elseif params.feet_on_ground(1) == 1 && params.feet_on_ground(2) == 0 - -% contact constraints for left foot on ground - params.constraintLinkNames = {'l_sole'}; - -elseif params.feet_on_ground(1) == 0 && params.feet_on_ground(2) == 1 - -% contact constraints for right foot on ground - params.constraintLinkNames = {'r_sole'}; - -end - - params.numConstraints = length(params.constraintLinkNames); - -%% Momentum jacobian and linearization of angular momentum integral -% momentum jacobian at the eq. point -Jw_b0 = zeros(6,6); -Jw_j0 = zeros(6,params.ndof); -Jc0 = zeros(6*params.numConstraints,params.ndof+6); - -for ii = 1:6 - -Nu_base = zeros(6,1); -Nu_base(ii) = 1; - -Jw_b0(:,ii) = wbm_centroidalMomentum(R_b0, x_b0, params.qjInit, zeros(params.ndof,1), Nu_base); - -end - -for ii = 1:params.ndof - -dqj_Jw = zeros(params.ndof,1); -dqj_Jw(ii) = 1; - -Jw_j0(:,ii) = wbm_centroidalMomentum(R_b0, x_b0, params.qjInit, dqj_Jw, zeros(6,1)); - -end - -% contacts jacobian at the eq. point -for i=1:params.numConstraints - - Jc0(6*(i-1)+1:6*i,:) = wbm_jacobian(R_b0,x_b0,params.qjInit,params.constraintLinkNames{i}); - -end - -Jw0 = Jw_j0 -Jw_b0*(eye(6)/Jc0(1:6,1:6))*Jc0(1:6,7:end); - -% angular momentum integral linearization -params.linAngInt = Jw0(4:end,:); - -% parameters for the new postural task -params.M0 = wbm_massMatrix(R_b0,x_b0,params.qjInit); -params.Jc0 = Jc0; - -%% Others parameters for balancing controller - [jl1,jl2] = wbm_jointLimits(); - limits = [jl1 jl2]; - params.limits = limits; - params.CoM_ini = wbm_forwardKinematics('com'); - - params.lfoot_ini = wbm_forwardKinematics('l_sole'); - params.rfoot_ini = wbm_forwardKinematics('r_sole'); - -%% Setup integration - plot_set - - params.tStart = 0; - params.tEnd = 10; - params.sim_step = 0.01; - params.wait = waitbar(0,'State integration in progress...'); - - forwardDynFunc = @(t,chi)forwardDynamics_SoT(t,chi,params); - -%% Integrate forward dynamics -if params.demo_movements == 0 - - options = odeset('RelTol',1e-3,'AbsTol', 1e-4); - -else - - options = odeset('RelTol',1e-6,'AbsTol',1e-6); - -end - - [t,chi] = ode15s(forwardDynFunc,params.tStart:params.sim_step:params.tEnd,params.chiInit,options); - - delete(params.wait) - -%% Visualize forward dynamics -params.wait = waitbar(0,'Graphics generation in progress...'); - -visualizer_SoT(t,chi,params) - -delete(params.wait) - \ No newline at end of file diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/stackOfTaskController.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/stackOfTaskController.m deleted file mode 100644 index aa10fde..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/stackOfTaskController.m +++ /dev/null @@ -1,228 +0,0 @@ -function [tau,errorCoM,f0] = stackOfTaskController(param, constraints, feet, gains, Nu, M, h, H, Jc, dJcNu,... - xCoM, J_CoM, desired_x_dx_ddx_CoM) -%% stackOfTaskController -% Computes the desired contact forces and moments and the joint torques for iCub balancing control using the "Stack of Task" approach. -% If QP_SOLVER is selected, it uses a quadratic programming to calculate the null space of desired contact forces which minimize -% the norm of joint torques. -% The output are: -% -% tau [nDof x 1] which is the vector of control torques at joints; -% -% errorCoM [3 x 1] which is the CoM position error with respect of the -% desired CoM trajectory; -% -% f0 [numConstraints x 1] which is the vector in the null space of contact -% forces and moments -% -%% Variables and tolerances definition - pinv_tol = 1e-8; - regHessiansQP = 5e-2; -%reg = 0.01; - -feet_on_ground = param.feet_on_ground; -ndof = param.ndof; -use_QPsolver = param.use_QPsolver; - -ConstraintsMatrix = constraints.ConstraintsMatrix; -bVectorConstraints = constraints.bVectorConstraints ; -footSize = constraints.footSize; - -e1 = [1;0;0]; -e2 = [0;1;0]; -e3 = [0;0;1]; - -gravAcc = 9.81; - -m = M(1,1); -Mb = M(1:6,1:6); -Mbj = M(1:6,7:end); - -S = [ zeros(6,ndof); - eye(ndof,ndof)]; - -f_grav = [ zeros(2,1); - -m*gravAcc; - zeros(3,1)]; - -% Feet position and rotation matrix -[pos_rightFoot,rotMatRightFoot] = frame2posrot(feet.r_sole); -[pos_leftFoot,rotMatLeftFoot] = frame2posrot(feet.l_sole); - -dxCoM = J_CoM(1:3,:)*Nu; -dqj = Nu(7:end); - -ddxCoMStar = desired_x_dx_ddx_CoM(:,3) - gains.gainsPCoM*(xCoM-desired_x_dx_ddx_CoM(:,1))... - -gains.gainsDCoM*(dxCoM-desired_x_dx_ddx_CoM(:,2)); - -Pr = pos_rightFoot - xCoM; % Application point of the contact force on the right foot w.r.t. CoM -Pl = pos_leftFoot - xCoM; % Application point of the contact force on the left foot w.r.t. CoM - -AL = [ eye(3),zeros(3); - skew(Pl),eye(3)]; -AR = [ eye(3),zeros(3); - skew(Pr),eye(3)]; - -%% One foot or two feet on ground selector -if sum(feet_on_ground) == 2 - - A = [AL,AR]; - pinvA = pinv(A,pinv_tol); - -else - - if feet_on_ground(1) == 1 - - A = AL; - - elseif feet_on_ground(2) == 1 - - A = AR; - - end - - pinvA = eye(6)/A; - -end - -%% Contact constraints equations - JcMinv = Jc/M; - JcMinvS = JcMinv*S; - JcMinvJct = JcMinv*transpose(Jc); - - Pinv_JcMinvS = pinv(JcMinvS,pinv_tol); -%Pinv_JcMinvS = JcMinvS'/(JcMinvS*JcMinvS' + reg*eye(size(JcMinvS,1))); - -%% Newton-Euler equations of motion at CoM - -% Closing the loop on centroidal orientation -Kphi = gains.gainPhi; -qTilde = param.qj-param.qjInit; -deltaPhi = param.linAngInt*qTilde; - -if param.use_Orientation == 1 - -HDotDes = [ m*ddxCoMStar; - -gains.gainMomentum*H(4:end)-Kphi*deltaPhi]; - -else - -HDotDes = [ m*ddxCoMStar; - -gains.gainMomentum*H(4:end)]; - -end - -f_HDot = pinvA*(HDotDes - f_grav); - -% Forces and torques null spaces -Nullfc = eye(6*param.numConstraints)-pinvA*A; -NullLambda = eye(ndof)-Pinv_JcMinvS*JcMinvS; - -%% Terms used to define the joints torques -JBar = transpose(Jc(:,7:end)) - Mbj'/Mb*transpose(Jc(:,1:6)); % multiplier of f in tau0 - -Sigma = -(Pinv_JcMinvS*JcMinvJct + NullLambda*JBar); -SigmaNA = Sigma*Nullfc; - -if param.use_Orientation == 1 - -% new postural gains -impedances = gains.impedances; -dampings = gains.dampings; - -GainCorr_imp = gains.GainCorr_imp; -GainCorr_damp = gains.GainCorr_damp; - -else - -impedances = diag(gains.impedances_old); -dampings = diag(gains.dampings_old); - -GainCorr_imp = eye(ndof); -GainCorr_damp = eye(ndof); - -end - -tauModel = Pinv_JcMinvS*(JcMinv*h - dJcNu) +NullLambda*(h(7:end) -Mbj'/Mb*h(1:6)... - -impedances*GainCorr_imp*qTilde -dampings*GainCorr_damp*dqj); - -%% Quadratic Programming solver -f0 = zeros(6,1); - -if use_QPsolver == 1 - -CL = ConstraintsMatrix; -CL(end-4,1:3) = -e3'*rotMatLeftFoot; -CL(end-3,:) = [ footSize(1,1)*e3'*rotMatLeftFoot', e2'*rotMatLeftFoot']; -CL(end-2,:) = [-footSize(1,2)*e3'*rotMatLeftFoot',-e2'*rotMatLeftFoot']; - -CL(end-1,:) = [ footSize(2,1)*e3'*rotMatLeftFoot',-e1'*rotMatLeftFoot']; -CL(end ,:) = [-footSize(2,2)*e3'*rotMatLeftFoot', e1'*rotMatLeftFoot']; - -CR = ConstraintsMatrix; -CR(end-4,1:3) = -e3'*rotMatRightFoot; -CR(end-3,:) = [ footSize(1,1)*e3'*rotMatRightFoot', e2'*rotMatRightFoot']; -CR(end-2,:) = [-footSize(1,2)*e3'*rotMatRightFoot',-e2'*rotMatRightFoot']; - -CR(end-1,:) = [ footSize(2,1)*e3'*rotMatRightFoot',-e1'*rotMatRightFoot']; -CR(end ,:) = [-footSize(2,2)*e3'*rotMatRightFoot', e1'*rotMatRightFoot']; - - -ConstraintsMatrix2Feet = blkdiag(CL,CR); -bVectorConstraints2Feet = [bVectorConstraints;bVectorConstraints]; - -ConstraintsMatrixQP1Foot = CL; -bVectorConstraintsQP1Foot = bVectorConstraints; - -if sum(feet_on_ground) == 1 - -HessianMatrixQP1Foot = A'*A; -gradientQP1Foot = -A'*(HDotDes - f_grav); - -[f_HDot, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot,... - [], [], [], bVectorConstraintsQP1Foot); - -elseif sum(feet_on_ground) == 2 - -ConstraintsMatrixQP2Feet = ConstraintsMatrix2Feet*Nullfc; -bVectorConstraintsQp2Feet = bVectorConstraints2Feet-ConstraintsMatrix2Feet*f_HDot; - -HessianMatrixQP2Feet = SigmaNA'*SigmaNA + eye(size(SigmaNA,2))*regHessiansQP; -gradientQP2Feet = SigmaNA'*(tauModel + Sigma*f_HDot); - -[f0, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP2Feet, gradientQP2Feet, ConstraintsMatrixQP2Feet,... - [], [], [], bVectorConstraintsQp2Feet); - -end - -if exitFlag ~= 0 - - disp('QP failed with:') - disp(exitFlag); - disp(iter); - disp(auxOutput); - disp(lambda); - error('qp_failed') - -end - -else - -% Desired f0 without Quadratic Programming - -if sum(feet_on_ground) == 2 - - f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*f_HDot); - -end - -end - -%% Joint torques and contact forces -% CoM error -errorCoM = xCoM - desired_x_dx_ddx_CoM(:,1); - -% Desired contact forces at feet and control torques -fc_des = f_HDot + Nullfc*f0; -tau = tauModel + Sigma*fc_des; - -end diff --git a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/visualizer_SoT.m b/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/visualizer_SoT.m deleted file mode 100644 index 150bbed..0000000 --- a/matlab-src/torqueBalancing_matlab/StackOfTask_balancing/visualizer_SoT.m +++ /dev/null @@ -1,394 +0,0 @@ -function [] = visualizer_SoT(t,chi,param) -%% visualizer_SoT -% Visualizes some user-defined parameters such as contact forces, torques, -% CoM error. It can also generate a demo of the robot's movements. - -%% Demo generation -ndof = param.ndof; - -if param.visualizer_demo == 1 - -BackGroundColor = [0 0 0]; -GridColor = [1 1 1]; -figure_main = figure('Name', 'iCub Simulator', 'NumberTitle', 'off',... - 'Position', [500,800,1200,650],'Color',BackGroundColor); - - sizeFig = [10 26 800 600]; -% sizeFig = get(0, 'MonitorPositions'); -% sizeFig = 2*sizeFig/3; -% sizeFig(1:2) = sizeFig(3:4)/10 ; - - set(gcf, 'position', sizeFig); - - param.figure_main = figure_main; - - set(figure_main, 'MenuBar', 'none', 'BackingStore', 'off'); - set(figure_main, 'BackingStore', 'off'); - - param.plot_main = zeros(1,4); - - plot_pos = [0.51,0.05,0.45,1; - 0.01,0.05,0.45,1]; - - for ii=1:2 - - param.plot_main(ii) = subplot('Position', plot_pos(ii,:)); - param.plot_objs{ii} = plot3(0,0,0,'.'); - hold on; - set(gca,'Color',BackGroundColor,'Xcolor',GridColor,'Ycolor',GridColor,'Zcolor',GridColor); - view([45 25 25]) - - end - - axes(param.plot_main(1)); - -% root link trajectory - param.demux.baseOrientationType = 1; - robotConfiguration_t = zeros(size(chi(:,1:8+param.ndof-1))); - - for i = 1:length(t) - - [basePosei,jointAnglesi,~,~] = stateDemux(chi(i,:)',param); - robotConfiguration_t(i,:) = [basePosei(1:3)',basePosei(4:7)',jointAnglesi']; - - end - - visualizeForwardDynamics(robotConfiguration_t,param); - -%% Plot base link position -set(0,'DefaultFigureWindowStyle','Docked'); - -x_b = chi(:,1:3); - -figure(2) -plot3(x_b(:,1),x_b(:,2),x_b(:,3)); -hold on -plot3(x_b(1,1),x_b(1,2),x_b(1,3),'ro'); -grid on; -title('Root link position') - - axis square; -%axis equal - -xlabel('X(m)'); -ylabel('Y(m)'); -zlabel('Z(m)'); - -end - -%% Graphics generation -if param.visualizer_graphics == 1 - -% generates the parameters defined in forwardDynamics_SoT.m -qj = zeros(param.ndof,length(t)); -fc = zeros(6*param.numConstraints,length(t)); -f0 = zeros(6*param.numConstraints,length(t)); -tau = zeros(param.ndof,length(t)); -error_CoM = zeros(3,length(t)); -pos_feet = zeros(14,length(t)); -norm_tau = zeros(1,length(t)); -CoP = zeros(4,length(t)); -phi_lfoot = zeros(3,length(t)); -phi_rfoot = zeros(3,length(t)); -H = zeros(6,length(t)); -Href = zeros(6,length(t)); -norm_qjErr = zeros(1,length(t)); -norm_HErr = zeros(1,length(t)); - -for time=1:length(t) - -[~,visual] = forwardDynamics_SoT(t(time), chi(time,:)', param); - -qj(:,time) = visual.qj; -pos_feet(:,time) = visual.pos_feet; -fc(:,time) = visual.fc; -f0(:,time) = visual.f0; -tau(:,time) = visual.tau; -error_CoM(:,time) = visual.error_com; -H(:,time) = visual.H; -Href(:,time) = visual.Href; - -% square norms -norm_tau(time) = norm(visual.tau); -norm_qjErr(time) = norm(visual.qj-param.qjInit); -norm_HErr(time) = norm(visual.H-visual.Href); - -CoP(1,time) = -visual.fc(5)/visual.fc(3); -CoP(2,time) = visual.fc(4)/visual.fc(3); - -if param.numConstraints == 2 - -CoP(3,time) = -visual.fc(11)/visual.fc(9); -CoP(4,time) = visual.fc(10)/visual.fc(9); - -end - -% left foot orientation -quat_lFoot = visual.pos_feet(1:7); -[~,Rot_lFoot] = frame2posrot(quat_lFoot); -[~,phi_lfoot(:,time)] = parametrization(Rot_lFoot); - -% right foot orientation -quat_rFoot = visual.pos_feet(8:end); -[~,Rot_rFoot] = frame2posrot(quat_rFoot); -[~,phi_rfoot(:,time)] = parametrization(Rot_rFoot); - -end - -%% Feet position and orientation -for k=1:3 - -if param.feet_on_ground(1) == 1 - -figure(3) -subplot(1,2,1) -hold all -grid on -plot(t,pos_feet(k,:)) -title('Left foot position') -xlabel('s') -ylabel('m') - -figure(3) -subplot(1,2,2) -hold all -grid on -plot(t,phi_lfoot(k,:)) -title('Left foot orientation') -xlabel('s') -ylabel('rad') - -else - -figure(3) -subplot(1,2,1) -hold all -grid on -plot(t,pos_feet(k+7,:)) -title('Right foot position') -xlabel('s') -ylabel('m') - -figure(3) -subplot(1,2,2) -hold all -grid on -plot(t,phi_rfoot(k,:)) -title('Right foot orientation') -xlabel('s') -ylabel('rad') - -end - -end - -%% Contact forces -if sum(param.feet_on_ground) == 1 - -for k=1:6 - -figure(4) -hold all -grid on -plot(t,fc(k,:)) -title('Contact forces and moments') -xlabel('s') -ylabel('N,Nm') - -end - -elseif sum(param.feet_on_ground) == 2 - -for k=1:12 - -figure(4) -hold all -grid on -plot(t,fc(k,:)) -title('Contact forces and moments') -xlabel('s') -ylabel('N,Nm') - -figure(5) -hold all -grid on -plot(t,f0(k,:)) -title('Contact forces and moments null space') -xlabel('s') -ylabel('N,Nm') - -end - -end - -%% Control torques and CoM error -for k=1:ndof - -figure(6) -hold all -grid on -plot(t,tau(k,:)) -title('Torques at joints') -xlabel('s') -ylabel('Nm') - -end - -for k=1:3 - -figure(7) -hold all -grid on -plot(t,error_CoM(k,:)) -title('CoM error') -xlabel('s') -ylabel('m') - -end - -figure(8) -hold on -grid on -plot(t,norm_tau) -title('Square norm of joints torques') -xlabel('s') -ylabel('Nm') - -%% Joints positions -if param.visualizer_jointsPos == 1 - -for k=1:5 - -% Robot arms -figure(9) -subplot(3,2,k) -plot(t,qj(k+3,:)) -hold on -grid on -xlabel('s') -ylabel('rad') -name = whatname('left_arm',k); -title(name) - -figure(10) -subplot(3,2,k) -plot(t,qj(k+3+5,:)) -hold on -grid on -xlabel('s') -ylabel('rad') -name = whatname('right_arm',k); -title(name) - -end - -for k=1:6 - -% Robot legs -figure(11) -subplot(3,2,k) -plot(t,qj(k+13,:)) -hold on -grid on -xlabel('s') -ylabel('rad') -name = whatname('left_leg',k); -title(name) - -figure(12) -subplot(3,2,k) -plot(t,qj(k+13+6,:)) -hold on -grid on -xlabel('s') -ylabel('rad') -name = whatname('right_leg',k); -title(name) - -end - -for k=1:3 - -% Robot torso -figure(13) -subplot(3,1,k) -plot(t,qj(k,:)) -hold on -grid on -xlabel('s') -ylabel('rad') -name = whatname('torso',k); -title(name) - -end - -end - -%% CoP at feet -for k=1:2 - -if sum(param.feet_on_ground) == 2 - -figure(18) -subplot(1,2,1) -plot(CoP(1,:),CoP(2,:)) -hold on -grid on -subplot(1,2,1) -plot(CoP(1,1),CoP(2,1),'or') -title('Left foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -figure(18) -subplot(1,2,2) -plot(CoP(3,:),CoP(4,:)) -hold on -grid on -subplot(1,2,2) -plot(CoP(3,1),CoP(4,1),'or') -title('Right foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -else - -figure(18) -plot(CoP(1,:),CoP(2,:)) -hold on -grid on -plot(CoP(1,1),CoP(2,1),'or') -title('Foot CoP') -xlabel('Y direction (m)') -ylabel('X direction (m)') -axis([-0.1 0.1 -0.1 0.1]) - -end - -end - -%% Square norm of joints positions -figure(15) -hold on -grid on -plot(t,norm_qjErr) - -xlabel('s') -ylabel('rad') -title('Square norm of joints pos error') - -%% Square norm of H error -figure(16) -hold on -grid on -plot(t,norm_HErr) - -xlabel('s') -ylabel('|H-H_{ref}|') -title('Square norm of H error') - -set(0,'DefaultFigureWindowStyle','Normal'); - -end diff --git a/mex-wholebodymodel/matlab/utilities/QPSolver.m b/mex-wholebodymodel/matlab/utilities/QPSolver.m new file mode 100644 index 0000000..40d86ad --- /dev/null +++ b/mex-wholebodymodel/matlab/utilities/QPSolver.m @@ -0,0 +1,106 @@ +function fcDes = QPSolver(controlParam,CONFIG,FORKINEMATICS) +%QPSOLVER implements a quadratic programming solver. The objective is the +% minimization of the control torques by means of the nullspace of +% contact forces, f0. Inequality constraints to ensure fc is inside +% the friction cones are also taken into account in the optimization. +% +% fcDes = QPSOLVER(controlParam,config,forKinematics) takes as +% input the structures: CONTROLPARAM, containing the controller +% parameters; CONFIG, which contains all the configuration +% parameters and FORKINEMATICS, which contains the forward kinematics +% of the robot. The output are the desired contact forces +% FCDES [6*numconstraints x 1]. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 + +% ------------Initialization---------------- +%% Config parameters +regHessianQP = CONFIG.reg_HessianQP; +feet_on_ground = CONFIG.feet_on_ground; + +%% Control parameters +fcHDot = controlParam.fcHDot; +tauModel = controlParam.tauModel; +Sigma = controlParam.Sigma; +Nullfc = controlParam.Nullfc; +A = controlParam.A; +HDotDes = controlParam.HDotDes; +f_grav = controlParam.f_grav; +SigmaNA = Sigma*Nullfc; + +%% Forward Kinematics +RFootPoseQuat = FORKINEMATICS.RFootPoseQuat; +LFootPoseQuat = FORKINEMATICS.LFootPoseQuat; + +%% Constraints for QP +e1 = [1;0;0]; +e2 = [0;1;0]; +e3 = [0;0;1]; +[~,RotRFoot] = frame2posrot(RFootPoseQuat); +[~,RotLFoot] = frame2posrot(LFootPoseQuat); + +% the friction cone is approximated by using linear interpolation of the circle. +% numberOfPoints defines the number of points used to interpolate the circle in +% each cicle's quadrant +numberOfPoints = 4; +forceFrictionCoefficient = 1; +torsionalFrictionCoefficient = 2/150; +footSize = [-0.1 0.1; % xMin, xMax + -0.1 0.1]; % yMin, yMax +fZmin = 10; + +% the QP solver will search a solution f0 that satisfies the inequality Aineq_f F(f0) < bineq_f +[ConstraintsMatrix,bVectorConstraints] = frictionCones(forceFrictionCoefficient,numberOfPoints,torsionalFrictionCoefficient,footSize,fZmin); + +%% QP SOLVER +CL = ConstraintsMatrix; +CL(end-4,1:3) = -e3'*RotLFoot; +CL(end-3,:) = [ footSize(1,1)*e3'*RotLFoot', e2'*RotLFoot']; +CL(end-2,:) = [-footSize(1,2)*e3'*RotLFoot',-e2'*RotLFoot']; +CL(end-1,:) = [ footSize(2,1)*e3'*RotLFoot',-e1'*RotLFoot']; +CL(end ,:) = [-footSize(2,2)*e3'*RotLFoot', e1'*RotLFoot']; +CR = ConstraintsMatrix; +CR(end-4,1:3) = -e3'*RotRFoot; +CR(end-3,:) = [ footSize(1,1)*e3'*RotRFoot', e2'*RotRFoot']; +CR(end-2,:) = [-footSize(1,2)*e3'*RotRFoot',-e2'*RotRFoot']; +CR(end-1,:) = [ footSize(2,1)*e3'*RotRFoot',-e1'*RotRFoot']; +CR(end ,:) = [-footSize(2,2)*e3'*RotRFoot', e1'*RotRFoot']; + +ConstraintsMatrix2Feet = blkdiag(CL,CR); +bVectorConstraints2Feet = [bVectorConstraints;bVectorConstraints]; +ConstraintsMatrixQP1Foot = CL; +bVectorConstraintsQP1Foot = bVectorConstraints; + +if sum(feet_on_ground) == 1 + + HessianMatrixQP1Foot = A'*A; + gradientQP1Foot = -A'*(HDotDes-f_grav); + + [fcDes, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot,... + [], [], [], bVectorConstraintsQP1Foot); + +elseif sum(feet_on_ground) == 2 + + ConstraintsMatrixQP2Feet = ConstraintsMatrix2Feet*Nullfc; + bVectorConstraintsQp2Feet = bVectorConstraints2Feet-ConstraintsMatrix2Feet*fcHDot; + HessianMatrixQP2Feet = SigmaNA'*SigmaNA + eye(size(SigmaNA,2))*regHessianQP; + gradientQP2Feet = SigmaNA'*(tauModel + Sigma*fcHDot); + + [f0, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP2Feet, gradientQP2Feet, ConstraintsMatrixQP2Feet,... + [], [], [], bVectorConstraintsQp2Feet); + + fcDes = fcHDot + Nullfc*f0; +end + +if exitFlag ~= 0 + + disp('QP failed with:') + disp(exitFlag); + disp(iter); + disp(auxOutput); + disp(lambda); + error('QP failed') +end + +end diff --git a/mex-wholebodymodel/matlab/utilities/euleroForward.m b/mex-wholebodymodel/matlab/utilities/euleroForward.m new file mode 100644 index 0000000..ecc2f98 --- /dev/null +++ b/mex-wholebodymodel/matlab/utilities/euleroForward.m @@ -0,0 +1,30 @@ +function [t,chi] = euleroForward(func,chiInit,tmax,tmin,tstep) +%EULEROFORWARD is a fixed step integrator for integrating the forward +% dynamics of the robot iCub in MATLAB. +% +% [t,chi] = EULEROFORWARD(func,chiInit,tmax,tmin,tstep) takes as input +% the function to be integrated, FUNC; the initial state of the robot, +% CHIINIT; the final and initial time and the time step, TMAX,TMIN,TSTEP. +% The outputs are the vector of integration time T and the state of the +% robot, CHI. +% +% Author : Gabriele Nava (gabriele.nava@iit.it) +% Genova, May 2016 +% + +% ------------Initialization---------------- +%% Initial conditions +t = transpose(tmin:tstep:tmax); +dimT = length(t); +variablesToIntegrate = length(chiInit); +chi = zeros(variablesToIntegrate,dimT); +chi(:,1) = chiInit; + +%% Euler fixed step integrator +for k = 2:dimT + + chi(:,k) = chi(:,k-1) + tstep.*func(t(k-1),chi(:,k-1)); +end + +chi = chi.'; +end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/frictionCones.m b/mex-wholebodymodel/matlab/utilities/frictionCones.m index b402c0f..a71af4c 100644 --- a/mex-wholebodymodel/matlab/utilities/frictionCones.m +++ b/mex-wholebodymodel/matlab/utilities/frictionCones.m @@ -20,41 +20,41 @@ % define equations for i = 1 : numberOfEquations - firstPoint = points(:, i); - secondPoint = points(:, rem(i, numberOfEquations) + 1); - - % define line passing through the above points - angularCoefficients = (secondPoint(2) - firstPoint(2)) / (secondPoint(1) - firstPoint(1)); - offsets = firstPoint(2) - angularCoefficients * firstPoint(1); - inequalityFactor = +1; - - % if any of the two points are between pi and 2pi, then the inequality is - % in the form of y >= m*x + q, and I need to change the sign of it. - - if (angle(i) > pi || angle(rem(i, numberOfEquations) + 1) > pi) - inequalityFactor = -1; - end - - % a force is 6 dimensional f = [fx, fy, fz, mux, muy, muz]' - % I have constraints on fy and fz, and the offset will be multiplied by - % mu * fx - Aineq(i,:) = inequalityFactor .* [-angularCoefficients, 1, -offsets * staticFrictionCoefficient, 0, 0, 0]; - + firstPoint = points(:, i); + secondPoint = points(:, rem(i, numberOfEquations) + 1); + + % define line passing through the above points + angularCoefficients = (secondPoint(2) - firstPoint(2)) / (secondPoint(1) - firstPoint(1)); + offsets = firstPoint(2) - angularCoefficients * firstPoint(1); + inequalityFactor = +1; + + % if any of the two points are between pi and 2pi, then the inequality is + % in the form of y >= m*x + q, and I need to change the sign of it. + + if (angle(i) > pi || angle(rem(i, numberOfEquations) + 1) > pi) + inequalityFactor = -1; + end + + % a force is 6 dimensional f = [fx, fy, fz, mux, muy, muz]' + % I have constraints on fy and fz, and the offset will be multiplied by + % mu * fx + Aineq(i,:) = inequalityFactor .* [-angularCoefficients, 1, -offsets * staticFrictionCoefficient, 0, 0, 0]; + end -CFoot = [ 0 , 0, -torsionalFrictionCoefficient, 0, 0, 1; - 0 , 0, -torsionalFrictionCoefficient, 0, 0,-1; +CFoot = [ 0 , 0, -torsionalFrictionCoefficient, 0, 0, 1; + 0 , 0, -torsionalFrictionCoefficient, 0, 0,-1; 0 , 0, -1, 0, 0, 0; - 0 , 0, footSize(1,1), 0, 1, 0; - 0 , 0, -footSize(1,2), 0, -1, 0; - 0 , 0, footSize(2,1), -1, 0, 0; + 0 , 0, footSize(1,1), 0, 1, 0; + 0 , 0, -footSize(1,2), 0, -1, 0; + 0 , 0, footSize(2,1), -1, 0, 0; 0 , 0, -footSize(2,2), 1, 0, 0]; CFoot = [ Aineq ; - CFoot ]; - + CFoot ]; + dConstraints = [zeros(size(Aineq,1), 1); zeros(7,1)]; -dConstraints(3 + size(Aineq,1)) = -fZmin; +dConstraints(3 + size(Aineq,1)) = -fZmin; end diff --git a/mex-wholebodymodel/matlab/utilities/fromMesh2sortedVector.m b/mex-wholebodymodel/matlab/utilities/fromMesh2sortedVector.m index c138659..909f727 100644 --- a/mex-wholebodymodel/matlab/utilities/fromMesh2sortedVector.m +++ b/mex-wholebodymodel/matlab/utilities/fromMesh2sortedVector.m @@ -1,9 +1,10 @@ +%% Utility function for visualizeForwardDynamics.m function P = fromMesh2sortedVector(x,y,z) - x = x(:)'; - y = y(:)'; - P = [x;y]; - if nargin == 3 - z = z(:)'; - P = [P;z]; - end +x = x(:)'; +y = y(:)'; +P = [x;y]; +if nargin == 3 + z = z(:)'; + P = [P;z]; +end end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/interpolation.m b/mex-wholebodymodel/matlab/utilities/interpolation.m deleted file mode 100644 index eaebea3..0000000 --- a/mex-wholebodymodel/matlab/utilities/interpolation.m +++ /dev/null @@ -1,57 +0,0 @@ -function [jointReferences, errorCoM] = interpolation(t,params,xCoM) -%% interpolation -% Interpolates the trajectory obtained with inverse kinematic integrator -% to fit the step of state integrator. -% The outputs are: -% jointReferences which is a structure containing the desired joint -% position, velocity and acceleration; -% -% errorCoM [3x1] which is the CoM position error -% -%% Trajectory interpolation -time = params.t_kin; - -% find in the vector "time" the step which is before t (current time) -logic_t = time l_max - tol; +res = sum(res); + +if res == 0 +else + disp('Joint limits reached at time:') + disp(t) + error('Joint limits reached '); +end + +end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/nonLinImp.m b/mex-wholebodymodel/matlab/utilities/nonLinImp.m deleted file mode 100644 index 5be8344..0000000 --- a/mex-wholebodymodel/matlab/utilities/nonLinImp.m +++ /dev/null @@ -1,9 +0,0 @@ -function nonlinearImpedances = nonLinImp(qjDes, qj, impedences_at_0, increasingRatesImp, qTildeMax) -%% nonlinearImpedances -% Corrects the impedances at joints by adding a nonlinear term. -% The output is the vector of nonlinear impedances [ndofx1] -qTilde = qj-qjDes; - -nonlinearImpedances = (impedences_at_0 + increasingRatesImp.*(tan(pi.*qTilde./(2*qTildeMax)).^2).')'; - -end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/parametrization.m b/mex-wholebodymodel/matlab/utilities/parametrization.m index cbe88ba..3c8f806 100644 --- a/mex-wholebodymodel/matlab/utilities/parametrization.m +++ b/mex-wholebodymodel/matlab/utilities/parametrization.m @@ -1,8 +1,8 @@ function [T_bar,angles] = parametrization(rot_matr) -%% parametrization -% Computes the Z-Y-X parametrization from a given rotation matrix +%% PARAMETRIZATION +% Computes the Z-Y-X parametrization from a given rotation matrix. % The outputs are: -% +% % T_bar [3x3] the matix which converts the angular velocity into the % Euler angles time derivative % @@ -21,5 +21,5 @@ T_bar = [1 0 -sin(theta); 0 cos(phi) sin(phi)*cos(theta) ; 0 -sin(phi) cos(phi)*cos(theta)]; - + end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/pinvDamped.m b/mex-wholebodymodel/matlab/utilities/pinvDamped.m new file mode 100644 index 0000000..c42cf3c --- /dev/null +++ b/mex-wholebodymodel/matlab/utilities/pinvDamped.m @@ -0,0 +1,13 @@ +function pinvDampA = pinvDamped(A,regDamp) +% PINVDAMPED computes the damped pseudoinverse of matrix A + +% [U,S,V] = svd(A*A'); +% for i=1:min(size(S)) +% if S(i,i) <= regDamp +% S(i,i) = regDamp; +% end +% end +% pinvDampA = A'/(U*S*V'); + +pinvDampA = A'/(A*A' + regDamp*eye(size(A,1))); +end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/pinvWeight.m b/mex-wholebodymodel/matlab/utilities/pinvWeight.m new file mode 100644 index 0000000..8bb2c05 --- /dev/null +++ b/mex-wholebodymodel/matlab/utilities/pinvWeight.m @@ -0,0 +1,15 @@ +function [pinvM,NullSpace] = pinvWeight(M,Weight) +% pinvWeight computes the weighted pseudoinverse of amtricx M +% +% [pinvM,NullSpace] = pinvWeight(M,Weight) solves the least squares +% problem +% min ||Weight(x-x0)|| s.t. Mx = y +% x +% the solution is x = pinvM(y) + NullSpace(x0) +% + +[~,n] = size(M); +pinvM = Weight\pinvDamped(M/Weight,1e-4); +NullSpace = eye(n) - pinvM*M; + +end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/plotQuat.m b/mex-wholebodymodel/matlab/utilities/plotQuat.m index 5bcee88..a9533e5 100644 --- a/mex-wholebodymodel/matlab/utilities/plotQuat.m +++ b/mex-wholebodymodel/matlab/utilities/plotQuat.m @@ -1,5 +1,5 @@ function plotQuat(q) -% q is nx7 +% Plot quaternions. q is nx7 n = length(q); @@ -31,27 +31,23 @@ function plotQuat(q) set(gca,'drawmode','fast'); - pause; + for ii=1:n R = vrrotvec2mat([rx(ii) ry(ii) rz(ii) q_ang(ii)]); - - plot3(x(ii),y(ii),z(ii),'.'); set(pos3,'xdata',[x(ii)],'ydata',[y(ii)],'zdata',[z(ii)]); set(orix,'xdata',[x(ii) x(ii)+R(1,1)],'ydata',[y(ii) y(ii)+R(2,1)],'zdata',[z(ii) z(ii)+R(3,1)],'erasemode','normal','LineWidth', 2,'color', 'blue'); set(oriy,'xdata',[x(ii) x(ii)+R(1,2)],'ydata',[y(ii) y(ii)+R(2,2)],'zdata',[z(ii) z(ii)+R(3,2)],'erasemode','normal','LineWidth', 2,'color', 'green'); set(oriz,'xdata',[x(ii) x(ii)+R(1,3)],'ydata',[y(ii) y(ii)+R(2,3)],'zdata',[z(ii) z(ii)+R(3,3)],'erasemode','normal','LineWidth', 2,'color', 'red'); - - -% set(ori3,'xdata',[x(ii) x(ii)+rx(ii)],'ydata',[y(ii) y(ii)+ry(ii)],'zdata',[z(ii) z(ii)+rz(ii)]); + % set(ori3,'xdata',[x(ii) x(ii)+rx(ii)],'ydata',[y(ii) y(ii)+ry(ii)],'zdata',[z(ii) z(ii)+rz(ii)]); title(num2str(ii)); pause(0.0005); -% pause; -% drawnow; + % pause; + % drawnow; end diff --git a/mex-wholebodymodel/matlab/utilities/plot_set.m b/mex-wholebodymodel/matlab/utilities/plot_set.m index 2d0d492..031e8e1 100644 --- a/mex-wholebodymodel/matlab/utilities/plot_set.m +++ b/mex-wholebodymodel/matlab/utilities/plot_set.m @@ -1,11 +1,9 @@ -%% plot_set +%% PLOT_SET % This function is to set the figures options for controllers as the user desires. % %% Setup figure parameters % set(0,'DefaultFigureWindowStyle','Docked'); -%commandwindow - %% AXES % factoryAxesFontAngle: 'normal' @@ -32,7 +30,7 @@ % factoryLineLineWidth: 0.5000 set(0,'DefaultLineLineWidth',2); -% + %% GRID % factoryAxesGridLineStyle: ':' @@ -57,5 +55,5 @@ % factoryLineMarkerSize: 6 % set(0,'DefaultLineMarkerFaceColor','auto'); - set(0,'DefaultLineMarkerSize',8); +set(0,'DefaultLineMarkerSize',8); diff --git a/mex-wholebodymodel/matlab/utilities/quaternion2dcm.m b/mex-wholebodymodel/matlab/utilities/quaternion2dcm.m index a28476a..63ae9ca 100644 --- a/mex-wholebodymodel/matlab/utilities/quaternion2dcm.m +++ b/mex-wholebodymodel/matlab/utilities/quaternion2dcm.m @@ -1,7 +1,7 @@ function [dcm] = quaternion2dcm(quat) %QUATERNION2DCM Converts a quaternion (real/imaginary) to direction cosine matrix (aka Rotation matrix) % Arguments : -% Normal Mode : +% Input : % quat - (4 X 1) vector representing the quaternion with real/imaginary serialization % Returns : % dcm - Discrete Cosine Matrix (Rotation matrix 3 X 3) diff --git a/mex-wholebodymodel/matlab/utilities/quaternionDerivative.m b/mex-wholebodymodel/matlab/utilities/quaternionDerivative.m index 7ce9f37..385123d 100644 --- a/mex-wholebodymodel/matlab/utilities/quaternionDerivative.m +++ b/mex-wholebodymodel/matlab/utilities/quaternionDerivative.m @@ -3,15 +3,14 @@ %quaternion from angular velocity as a vector % Computes a derivate of quaternion using the classical form and includes % a stabilization term to make the vector sum equal to 1 -% The formula for the conversion from omega to the derivative of the -% quaternion comes from https://www-sop.inria.fr/act_recherche/formulaire/uploads/phd-425.pdf -% page 101. +% The formula for the conversion from omega to the derivative of the +% quaternion comes from https://www-sop.inria.fr/act_recherche/formulaire/uploads/phd-425.pdf +% page 101. K = 1; omegaCross = [0 -omega';omega -skew(omega)]; - -qDot = 0.5*omegaCross * q + K*(1-norm(q)) * q; +qDot = 0.5*omegaCross * q + K*(1-norm(q)) * q; end diff --git a/mex-wholebodymodel/matlab/utilities/roty.m b/mex-wholebodymodel/matlab/utilities/roty.m index a63379a..d64c6b3 100644 --- a/mex-wholebodymodel/matlab/utilities/roty.m +++ b/mex-wholebodymodel/matlab/utilities/roty.m @@ -1,9 +1,9 @@ - +%% Utility function for visualizeForwardDynamics.m function R = roty(alpha) - R = zeros(3, 3); - R(2,2) = 1; - R(1,1) = cos(alpha); - R(1,3) = sin(alpha); - R(3,1) = -sin(alpha); - R(3,3) = cos(alpha); +R = zeros(3, 3); +R(2,2) = 1; +R(1,1) = cos(alpha); +R(1,3) = sin(alpha); +R(3,1) = -sin(alpha); +R(3,3) = cos(alpha); end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/utilities/skew.m b/mex-wholebodymodel/matlab/utilities/skew.m index 6cc27ae..ebf20e1 100644 --- a/mex-wholebodymodel/matlab/utilities/skew.m +++ b/mex-wholebodymodel/matlab/utilities/skew.m @@ -1,7 +1,6 @@ function [ X ] = skew( x ) %SKEW Function to generate a 3X3 Skew Symmetric matrix out of a 3X1 vector. % Detailed explanation goes here - X=[0 -x(3) x(2) ; x(3) 0 -x(1) ; -x(2) x(1) 0 ]; - +X=[0 -x(3) x(2) ; x(3) 0 -x(1) ; -x(2) x(1) 0 ]; end diff --git a/mex-wholebodymodel/matlab/utilities/sortedVector2mesh.m b/mex-wholebodymodel/matlab/utilities/sortedVector2mesh.m index 022305e..96fb3d9 100644 --- a/mex-wholebodymodel/matlab/utilities/sortedVector2mesh.m +++ b/mex-wholebodymodel/matlab/utilities/sortedVector2mesh.m @@ -1,11 +1,11 @@ - +%% Utility function for visualizeForwardDynamics.m function P = sortedVector2mesh(x,Dx) - i1 = 1; - i2 = Dx; - for i =1:length(x)/Dx - P(:,i) = x(i1:i2); - i1 = i2 + 1; - i2 = i2 + Dx; - end +i1 = 1; +i2 = Dx; +for i =1:length(x)/Dx + P(:,i) = x(i1:i2); + i1 = i2 + 1; + i2 = i2 + Dx; +end end diff --git a/mex-wholebodymodel/matlab/utilities/stateDemux.m b/mex-wholebodymodel/matlab/utilities/stateDemux.m index 9ae7109..ef7c108 100644 --- a/mex-wholebodymodel/matlab/utilities/stateDemux.m +++ b/mex-wholebodymodel/matlab/utilities/stateDemux.m @@ -1,35 +1,34 @@ function [basePose,jointAngles,baseVelocity,jointsVelocity] = stateDemux(state,param) %#codegen - % state - % state(1:3) = I_p_b, i.e. base position w.r.t. inertial frame - % state(4:7) = Q_b, i.e. quaternion representing base orientation w.r.t. base frame. More precisely, dot(Q_b) = -0.5 SB(b_omega_b)Q_b - % state(8: 8+nDof-1) = qj, i.e. joint angles - % state(8+nDof:10+nDof) = I_v_b, i.e. base linear velocity w.r.t. inertial frame - % state(11+nDof:13+nDof) = I_omega_b, i.e. base angular velocity w.r.t. inertial frame - % state(14+nDof:13+2*nDof) = qjDot, i.e. joint angles +% state +% state(1:3) = I_p_b, i.e. base position w.r.t. inertial frame +% state(4:7) = Q_b, i.e. quaternion representing base orientation w.r.t. base frame. More precisely, dot(Q_b) = -0.5 SB(b_omega_b)Q_b +% state(8: 8+nDof-1) = qj, i.e. joint angles +% state(8+nDof:10+nDof) = I_v_b, i.e. base linear velocity w.r.t. inertial frame +% state(11+nDof:13+nDof) = I_omega_b, i.e. base angular velocity w.r.t. inertial frame +% state(14+nDof:13+2*nDof) = qjDot, i.e. joint angles + +nDof = param.ndof; +I_p_b = state(1:3,:); +Q_b = state(4:7,:); +jointAngles = state(8:8+nDof-1,:); +baseVelocity = state(8+nDof:13+nDof,:); +jointsVelocity = state(14+nDof:13+2*nDof,:); + + +if param.demux.baseOrientationType == 0 - nDof = param.ndof; - I_p_b = state(1:3,:); - Q_b = state(4:7,:); - jointAngles = state(8:8+nDof-1,:); - baseVelocity = state(8+nDof:13+nDof,:); - jointsVelocity = state(14+nDof:13+2*nDof,:); + % transformation matrix + I_R_b = quaternion2dcm(Q_b(:,1)); + basePose = [I_R_b,I_p_b(:,1);zeros(1,3),1]; +elseif param.demux.baseOrientationType == 1 - if param.demux.baseOrientationType == 0 - - % transformation matrix - I_R_b = quaternion2dcm(Q_b(:,1)); - basePose = [I_R_b,I_p_b(:,1);zeros(1,3),1]; - - elseif param.demux.baseOrientationType == 1 - - % position + quaternions - basePose = [I_p_b; Q_b]; - - end + % position + quaternions + + basePose = [I_p_b; Q_b]; +end - end diff --git a/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m b/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m index 16a34a5..b99ffe0 100644 --- a/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m +++ b/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m @@ -1,22 +1,22 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) -%% visualize_forwardDyn -% Visualize the simulation results obtained from integration -% of the forward dynamics of the iCub. -% -% visualize_forwardDyn(XOUT,PARAMETERS) visualizes the motion -% of the robot. XOUT is the output vector of the integration carried +%% visualize_forwardDyn +% Visualize the simulation results obtained from integration +% of the forward dynamics of the iCub. +% +% visualize_forwardDyn(XOUT,PARAMETERS) visualizes the motion +% of the robot. XOUT is the output vector of the integration carried % out in the forward dynamics part, containing the position and the -% orientation of the base and the joint positions in the first 32 -% elements of its row vectors along a time span. PARAMETERS is the -% struct variable which contains constant parameters related to the +% orientation of the base and the joint positions in the first 32 +% elements of its row vectors along a time span. PARAMETERS is the +% struct variable which contains constant parameters related to the % simulation environment, robot, controller etc. -% +% if nargin > 2 params.plotThusts = true; - % the list of link names that are used for identifying the jets positions + % the list of link names that are used for identifying the jets positions params.jets.frames = cell(4,1); params.jets.frames{1} = 'l_hand_dh_frame'; params.jets.frames{2} = 'r_hand_dh_frame'; @@ -32,25 +32,25 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) COLOR = [1 0.1 0]; n = size(xout,1); % number of instances of the simulation results -q = xout(:,1:7); % first 3 elements provide the position and next 4 elements provide the orientation of the base +q = xout(:,1:7); % first 3 elements provide the position and next 4 elements provide the orientation of the base q_noBpos = q; q_noBpos(:,1:3) = repmat([0 0 0.7],size(q(:,1:3),1),1); qj = xout(:,8:32); % joint positions -vis_speed = 1; % this variable is set to change the visualization speed, - % to make its speed close to the real time in case - % the simulation time step is changed. - -% the list of link/joint names that are used to construct the robot in the visualizer +vis_speed = 1; % this variable is set to change the visualization speed, +% to make its speed close to the real time in case +% the simulation time step is changed. + +% the list of link/joint names that are used to construct the robot in the visualizer L = cell(15,1); L{1} = 'root_link' ; L{2} = 'r_hip_1' ; L{3} = 'r_lower_leg' ; L{4} = 'r_sole' ; L{5} = 'l_hip_1' ; -L{6} = 'l_lower_leg' ; +L{6} = 'l_lower_leg' ; L{7} = 'l_sole' ; L{8} = 'neck_1' ; L{9} = 'r_shoulder_1'; @@ -61,7 +61,7 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) L{14} = 'l_gripper' ; L{15} = 'com' ; -% since for now the world reference frame is that of the codyco_balancing_world, +% since for now the world reference frame is that of the codyco_balancing_world, % the z-axis is the vertical axis for the robot. xaxis = 'xdata'; yaxis = 'ydata'; @@ -76,31 +76,31 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) for jj=1:n_plot for ii=1:n % at each instance - - % convert base state to rotation - [pB,R] = frame2posrot(squeeze(q(ii,:)')); - [pB_noBpos,R_noBpos] = frame2posrot(squeeze(q_noBpos(ii,:)')); - - % set world to base - wbm_setWorldFrame(R,pB,[0,0,-9.81]'); - kin(ii,:,jj) = (wbm_forwardKinematics(R,pB,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links - kin_noBpos(ii,:,jj) = (wbm_forwardKinematics(R_noBpos,pB_noBpos,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links - + + % convert base state to rotation + [pB,R] = frame2posrot(squeeze(q(ii,:)')); + [pB_noBpos,R_noBpos] = frame2posrot(squeeze(q_noBpos(ii,:)')); + + % set world to base + wbm_setWorldFrame(R,pB,[0,0,-9.81]'); + kin(ii,:,jj) = (wbm_forwardKinematics(R,pB,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links + kin_noBpos(ii,:,jj) = (wbm_forwardKinematics(R_noBpos,pB_noBpos,qj(ii,:)',L{jj}))'; % forward kinematics for the list of joints/links + end - + end kin(:,:,1) = q; % use base data instead of fwdkin rootlink % clear and reset the plots -for ii=2:-1:1 +for ii=2:-1:1 axes(params.plot_main(ii)); axis([-0.5 0.5 -0.42 0.58 0 1]); - set(gca,'XGrid','off','YGrid','off','ZGrid','off','XTick', [],'YTick', [],'ZTick', []); + set(gca,'XGrid','off','YGrid','off','ZGrid','off','XTick', [],'YTick', [],'ZTick', []); set(gca,'LineWidth',5); - set(gca, 'drawmode', 'fast'); + %set(gca, 'drawmode', 'fast'); cla; view([45 25 25]) drawnow @@ -133,7 +133,7 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) x(jj) = Ptemp(1); y(jj) = Ptemp(2); z(jj) = Ptemp(3); - + col = 'w.'; pos(jj) = plot3(x(jj),y(jj),z(jj),col); @@ -142,11 +142,11 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) % plot the position of the center of mass jj = n_plot; - [Ptemp,~] = frame2posrot(kin(1,:,jj)'); - x(jj) = Ptemp(1); - y(jj) = Ptemp(2); - z(jj) = Ptemp(3); - +[Ptemp,~] = frame2posrot(kin(1,:,jj)'); +x(jj) = Ptemp(1); +y(jj) = Ptemp(2); +z(jj) = Ptemp(3); + pos(jj) = plot3(x(jj),y(jj),z(jj),'g*'); % define the pairs between the joints that will form the links @@ -170,7 +170,7 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) xyzpatch.vertices = zeros(8,3); xyzpatch.faces = zeros(6,4); -% constant multipliers related to the sizes of the patches around the links to form the robot figure +% constant multipliers related to the sizes of the patches around the links to form the robot figure mult_patch = [0.07 , 0.03; 0.04 , 0.02; 0.03 , 0.02; @@ -184,16 +184,16 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 0.03 , 0.02; 0.025, 0.02; 0.02 , 0.02]; - + % plot the lines depicting the links for jj=1:n_lin - lin(jj) = line(xaxis,xyzpairs(jj,1:2),yaxis,xyzpairs(jj,3:4),zaxis,xyzpairs(jj,5:6),'erasemode','normal','linewidth',3,'color','red'); + lin(jj) = line(xaxis,xyzpairs(jj,1:2),yaxis,xyzpairs(jj,3:4),zaxis,xyzpairs(jj,5:6),'linewidth',3,'color','red'); % for the patches (to determine the orientation of the patch to be applied to the links) vectlnk = [xyzpairs(jj,2)-xyzpairs(jj,1),xyzpairs(jj,4)-xyzpairs(jj,3),xyzpairs(jj,6)-xyzpairs(jj,5)]; orthlnk = null(vectlnk); - orthlnk1 = mult_patch(jj,1)*orthlnk(:,1); + orthlnk1 = mult_patch(jj,1)*orthlnk(:,1); orthlnk2 = mult_patch(jj,2)*orthlnk(:,2); % offsets in the direction orthogonal to the link @@ -206,28 +206,28 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) xyzpatch.vertices = [xyzpairs(jj,2)+qq1(1) , xyzpairs(jj,4)+qq1(2) , xyzpairs(jj,6)+qq1(3); xyzpairs(jj,2)+qq2(1) , xyzpairs(jj,4)+qq2(2) , xyzpairs(jj,6)+qq2(3); xyzpairs(jj,2)+qq3(1) , xyzpairs(jj,4)+qq3(2) , xyzpairs(jj,6)+qq3(3); - xyzpairs(jj,2)+qq4(1) , xyzpairs(jj,4)+qq4(2) , xyzpairs(jj,6)+qq4(3); + xyzpairs(jj,2)+qq4(1) , xyzpairs(jj,4)+qq4(2) , xyzpairs(jj,6)+qq4(3); xyzpairs(jj,1)+qq1(1) , xyzpairs(jj,3)+qq1(2) , xyzpairs(jj,5)+qq1(3); xyzpairs(jj,1)+qq2(1) , xyzpairs(jj,3)+qq2(2) , xyzpairs(jj,5)+qq2(3); xyzpairs(jj,1)+qq3(1) , xyzpairs(jj,3)+qq3(2) , xyzpairs(jj,5)+qq3(3); xyzpairs(jj,1)+qq4(1) , xyzpairs(jj,3)+qq4(2) , xyzpairs(jj,5)+qq4(3)]; - - + + xyzpatch.faces = [ 1 2 3 4; 1 4 8 5; 5 8 7 6; 7 3 2 6; 2 6 5 1; 3 7 8 4]; - + lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,... 'FaceAlpha',0.2,'FaceColor',COLOR); - + end %% FEET PATCHES % right foot patch -jj = n_lin+1; +jj = n_lin+1; orthlnk1 = [0 0.03 0]'; orthlnk2 = [0 0 0.03]'; @@ -245,13 +245,13 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) xyzpairs(4,2)+qq2(1)+0.03 , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); xyzpairs(4,2)+qq3(1)+0.03 , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); xyzpairs(4,2)+qq4(1)+0.03 , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3)]; - + lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,'FaceAlpha',0.2,... - 'FaceColor',COLOR); + 'FaceColor',COLOR); % left foot patch -jj = n_lin+2; +jj = n_lin+2; orthlnk1 = [0 0.03 0]'; @@ -272,11 +272,11 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) xyzpairs(7,2)+qq4(1)+0.03 , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3)]; lnkpatch(jj) = patch('vertices',xyzpatch.vertices,'faces',xyzpatch.faces,'FaceAlpha',0.2,... - 'FaceColor',COLOR); - + 'FaceColor',COLOR); + %% PLOT THRUSTS if params.plotThusts - + F = 0.01; delta = 5*pi/180; nT = 2; @@ -287,21 +287,21 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) xT = sortedVector2mesh(Coord_Thrust(1,:),nT); yT = sortedVector2mesh(Coord_Thrust(2,:),nT); zT = sortedVector2mesh(Coord_Thrust(3,:),nT); - + hThI(1) = surf(xT,yT,zT); - set(hThI(1),'EdgeColor',[1 0 0],'LineStyle','none'); - + set(hThI(1),'EdgeColor',[1 0 0],'LineStyle','none'); + hold on; hThI(2) = surf(xT,yT,zT); - set(hThI(2),'EdgeColor',[1 0 0],'LineStyle','none'); + set(hThI(2),'EdgeColor',[1 0 0],'LineStyle','none'); hThI(3) = surf(xT,yT,zT); - set(hThI(3),'EdgeColor',[1 0 0],'LineStyle','none'); + set(hThI(3),'EdgeColor',[1 0 0],'LineStyle','none'); hThI(4) = surf(xT,yT,zT); - set(hThI(4),'EdgeColor',[1 0 0],'LineStyle','none'); + set(hThI(4),'EdgeColor',[1 0 0],'LineStyle','none'); hold off end - + % store axes objects' handles to a vector params.plot_objs{1} = [lnkpatch';lin';pos'];%;plot_traj]; @@ -310,211 +310,211 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) params.plot_objs{2} = copyobj(params.plot_objs{1},params.plot_main(2)); if params.plotThusts - + hThB = copyobj(hThI,params.plot_main(2)); end %% UPDATING THE PLOTS -ii=2; +ii=2; - while ii0 -vectlnk = [xyzpairs(4,2)-xyzpairs(4,1),xyzpairs(4,4)-xyzpairs(4,3),xyzpairs(4,6)-xyzpairs(4,5)]; -orthlnk = null(vectlnk); -orthlnk1 = mult_patch(4,1)*orthlnk(:,1); -orthlnk2 = mult_patch(4,2)*orthlnk(:,2); - -qq1 = orthlnk1+2*orthlnk2; -qq2 = -orthlnk1+2*orthlnk2; -qq3 = -orthlnk1-orthlnk2; -qq4 = orthlnk1-orthlnk2; - -xyzpatch.vertices = [xyzpairs(4,2)+qq1(1) , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); - xyzpairs(4,2)+qq2(1) , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); - xyzpairs(4,2)+qq3(1) , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); - xyzpairs(4,2)+qq4(1) , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3); - xyzpairs(4,2)+qq1(1)+0.03 , xyzpairs(4,4)+qq1(2) , xyzpairs(4,6)+qq1(3); - xyzpairs(4,2)+qq2(1)+0.03 , xyzpairs(4,4)+qq2(2) , xyzpairs(4,6)+qq2(3); - xyzpairs(4,2)+qq3(1)+0.03 , xyzpairs(4,4)+qq3(2) , xyzpairs(4,6)+qq3(3); - xyzpairs(4,2)+qq4(1)+0.03 , xyzpairs(4,4)+qq4(2) , xyzpairs(4,6)+qq4(3)]; - - -set(lnkpatch(jj),'vertices',xyzpatch.vertices); + pause(time_dif); -% left foot -jj = n_lin+2; - -vectlnk = [xyzpairs(7,2)-xyzpairs(7,1),xyzpairs(7,4)-xyzpairs(7,3),xyzpairs(7,6)-xyzpairs(7,5)]; -orthlnk = null(vectlnk); -orthlnk1 = mult_patch(7,1)*orthlnk(:,1); -orthlnk2 = mult_patch(7,2)*orthlnk(:,2); - -qq1 = orthlnk1+2*orthlnk2; -qq2 = -orthlnk1+2*orthlnk2; -qq3 = -orthlnk1-orthlnk2; -qq4 = orthlnk1-orthlnk2; - -xyzpatch.vertices = [xyzpairs(7,2)+qq1(1) , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); - xyzpairs(7,2)+qq2(1) , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); - xyzpairs(7,2)+qq3(1) , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); - xyzpairs(7,2)+qq4(1) , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3); - xyzpairs(7,2)+qq1(1)+0.03 , xyzpairs(7,4)+qq1(2) , xyzpairs(7,6)+qq1(3); - xyzpairs(7,2)+qq2(1)+0.03 , xyzpairs(7,4)+qq2(2) , xyzpairs(7,6)+qq2(3); - xyzpairs(7,2)+qq3(1)+0.03 , xyzpairs(7,4)+qq3(2) , xyzpairs(7,6)+qq3(3); - xyzpairs(7,2)+qq4(1)+0.03 , xyzpairs(7,4)+qq4(2) , xyzpairs(7,6)+qq4(3)]; - -set(lnkpatch(jj),'vertices',xyzpatch.vertices); - -% store axes objects to a vector -params.plot_objs{num_gigure} = [lnkpatch;lin;pos];%;plot_traj]; - -drawnow; - -% to update the visualizer speed to keep it close to real simulation time -time_dif = vis_speed*params.sim_step-toc(); + else -if time_dif>0 + vis_speed = vis_speed+1; - pause(time_dif); - -else - - vis_speed = vis_speed+1; - -end - end - -ii = ii+vis_speed; - + end - + + ii = ii+vis_speed; + +end + end diff --git a/mex-wholebodymodel/matlab/utilities/whatname.m b/mex-wholebodymodel/matlab/utilities/whatname.m index bad952d..b14f971 100644 --- a/mex-wholebodymodel/matlab/utilities/whatname.m +++ b/mex-wholebodymodel/matlab/utilities/whatname.m @@ -1,30 +1,25 @@ function name = whatname(name_group,k) -%% whatname -% It assigns titles to graphics according to robot's joints distribution. -% The inputs are the robot link name (torso, r_arm, l_arm,...) and the interested -% degree of freedom. +% WHATNAME is used to insert titles in the joints plots. It assign the +% titles according to the joints configuration in yarpRobotInterface.ini -%% Torso graphics +%% Torso a1 = strcmp(name_group,'torso'); if a1 == 1 -if k==1 - - name = 'torso pitch'; - -elseif k==2 - - name = 'torso roll'; - -elseif k==3 - - name = 'torso yaw'; - -end + if k==1 + + name = 'torso pitch'; + elseif k==2 + + name = 'torso roll'; + elseif k==3 + + name = 'torso yaw'; + end end -%% Rigth arm graphics +%% Rigth arm a2 = strcmp(name_group,'right_arm'); if a2 == 1 @@ -32,28 +27,22 @@ if k==1 name = 'r shoulder pitch'; - elseif k==2 name = 'r shoulder roll'; - elseif k==3 name = 'r shoulder yaw'; - elseif k==4 name = 'r elbow'; - elseif k==5 name = 'r wrist prosup'; - end - end - -%% Left arm graphics + +%% Left arm a3 = strcmp(name_group,'left_arm'); if a3 == 1 @@ -61,28 +50,22 @@ if k==1 name = 'l shoulder pitch'; - elseif k==2 name = 'l shoulder roll'; - elseif k==3 name = 'l shoulder yaw'; - elseif k==4 name = 'l elbow'; - elseif k==5 name = 'l wrist prosup'; - end - end -%% Left leg graphics +%% Left leg a4 = strcmp(name_group,'left_leg'); if a4 == 1 @@ -90,33 +73,25 @@ if k==1 name = 'l hip pitch'; - elseif k==2 name = 'l hip roll'; - elseif k==3 name = 'l hip yaw'; - elseif k==4 name = 'l knee'; - elseif k==5 name = 'l ankle pitch'; - - elseif k==6 name = 'l ankle roll'; - end - end -%% Rigth leg graphics +%% Rigth leg a5 = strcmp(name_group,'right_leg'); if a5 == 1 @@ -124,40 +99,30 @@ if k==1 name = 'r hip pitch'; - elseif k==2 name = 'r hip roll'; - elseif k==3 name = 'r hip yaw'; - elseif k==4 name = 'r knee'; - elseif k==5 name = 'r ankle pitch'; - - elseif k==6 name = 'r ankle roll'; - end - end - -%% Name check +%% Check the input atot = a1+a2+a3+a4+a5; -if atot ==0 - - error('The name required as a title for some joints-related graphics does not match any known joints name') +if atot == 0 + error('The input does not match any known joints name') end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_centroidalMomentum.m b/mex-wholebodymodel/matlab/wrappers/wbm_centroidalMomentum.m index 33f5f51..41e9492 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_centroidalMomentum.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_centroidalMomentum.m @@ -2,7 +2,7 @@ %WMB_CENTROIDALMOMENTUM computes the centroidal momentum of the system - %functions of the state q, state derivative qDot, and floating base %velocity vxb -% Arguments : +% Arguments : % Optimised Mode : No arguments % Normal Mode : R - rotation from rootLink to world frame (3 x 3) % p - translation from rootLink to world frame (3 x 1) @@ -14,13 +14,13 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - H = mexWholeBodyModel('centroidal-momentum'); - case 5 - H = mexWholeBodyModel('centroidal-momentum',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5}); - otherwise - disp('centroidalMomentum : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + H = mexWholeBodyModel('centroidal-momentum'); + case 5 + H = mexWholeBodyModel('centroidal-momentum',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5}); + otherwise + disp('centroidalMomentum : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_djdq.m b/mex-wholebodymodel/matlab/wrappers/wbm_djdq.m index c7d946e..a9ef2f3 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_djdq.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_djdq.m @@ -1,7 +1,7 @@ function [ dJdq ] = wbm_djdq( varargin ) %WBM_DJDQ computes the product of derivative of Jacobian wrt to state %(DJ/dq) and the derivative of state for a desired link. Used for providing external contact contraints -% Arguments : +% Arguments : % Optimised Mode : link_name - string matching URDF name of the link (frame) % Normal Mode : R - rotation from rootLink to world frame (3 x 3) % p - translation from rootLink to world frame (3 x 1) @@ -14,15 +14,15 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genovas, Dec 2014 - switch(nargin) - case 1 - dJdq = mexWholeBodyModel('djdq',varargin{1}); - case 6 - dJdq = mexWholeBodyModel('djdq',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5},varargin{6}); - otherwise - disp('djdq : Incorrect number of arguments, check docs'); - end - +switch(nargin) + case 1 + dJdq = mexWholeBodyModel('djdq',varargin{1}); + case 6 + dJdq = mexWholeBodyModel('djdq',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5},varargin{6}); + otherwise + disp('djdq : Incorrect number of arguments, check docs'); +end + end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_forwardKinematics.m b/mex-wholebodymodel/matlab/wrappers/wbm_forwardKinematics.m index 286dad0..c80503a 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_forwardKinematics.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_forwardKinematics.m @@ -1,6 +1,6 @@ - function [p] = wbm_forwardKinematics(varargin) +function [p] = wbm_forwardKinematics(varargin) %WBM_FORWARDKINEMATICS computes the forward kinematics rototranslation to a specified link in the current joint -% configuration. +% configuration. % Arguments : % Optimised Mode : link_name - string matching URDF name of the link (frame) % Normal Mode : R - rotation from rootLink to world frame (3 x 3) @@ -14,13 +14,13 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 1 - p = mexWholeBodyModel('forward-kinematics',varargin{1}); - case 4 - p = mexWholeBodyModel('forward-kinematics',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4}); - otherwise - disp('forwardKinematics : Incorrect number of arguments, check docs'); - end - +switch(nargin) + case 1 + p = mexWholeBodyModel('forward-kinematics',varargin{1}); + case 4 + p = mexWholeBodyModel('forward-kinematics',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4}); + otherwise + disp('forwardKinematics : Incorrect number of arguments, check docs'); +end + end \ No newline at end of file diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_generalisedBiasForces.m b/mex-wholebodymodel/matlab/wrappers/wbm_generalisedBiasForces.m index f3aaf89..45f2aa5 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_generalisedBiasForces.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_generalisedBiasForces.m @@ -9,18 +9,18 @@ % qj - joint angles (NumDoF x 1) % dqj - joint velocities (NumDoF x 1) % vxb - floating base velocity (6 x 1) -% Returns : Cqv - Generalised bias forces (6+NumDoF x 1) +% Returns : Cqv - Generalised bias forces (6+NumDoF x 1) % % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - Cqv = mexWholeBodyModel('generalised-forces'); - case 5 - Cqv = mexWholeBodyModel('generalised-forces',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5}); - otherwise - disp('generalisedBiasForces : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + Cqv = mexWholeBodyModel('generalised-forces'); + case 5 + Cqv = mexWholeBodyModel('generalised-forces',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4},varargin{5}); + otherwise + disp('generalisedBiasForces : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_getState.m b/mex-wholebodymodel/matlab/wrappers/wbm_getState.m index 3d4844c..d640f1d 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_getState.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_getState.m @@ -14,12 +14,12 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - [qj,xTbT,qjDot,vb] = mexWholeBodyModel('get-state'); - xTb = xTbT; - otherwise - disp('getState : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + [qj,xTbT,qjDot,vb] = mexWholeBodyModel('get-state'); + xTb = xTbT; + otherwise + disp('getState : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_getWorldFrameFromFixedLink.m b/mex-wholebodymodel/matlab/wrappers/wbm_getWorldFrameFromFixedLink.m index e0a8da8..aa34aa4 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_getWorldFrameFromFixedLink.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_getWorldFrameFromFixedLink.m @@ -1,7 +1,7 @@ function [ world_R_base,world_p_base ] = wbm_getWorldFrameFromFixedLink( varargin ) %WBM_GETWORLDFRAMEFROMFIXEDLINK returns the floating base position wrt to a %world frame that is intentionally set at a specified link frame. The -%returned floating base position and rotation is obtained from forward kinematics +%returned floating base position and rotation is obtained from forward kinematics %wrt the specified link frame % % Arguments : @@ -9,41 +9,41 @@ % Normal Mode : link_name - string matching URDF name of the link (frame) % qj - joint configuration ( (NumDoF x 1)) % Returns : R - rotation from rootLink to world frame (3 x 3) -% p - translation from rootLink to world frame (3 x 1) +% p - translation from rootLink to world frame (3 x 1) % % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 1 - [world_R_base, world_p_base]= computeNewWorldToBase(varargin{1}); - case 2 - [world_R_base, world_p_base]= computeNewWorldToBase(varargin{1},varargin{2}); - otherwise - disp('getWorldFrameFromFixedLink : Incorrect number of arguments, check docs'); - end - +switch(nargin) + case 1 + [world_R_base, world_p_base]= computeNewWorldToBase(varargin{1}); + case 2 + [world_R_base, world_p_base]= computeNewWorldToBase(varargin{1},varargin{2}); + otherwise + disp('getWorldFrameFromFixedLink : Incorrect number of arguments, check docs'); +end + end function [world_R_base, world_p_base] = computeNewWorldToBase(varargin) - - [~,oldWorld_qH_base,~,~] = wbm_getState(); - [oldWorld_p_base,oldWorld_R_base] = frame2posrot(oldWorld_qH_base); - oldWorld_H_base = [oldWorld_R_base oldWorld_p_base; zeros(1,3) 1]; - - - if(nargin == 1) - [oldWorld_qH_referenceLink] = wbm_forwardKinematics(varargin{1}); - else - [oldWorld_qH_referenceLink] = wbm_forwardKinematics(oldWorld_R_base,oldWorld_p_base,varargin{2},varargin{1}); - end - - [oldWorld_p_referenceLink,oldWorld_R_referenceLink] = frame2posrot(oldWorld_qH_referenceLink); - - oldWorld_H_referenceLink = [oldWorld_R_referenceLink oldWorld_p_referenceLink; zeros(1,3) 1]; - newWorld_H_base = oldWorld_H_referenceLink \ oldWorld_H_base; - - world_p_base = newWorld_H_base(1:3,4); - world_R_base = newWorld_H_base(1:3,1:3); + +[~,oldWorld_qH_base,~,~] = wbm_getState(); +[oldWorld_p_base,oldWorld_R_base] = frame2posrot(oldWorld_qH_base); +oldWorld_H_base = [oldWorld_R_base oldWorld_p_base; zeros(1,3) 1]; + + +if(nargin == 1) + [oldWorld_qH_referenceLink] = wbm_forwardKinematics(varargin{1}); +else + [oldWorld_qH_referenceLink] = wbm_forwardKinematics(oldWorld_R_base,oldWorld_p_base,varargin{2},varargin{1}); +end + +[oldWorld_p_referenceLink,oldWorld_R_referenceLink] = frame2posrot(oldWorld_qH_referenceLink); + +oldWorld_H_referenceLink = [oldWorld_R_referenceLink oldWorld_p_referenceLink; zeros(1,3) 1]; +newWorld_H_base = oldWorld_H_referenceLink \ oldWorld_H_base; + +world_p_base = newWorld_H_base(1:3,4); +world_R_base = newWorld_H_base(1:3,1:3); end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_jacobian.m b/mex-wholebodymodel/matlab/wrappers/wbm_jacobian.m index 7a898fc..1493414 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_jacobian.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_jacobian.m @@ -2,10 +2,10 @@ %WMB_JACOBIAN computes the Jacobian to a desired link (frame) at a given %joint configuration %velocity vxb -% Arguments : +% Arguments : % Optimised Mode : link_name - string matching URDF name of the link (frame) % Normal Mode : R - rotation from rootLink to world frame (3 x 3) -% p - translation from rootLink to world frame (3 x 1) +% p - translation from rootLink to world frame (3 x 1) % qj - joint position (NumDoF x 1) % link_name - string matching URDF name of the link (frame) % @@ -14,12 +14,12 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 1 - J = mexWholeBodyModel('jacobian',varargin{1}); - case 4 - J = mexWholeBodyModel('jacobian',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4}); - otherwise - disp('jacobian : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 1 + J = mexWholeBodyModel('jacobian',varargin{1}); + case 4 + J = mexWholeBodyModel('jacobian',reshape(varargin{1},[],1), varargin{2},varargin{3},varargin{4}); + otherwise + disp('jacobian : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_jointLimits.m b/mex-wholebodymodel/matlab/wrappers/wbm_jointLimits.m index e94bee9..a16bc66 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_jointLimits.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_jointLimits.m @@ -1,7 +1,7 @@ function [jl_lower,jl_upper] = wbm_jointLimits( varargin ) %WBM_JOINTLIMITS Obtains the joint limits for the chosen robot % -% Arguments : +% Arguments : % Optimised Mode : No arguments % Normal Mode : No arguments % Returns : jl_lower - lower joint limits (NumDoF x 1) @@ -10,11 +10,11 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - [jl_lower,jl_upper] = mexWholeBodyModel('joint-limits'); - otherwise - disp('jointLimits : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + [jl_lower,jl_upper] = mexWholeBodyModel('joint-limits'); + otherwise + disp('jointLimits : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_massMatrix.m b/mex-wholebodymodel/matlab/wrappers/wbm_massMatrix.m index ab8e07b..038d4e2 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_massMatrix.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_massMatrix.m @@ -2,7 +2,7 @@ %WMB_MASSMATRIX computes the mass matrix of the floating base system at a %given joint configuration % -% Arguments : +% Arguments : % Optimised Mode : No Arguments % Normal Mode : R - rotation from rootLink to world frame (3 x 3) % p - translation from rootLink to world frame (3 x 1) @@ -13,12 +13,12 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - M = mexWholeBodyModel('mass-matrix'); - case 3 - M = mexWholeBodyModel('mass-matrix',reshape(varargin{1},[],1), varargin{2}, varargin{3}); - otherwise - disp('massMatrix : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + M = mexWholeBodyModel('mass-matrix'); + case 3 + M = mexWholeBodyModel('mass-matrix',reshape(varargin{1},[],1), varargin{2}, varargin{3}); + otherwise + disp('massMatrix : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialise.m b/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialise.m index 6391254..9b01fd8 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialise.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialise.m @@ -1,6 +1,6 @@ function [ ] = wbm_modelInitialise( varargin ) %WMB_MODELINITIALISE initialises the whole body model -% Arguments : +% Arguments : % Optimised Mode : No arguments % Normal Mode : robot-name as a string (URDF must exist in the whole body % interface folder) @@ -9,12 +9,12 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 0 - mexWholeBodyModel('model-initialise'); - case 1 - mexWholeBodyModel('model-initialise',varargin{1}); - otherwise - disp('modelInitialise : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 0 + mexWholeBodyModel('model-initialise'); + case 1 + mexWholeBodyModel('model-initialise',varargin{1}); + otherwise + disp('modelInitialise : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialiseFromURDF.m b/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialiseFromURDF.m index 1a0f563..792ae8a 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialiseFromURDF.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_modelInitialiseFromURDF.m @@ -7,10 +7,10 @@ % Author : Silvio Traversaro (silvio.traversaro@iit.it) % Genova, Nov 2015 - switch(nargin) - case 1 - mexWholeBodyModel('model-initialise-urdf',varargin{1}); - otherwise - disp('modelInitialiseFromURDF : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 1 + mexWholeBodyModel('model-initialise-urdf',varargin{1}); + otherwise + disp('modelInitialiseFromURDF : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_setWorldFrame.m b/mex-wholebodymodel/matlab/wrappers/wbm_setWorldFrame.m index c6c85a1..a9d8d02 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_setWorldFrame.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_setWorldFrame.m @@ -4,21 +4,21 @@ %rototranslation from the root link to the world can then be computed to %aid optimised computation of all other dynamics components % -% Arguments : +% Arguments : % Optimised Mode : Does not exist % Normal Mode : R - rotation from rootLink to world frame (3 x 3) -% p - translation from rootLink to world frame (3 x 1) +% p - translation from rootLink to world frame (3 x 1) % g - gravity vector in world frame % Returns : None % % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 3 - mexWholeBodyModel('set-world-frame',reshape(varargin{1},[],1), varargin{2}, varargin{3}); - otherwise - disp('setWorldFrame : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 3 + mexWholeBodyModel('set-world-frame',reshape(varargin{1},[],1), varargin{2}, varargin{3}); + otherwise + disp('setWorldFrame : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_setWorldLink.m b/mex-wholebodymodel/matlab/wrappers/wbm_setWorldLink.m index c4efbfe..3b1cb4c 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_setWorldLink.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_setWorldLink.m @@ -4,26 +4,26 @@ %rototranslation from the root link to the world can then be computed to %aid optimised computation of all other dynamics components % -% Arguments : +% Arguments : % Optimised Mode : R - rotation from reference link to world frame (3 x 3) -% p - translation from reference link to world frame (3 x 1) +% p - translation from reference link to world frame (3 x 1) % (link name used is the previously set or default) % Normal Mode : link_name - string matching URDF name of the link (frame) % R - rotation from reference link to world frame (3 x 3) -% p - translation from reference link to world frame (3 x 1) +% p - translation from reference link to world frame (3 x 1) % g - gravity vector in the world frame (3 x 1) % Returns : None % % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 2 - mexWholeBodyModel('set-world-link',reshape(varargin{1},[],1), varargin{2}); - case 4 - mexWholeBodyModel('set-world-link',varargin{1}, reshape(varargin{2},[],1), varargin{3}, varargin{4}); - otherwise - disp('setWorldLink : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 2 + mexWholeBodyModel('set-world-link',reshape(varargin{1},[],1), varargin{2}); + case 4 + mexWholeBodyModel('set-world-link',varargin{1}, reshape(varargin{2},[],1), varargin{3}, varargin{4}); + otherwise + disp('setWorldLink : Incorrect number of arguments, check docs'); +end end diff --git a/mex-wholebodymodel/matlab/wrappers/wbm_updateState.m b/mex-wholebodymodel/matlab/wrappers/wbm_updateState.m index 3f957ed..b657f94 100644 --- a/mex-wholebodymodel/matlab/wrappers/wbm_updateState.m +++ b/mex-wholebodymodel/matlab/wrappers/wbm_updateState.m @@ -2,7 +2,7 @@ %WBM_UPDATESTATE Updates the robot state, i.e. joint angles, joint %velocities, floating base velocity. % -% Arguments : +% Arguments : % Optimised Mode : Does not exist % Normal Mode : qj - joint angles (numDoF x 1) % qjDot - joint velocity (numDoF x 1) @@ -12,11 +12,11 @@ % Author : Naveen Kuppuswamy (naveen.kuppuswamy@iit.it) % Genova, Dec 2014 - switch(nargin) - case 3 - mexWholeBodyModel('update-state',varargin{1}, varargin{2}, varargin{3}); - otherwise - disp('updateState : Incorrect number of arguments, check docs'); - end +switch(nargin) + case 3 + mexWholeBodyModel('update-state',varargin{1}, varargin{2}, varargin{3}); + otherwise + disp('updateState : Incorrect number of arguments, check docs'); +end end diff --git a/tests/WBMAssertEqual.m b/tests/WBMAssertEqual.m index 3a296f8..c9f2f03 100644 --- a/tests/WBMAssertEqual.m +++ b/tests/WBMAssertEqual.m @@ -3,15 +3,15 @@ % don't do anything if val1 == val2, print the error message % and exit otherwise % - if( nargin < 3 ) - errorMsg = ''; - end - if( nargin < 4 ) - tol = 1e-5; - end - if( norm(val1-val2) > tol ) - disp([ 'WBM Tests: ' errorMsg]) - disp([ 'mismatch between ' mat2str(val1) ' and ' mat2str(val2)]) - assert(norm(val1-val2) <= tol) - end +if( nargin < 3 ) + errorMsg = ''; +end +if( nargin < 4 ) + tol = 1e-5; +end +if( norm(val1-val2) > tol ) + disp([ 'WBM Tests: ' errorMsg]) + disp([ 'mismatch between ' mat2str(val1) ' and ' mat2str(val2)]) + assert(norm(val1-val2) <= tol) +end end diff --git a/tests/baseLinkFrameTest.m b/tests/baseLinkFrameTest.m index 469bd5e..673a982 100644 --- a/tests/baseLinkFrameTest.m +++ b/tests/baseLinkFrameTest.m @@ -4,7 +4,6 @@ %% initialise mexWholeBodyModel wbm_modelInitialise('icubGazeboSim'); - %% setup params params.ndof = 25; param.dampingCoeff = 0.5; @@ -20,7 +19,7 @@ params.rightArmInit = [ -19.7 29.7 0.0 44.9 0.0]';%params.rightArmInit = zeros(size(params.rightArmInit)); params.leftLegInit = [ 25.5 0.1 0.0 -38.5 -5.5 -0.1]';%params.leftLegInit = zeros(size(params.leftLegInit)); params.rightLegInit = [ 25.5 0.1 0.0 -38.5 -5.5 -0.1]';%params.rightLegInit = zeros(size(params.rightLegInit)); - + params.qjInit = [params.torsoInit;params.leftArmInit;params.rightArmInit;params.leftLegInit;params.rightLegInit] * (pi/180); else % random pose within joint limits @@ -42,14 +41,11 @@ fprintf('Prior rotation \n'); disp(rot); - fprintf('Prior rotation check (R^T*R)\n'); disp(rot'*rot); - wbm_updateState(params.qjInit,zeros(params.ndof,1),zeros(6,1)); - [qj,T_b_Got,dqj,vb] = wbm_getState(); [posGot,rotGot] = frame2posrot(T_b_Got); diff --git a/tests/forwardDynamics_kinEnergyTest.m b/tests/forwardDynamics_kinEnergyTest.m index b727317..40bf542 100644 --- a/tests/forwardDynamics_kinEnergyTest.m +++ b/tests/forwardDynamics_kinEnergyTest.m @@ -44,7 +44,6 @@ %reconstructing rotation of root to world from the quaternion %[~,T_b,~,~] = wholeBodyModel('get-state'); - w_R_b = quaternion2dcm(qt_b); wbm_setWorldFrame(w_R_b,x_b,[0 0 0]'); @@ -74,7 +73,6 @@ %% control torque tau = param.tau(t); - %% Contact forces computation JcMinv = Jc/M; JcMinvJct = JcMinv * Jc'; diff --git a/tests/kinEnergyConservationTest.m b/tests/kinEnergyConservationTest.m index f8d62fa..c244d08 100644 --- a/tests/kinEnergyConservationTest.m +++ b/tests/kinEnergyConservationTest.m @@ -1,71 +1,70 @@ function [] = kinEnergyConservationTest( params ) - rng(0) +rng(0) +clear wholeBodyModel; - clear wholeBodyModel; +%% initialise mexWholeBodyModel +if( params.isURDF ) + wbm_modelInitialiseFromURDF(params.urdfFilePath); + robotDisplayName = params.urdfFilePath; +else + wbm_modelInitialise(params.yarpRobotName); + robotDisplayName = params.yarpRobotName; +end - %% initialise mexWholeBodyModel - if( params.isURDF ) - wbm_modelInitialiseFromURDF(params.urdfFilePath); - robotDisplayName = params.urdfFilePath; - else - wbm_modelInitialise(params.yarpRobotName); - robotDisplayName = params.yarpRobotName; - end +%% get limits +[jlMin,jlMax] = wbm_jointLimits(); - %% get limits - [jlMin,jlMax] = wbm_jointLimits(); +%% setup params +params.ndof = size(jlMin,1); - %% setup params - params.ndof = size(jlMin,1); +%% random initial conditions inside - %% random initial conditions inside +deltaJl = jlMax - jlMin; +qjInit = jlMin + rand(params.ndof,1) .* deltaJl ; +maxVel = 10; - deltaJl = jlMax - jlMin; - qjInit=jlMin + rand(params.ndof,1) .* deltaJl ; - maxVel = 10; +dqjInit = maxVel*(0.5*ones(params.ndof,1)-1*rand(params.ndof,1)); - dqjInit =maxVel*(0.5*ones(params.ndof,1)-1*rand(params.ndof,1)); +params.qjInit = qjInit; +params.dqjInit = dqjInit; - params.qjInit = qjInit; - params.dqjInit = dqjInit; +params.dx_bInit = 2.5*rand(3,1) - 5*ones(3,1); %zeros(3,1); +params.omega_bInit = 2.5*rand(3,1) - 5*ones(3,1);%zeros(3,1); +params.dampingCoeff = 0; - params.dx_bInit = 2.5*rand(3,1) - 5*ones(3,1); %zeros(3,1); - params.omega_bInit = 2.5*rand(3,1) - 5*ones(3,1);%zeros(3,1); - params.dampingCoeff = 0; +fprintf('fwdDynKinEnergyTest: Random Initial configuration\n'); +disp(params.qjInit'); +fprintf('fwdDynKinEnergyTest: Random Initial velocity\n'); +disp(params.dqjInit'); - fprintf('fwdDynKinEnergyTest: Random Initial configuration\n'); - disp(params.qjInit'); - fprintf('fwdDynKinEnergyTest: Random Initial velocity\n'); - disp(params.dqjInit'); +wbm_setWorldFrame(eye(3),[0 0 0]',[0 0 0]'); +wbm_updateState(params.qjInit,params.dqjInit,[params.dx_bInit;params.omega_bInit]); - wbm_setWorldFrame(eye(3),[0 0 0]',[0 0 0]'); - wbm_updateState(params.qjInit,params.dqjInit,[params.dx_bInit;params.omega_bInit]); +[qj,T_bInit,dqj,vb] = wbm_getState(); +[Ptemp,Rtemp] = frame2posrot(T_bInit); +params.chiInit = [T_bInit;params.qjInit;... + params.dx_bInit;params.omega_bInit;params.dqjInit]; - [qj,T_bInit,dqj,vb] = wbm_getState(); - [Ptemp,Rtemp] = frame2posrot(T_bInit); - params.chiInit = [T_bInit;params.qjInit;... - params.dx_bInit;params.omega_bInit;params.dqjInit]; +%% contact constraints (no constraint, free floating system) +params.constraintLinkNames = {}; - %% contact constraints (no constraint, free floating system) - params.constraintLinkNames = {}; +%% no control torques: zero input torques +params.tau = @(t)zeros(params.ndof,1); - %% no control torques: zero input torques - params.tau = @(t)zeros(params.ndof,1); +%% setup integration +forwardDynFunc = @(t,chi)forwardDynamics_kinEnergyTest(t,chi,params); +tStart = 0; +tEnd = params.simulationLengthInSecs; - %% setup integration - forwardDynFunc = @(t,chi)forwardDynamics_kinEnergyTest(t,chi,params); - tStart = 0; - tEnd = params.simulationLengthInSecs; +%% integrate forward dynamics +disp('starting integration'); +options = odeset('RelTol',1e-5,'AbsTol',1e-7); +[t,chi] = ode15s(forwardDynFunc,[tStart tEnd],params.chiInit,options); - %% integrate forward dynamics - disp('starting integration'); - options = odeset('RelTol',1e-5,'AbsTol',1e-7); - [t,chi] = ode15s(forwardDynFunc,[tStart tEnd],params.chiInit,options); +%% plot results +ndof = params.ndof; - %% plot results - ndof = params.ndof; - params.demux.baseOrientationType = 1; % sets the base orientation in stateDemux.m as positions + quaternions (1) or transformation matrix (0) [basePose,qj,baseVelocity,dqj] = stateDemux(chi,params); @@ -80,100 +79,100 @@ % linear and angular velocity dx_b = baseVelocity(1:3,:); omega_W = baseVelocity(4:6,:); - - x = [x_b' qt_b' qj']; - v = [dx_b' omega_W' dqj' ]; - kinEnergy = zeros(length(t),1); - chiDot = zeros(length(t),size(chi,2)); - hOut = zeros(length(t),ndof+6); - gOut = zeros(length(t),ndof+6); - - fc = zeros(length(t),ndof+6); - wbm_setWorldFrame(eye(3),[0 0 0]', [0 0 0]'); - - for tCnt = 1:length(t) - [chiDot(tCnt,:),hOut(tCnt,:),gOut(tCnt,:),~,kinEnergy(tCnt) ] = forwardDynamics_kinEnergyTest(t(tCnt,:),chi(tCnt,:)',params); - end - - if( params.plot ) - figure - plot(t,kinEnergy,'b'); - hold on; - %plot(t,kinEnergy2,'r'); - xlabel('Time t(sec)'); - ylabel(' (J)'); - title(['Kinetic Energy for ',robotDisplayName]); - end - - kinEnergyMaxErrRel = max(abs(kinEnergy-kinEnergy(1)))/kinEnergy(1); - - fprintf('Relative error for energy for model %s: %f\n',robotDisplayName,kinEnergyMaxErrRel); - - if( params.raiseErrorOnFail ) - WBMAssertEqual(kinEnergyMaxErrRel,0,'Kinetic energy is not constant',params.relTol); - end + +x = [x_b' qt_b' qj']; +v = [dx_b' omega_W' dqj' ]; +kinEnergy = zeros(length(t),1); +chiDot = zeros(length(t),size(chi,2)); +hOut = zeros(length(t),ndof+6); +gOut = zeros(length(t),ndof+6); + +fc = zeros(length(t),ndof+6); +wbm_setWorldFrame(eye(3),[0 0 0]', [0 0 0]'); + +for tCnt = 1:length(t) + [chiDot(tCnt,:),hOut(tCnt,:),gOut(tCnt,:),~,kinEnergy(tCnt) ] = forwardDynamics_kinEnergyTest(t(tCnt,:),chi(tCnt,:)',params); +end + +if( params.plot ) + figure + plot(t,kinEnergy,'b'); + hold on; + %plot(t,kinEnergy2,'r'); + xlabel('Time t(sec)'); + ylabel(' (J)'); + title(['Kinetic Energy for ',robotDisplayName]); +end + +kinEnergyMaxErrRel = max(abs(kinEnergy-kinEnergy(1)))/kinEnergy(1); + +fprintf('Relative error for energy for model %s: %f\n',robotDisplayName,kinEnergyMaxErrRel); + +if( params.raiseErrorOnFail ) + WBMAssertEqual(kinEnergyMaxErrRel,0,'Kinetic energy is not constant',params.relTol); +end % figure; % plot(t,dqj); % xlabel('Time t(sec)'); % ylabel('dqj (rad/sec)'); % title('Joint velocities '); -% +% % figure; % plot(t,dx_b); % xlabel('Time t(sec)'); % ylabel('(m/sec)'); % title('Floating Base translation velocities'); -% -% +% +% % figure; % plot(t,omega_W); % xlabel('Time t(sec)'); % ylabel(' (m/sec)'); % title('Floating Base rotational velocities'); -% +% % ddx_b = chiDot(:,1:3); % domega_b = chiDot(:,4:6); % ddqj = chiDot(:,7:7+ndof); -% +% % figure; % plot(t,ddqj); % xlabel('Time t(sec)'); % ylabel('(rad/sec^2)'); % title('Joint accelerations '); -% +% % figure; % plot(t,ddx_b); % xlabel('Time t(sec)'); % ylabel('(m/sec^2)'); % title(' Floating Base translation acceleration '); -% +% % figure; % plot(t,domega_b); % xlabel('Time t(sec)'); % ylabel('(m/sec^2)'); % title('Floating Base angular acceleration'); -% +% % figure; % plot(t,gOut); % xlabel('Time t(sec)'); % ylabel('mixed units'); % title('g'); -% +% % figure; % plot(t,hOut); % xlabel('Time t(sec)'); % ylabel('mixed units'); % title('h'); -% +% % figure(1); -% +% % figure; % plot(t,fc); % xlabel('Time t(sec)'); % ylabel('mixed units'); % title('fc'); -% +% % plot3(x_b(:,1),x_b(:,2),x_b(:,3));hold on; % plot3(x_b(1,1),x_b(1,2),x_b(1,3),'ro'); % grid on;