diff --git a/startup_Mex.m.in b/.startup_mexWholeBodyModel.m.in similarity index 88% rename from startup_Mex.m.in rename to .startup_mexWholeBodyModel.m.in index 4ca9963..19e4c36 100644 --- a/startup_Mex.m.in +++ b/.startup_mexWholeBodyModel.m.in @@ -1,10 +1,10 @@ -%% startup_Mex.m +%% 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_BINARY_DIR@/install'; +installDir = '@CMAKE_INSTALL_PREFIX@'; mexDir = [installDir, filesep, 'mex']; mexWrapDir = [mexDir, filesep, 'mexwbi-wrappers']; mexUtDir = [mexDir, filesep, 'mexwbi-utilities']; @@ -38,7 +38,7 @@ 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 Mex-Toolbox to ', ... + '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'); diff --git a/CMakeLists.txt b/CMakeLists.txt index e6abf08..b58246e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,11 +59,11 @@ enable_testing() add_subdirectory(tests) -#STARTUP MEX-TOOLBOX -# The following line is to properly configure the installation script of the Mex-toolbox -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/startup_Mex.m.in ${CMAKE_BINARY_DIR}/startup_Mex.m) +#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_Mex.m DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) +install(FILES ${CMAKE_BINARY_DIR}/startup_mexWholeBodyModel.m DESTINATION mex) diff --git a/README.md b/README.md index 3f6e8ac..1a4879f 100644 --- a/README.md +++ b/README.md @@ -5,8 +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 this purpose, run only once the script `startupMex.m` in -`${CODYCO_SUPERBUILD_ROOT}/main/mexWholeBodyModel`. This should be enough to premanently add the required directories to your MATLAB path. +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 diff --git a/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m b/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m index 8fb9243..0fdff5a 100644 --- a/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m +++ b/controllers/experiments/torqueBalancingGainTuning/initializeTorqueBalancing.m @@ -57,19 +57,19 @@ % 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 +CONFIG.linearizationDebug = 1; %either 0 or 1 % enter in debug mode if CONFIG.linearizationDebug == 1 - CONFIG.demo_movements = 0; - CONFIG.use_QPsolver = 0; + 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 +CONFIG.integrateWithFixedStep = 0; %either 0 or 1 % The fixed step integration needs a desingularization of system mass matrix % to converge to a solution diff --git a/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m b/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m index ac5362f..8a753bd 100644 --- a/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m +++ b/controllers/experiments/torqueBalancingGainTuning/src/forwardDynamics.m @@ -70,7 +70,7 @@ 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); + -CONFIG.linearization.KD*(dqj-trajectory.JointReferences.dqjRef); end %% Balancing controller diff --git a/controllers/experiments/torqueBalancingGainTuning/src/runController.m b/controllers/experiments/torqueBalancingGainTuning/src/runController.m index e399a51..3ab0556 100644 --- a/controllers/experiments/torqueBalancingGainTuning/src/runController.m +++ b/controllers/experiments/torqueBalancingGainTuning/src/runController.m @@ -23,7 +23,7 @@ use_QPsolver = CONFIG.use_QPsolver; initForKinematics = CONFIG.initForKinematics; S = [zeros(6,ndof); - eye(ndof,ndof)]; + eye(ndof,ndof)]; %% Dynamics dJcNu = DYNAMICS.dJcNu; diff --git a/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m b/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m index 754a7b4..4c78553 100644 --- a/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m +++ b/controllers/experiments/torqueBalancingGainTuning/src/stackOfTaskController.m @@ -72,10 +72,10 @@ % General parameters gravAcc = 9.81; S = [zeros(6,ndof); - eye(ndof,ndof)]; + eye(ndof,ndof)]; f_grav = [zeros(2,1); - -m*gravAcc; - zeros(3,1)]; + -m*gravAcc; + zeros(3,1)]; %% Multiplier of contact wrenches at CoM posRFoot = RFootPoseEul(1:3); @@ -83,9 +83,9 @@ 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)]; + skew(distLF),eye(3)]; AR = [eye(3), zeros(3); - skew(distRF),eye(3)]; + skew(distRF),eye(3)]; % One foot or two feet on ground selector if sum(feet_on_ground) == 2 @@ -132,7 +132,7 @@ 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); + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); %% Desired contact forces computation fcHDot = pinvA*(HDotDes - f_grav); diff --git a/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m b/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m index 41fd26d..2a14bf0 100644 --- a/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m +++ b/controllers/experiments/torqueBalancingGainTuning/src/trajectoryGenerator.m @@ -21,10 +21,10 @@ % 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 + %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 + % that the robot waits before starting the left-and-right %% Trajectory definition if demo_movements == 1 diff --git a/controllers/experiments/torqueBalancingJointControl/src/runController.m b/controllers/experiments/torqueBalancingJointControl/src/runController.m index e8bd770..b41579f 100644 --- a/controllers/experiments/torqueBalancingJointControl/src/runController.m +++ b/controllers/experiments/torqueBalancingJointControl/src/runController.m @@ -22,7 +22,7 @@ feet_on_ground = CONFIG.feet_on_ground; initForKinematics = CONFIG.initForKinematics; S = [zeros(6,ndof); - eye(ndof,ndof)]; + eye(ndof,ndof)]; %% Dynamics dJcNu = DYNAMICS.dJcNu; diff --git a/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m b/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m index 41fd26d..2a14bf0 100644 --- a/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m +++ b/controllers/experiments/torqueBalancingJointControl/src/trajectoryGenerator.m @@ -21,10 +21,10 @@ % 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 + %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 + % that the robot waits before starting the left-and-right %% Trajectory definition if demo_movements == 1 diff --git a/controllers/experiments/visualizerNew/visualizeForwardDynamics.m b/controllers/experiments/visualizerNew/visualizeForwardDynamics.m index c3b9615..41b7e81 100644 --- a/controllers/experiments/visualizerNew/visualizeForwardDynamics.m +++ b/controllers/experiments/visualizerNew/visualizeForwardDynamics.m @@ -43,16 +43,12 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) % 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' ; @@ -126,13 +122,10 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)); @@ -196,18 +189,18 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) % 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]; + 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 @@ -228,13 +221,13 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) % 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)]; + 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); @@ -242,14 +235,14 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) %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]; + 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); + 'FaceAlpha',0.2,'FaceColor',robotColor); end %% FEET PATCHES @@ -265,17 +258,17 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)]; + 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); + 'FaceColor',robotColor); % left foot patch jj=n_lin+2; @@ -286,16 +279,16 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)]; + 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); + 'FaceColor',robotColor); %% PLOT THRUSTS F = 0.01; delta = 5*pi/180; @@ -405,9 +398,9 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)); + 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)); + 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),... @@ -467,13 +460,13 @@ function visualizeForwardDynamics(xout,tSpan,config,references,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,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)]; + 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 @@ -494,13 +487,13 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)]; + 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); @@ -518,20 +511,19 @@ function visualizeForwardDynamics(xout,tSpan,config,references,jetsIntensities) 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)]; + 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 diff --git a/controllers/experiments/visualizerNew/visualizer_demo.m b/controllers/experiments/visualizerNew/visualizer_demo.m index 1f34782..519b2f4 100644 --- a/controllers/experiments/visualizerNew/visualizer_demo.m +++ b/controllers/experiments/visualizerNew/visualizer_demo.m @@ -6,7 +6,7 @@ clear all; clc; % load('helicoidalShort.mat'); - % load('helicoidal.mat'); + % load('helicoidal.mat'); fileName = 'helicoidal.mat'; load(fileName); config.fileName = fileName; @@ -33,7 +33,7 @@ BackGroundColor = [0 0 0]; GridColor = [1 1 1]; figure_main = figure('Name', 'iCub Simulator', 'NumberTitle', 'off',... - 'Position', [500,800,1200,650],'Color',BackGroundColor); + 'Position', [500,800,1200,650],'Color',BackGroundColor); %% ADAPT FIGURE DIMENSION DEPENDING ON SCREEN SIZE sizeFig = get(0, 'MonitorPositions'); @@ -42,11 +42,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]; + 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,'.'); diff --git a/controllers/torqueBalancing/src/runController.m b/controllers/torqueBalancing/src/runController.m index ac9b528..0d71527 100644 --- a/controllers/torqueBalancing/src/runController.m +++ b/controllers/torqueBalancing/src/runController.m @@ -23,7 +23,7 @@ use_QPsolver = CONFIG.use_QPsolver; initForKinematics = CONFIG.initForKinematics; S = [zeros(6,ndof); - eye(ndof,ndof)]; + eye(ndof,ndof)]; %% Dynamics dJcNu = DYNAMICS.dJcNu; diff --git a/controllers/torqueBalancing/src/stackOfTaskController.m b/controllers/torqueBalancing/src/stackOfTaskController.m index 58b0c9b..866b1d7 100644 --- a/controllers/torqueBalancing/src/stackOfTaskController.m +++ b/controllers/torqueBalancing/src/stackOfTaskController.m @@ -72,10 +72,10 @@ % General parameters gravAcc = 9.81; S = [zeros(6,ndof); - eye(ndof,ndof)]; + eye(ndof,ndof)]; f_grav = [zeros(2,1); - -m*gravAcc; - zeros(3,1)]; + -m*gravAcc; + zeros(3,1)]; %% Multiplier of contact wrenches at CoM posRFoot = RFootPoseEul(1:3); @@ -83,9 +83,9 @@ 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)]; + skew(distLF),eye(3)]; AR = [eye(3), zeros(3); - skew(distRF),eye(3)]; + skew(distRF),eye(3)]; % One foot or two feet on ground selector if sum(feet_on_ground) == 2 @@ -135,7 +135,7 @@ 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); + + Mbar*ddqjRef - impedances*posturalCorr*qjTilde - dampings*posturalCorr*dqjTilde); %% Desired contact forces computation fcHDot = pinvA*(HDotDes - f_grav); @@ -145,7 +145,7 @@ if sum(feet_on_ground) == 2 - f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*fcHDot); + f0 = -pinv(SigmaNA,pinv_tol)*(tauModel+Sigma*fcHDot); end fcDes = fcHDot + Nullfc*f0; diff --git a/controllers/torqueBalancing/src/trajectoryGenerator.m b/controllers/torqueBalancing/src/trajectoryGenerator.m index 41fd26d..2a14bf0 100644 --- a/controllers/torqueBalancing/src/trajectoryGenerator.m +++ b/controllers/torqueBalancing/src/trajectoryGenerator.m @@ -21,10 +21,10 @@ % 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 + %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 + % that the robot waits before starting the left-and-right %% Trajectory definition if demo_movements == 1 diff --git a/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m index 9a69e00..af368f8 100644 --- a/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m +++ b/controllers/utilityMatlabFunctions/CentroidalTransformation/centroidalTransformationT_TDot.m @@ -6,7 +6,7 @@ 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); @@ -15,23 +15,23 @@ 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; dX = [zeros(3),skew(dr)'; - zeros(3),zeros(3)]; + zeros(3),zeros(3)]; dMb = [zeros(3),skew(mdr)'; - skew(mdr),zeros(3)]; + skew(mdr),zeros(3)]; 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/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m b/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m index 36c1144..bb3e5c2 100644 --- a/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m +++ b/controllers/utilityMatlabFunctions/CentroidalTransformation/fromFloatingToCentroidalDynamics.m @@ -36,8 +36,8 @@ C_cNu_c_dT = invTt*CNu - M_c*dT*Nu; C_cNu_c = [ zeros(3,1); - C_cNu_c_dT(4:6); - CNu_j-(Mbj')*(Mb\CNu_b)]; + 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); diff --git a/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m b/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m index 8e1c11b..0913397 100644 --- a/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m +++ b/controllers/utilityMatlabFunctions/InverseKinematics/initInverseKinematics.m @@ -54,7 +54,7 @@ % 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; + 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 diff --git a/controllers/utilityMatlabFunctions/InverseKinematics/inverseKinematics.m b/controllers/utilityMatlabFunctions/InverseKinematics/inverseKinematics.m index 940c0b4..c451bd8 100644 --- a/controllers/utilityMatlabFunctions/InverseKinematics/inverseKinematics.m +++ b/controllers/utilityMatlabFunctions/InverseKinematics/inverseKinematics.m @@ -112,7 +112,7 @@ % second task: generate a desired Momentum trajectory MomErrorDynamics = -dJHNu +[m*desired_x_dx_ddx_CoM(:,3);zeros(3,1)] -KPosMom*[m*deltaPosCoM; deltaIntAng] ... - -KVelMom*[m*deltaVelCoM; deltaAngMom] -JH*pinv(Jc,pinv_tol)*feetErrorDynamics; + -KVelMom*[m*deltaVelCoM; deltaAngMom] -JH*pinv(Jc,pinv_tol)*feetErrorDynamics; JMomTaks = JH*NullFeet; NullMom = eye(ndof+6) - pinv(JMomTaks,pinv_tol)*JMomTaks; @@ -120,7 +120,7 @@ JPost = [zeros(ndof,6) eye(ndof)]; postErrorDynamics = -KVelPost*Nu(7:end) -KPosPost*(qj-initState.qj)... - -JPost*pinv(Jc,pinv_tol)*feetErrorDynamics -JPost*NullFeet*pinv(JMomTaks,pinv_tol)*MomErrorDynamics; + -JPost*pinv(Jc,pinv_tol)*feetErrorDynamics -JPost*NullFeet*pinv(JMomTaks,pinv_tol)*MomErrorDynamics; JPostTask = JPost*NullFeet*NullMom; % accelerations from inverse kinematics diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/gainsTuning.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/gainsTuning.m index 28f11d6..54ea957 100644 --- a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/gainsTuning.m +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/gainsTuning.m @@ -44,10 +44,10 @@ % new state matrix AStateNew = [zeros(ndof) eye(ndof); - -KSn -KDn]; + -KSn -KDn]; % state matrix verification AStateDes = [zeros(ndof) eye(ndof); - -KSdes -KDdes]; + -KSdes -KDdes]; eigAStateDes = -real(eig(AStateDes)); eigAStateNew = -real(eig(AStateNew)); diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m index cf677f9..9e8b242 100644 --- a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/jointSpaceLinearization.m @@ -73,7 +73,7 @@ r = posFoot - xCoM; A = [eye(3) zeros(3); - skew(r) eye(3) ]; + skew(r) eye(3) ]; pinvA = eye(6)/A; else @@ -81,9 +81,9 @@ Pl = posLFoot - xCoM; AL = [ eye(3), zeros(3); - skew(Pl), eye(3)]; + skew(Pl), eye(3)]; AR = [ eye(3), zeros(3); - skew(Pr), eye(3)]; + skew(Pr), eye(3)]; A = [AL, AR]; pinvA = pinv(A,pinv_tol); end diff --git a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m index 816d0f0..1d7106c 100644 --- a/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m +++ b/controllers/utilityMatlabFunctions/LinearizationAndGainTuning/oneStepTuning.m @@ -1,5 +1,5 @@ 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) + = 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; diff --git a/controllers/utilityMatlabFunctions/Visualization/visualizeInverseKin.m b/controllers/utilityMatlabFunctions/Visualization/visualizeInverseKin.m index 7bc6106..894576a 100644 --- a/controllers/utilityMatlabFunctions/Visualization/visualizeInverseKin.m +++ b/controllers/utilityMatlabFunctions/Visualization/visualizeInverseKin.m @@ -71,9 +71,9 @@ %% Feet errors titFeet = {'Error on left foot pos along x axis','Error on left foot pos along y axis','Error on left foot pos along z axis',... - 'Error on left foot ori along x axis','Error on left foot ori along y axis','Error on left foot ori along z axis',... - 'Error on right foot pos along x axis','Error on right foot pos along y axis','Error on right foot pos along z axis',... - 'Error on right foot ori along x axis','Error on right foot ori along y axis','Error on right foot ori along z axis'}; + 'Error on left foot ori along x axis','Error on left foot ori along y axis','Error on left foot ori along z axis',... + 'Error on right foot pos along x axis','Error on right foot pos along y axis','Error on right foot pos along z axis',... + 'Error on right foot ori along x axis','Error on right foot ori along y axis','Error on right foot ori along z axis'}; titYFeet = {'[m]' '[m]' '[m]' '[rad]' '[rad]' '[rad]'}; diff --git a/controllers/utilityMatlabFunctions/Visualization/visualizeSimulation.m b/controllers/utilityMatlabFunctions/Visualization/visualizeSimulation.m index f241af9..022465a 100644 --- a/controllers/utilityMatlabFunctions/Visualization/visualizeSimulation.m +++ b/controllers/utilityMatlabFunctions/Visualization/visualizeSimulation.m @@ -26,7 +26,7 @@ BackGroundColor = [0 0 0]; GridColor = [1 1 1]; figure_main = figure('Name', 'iCub Simulator', 'NumberTitle', 'off',... - 'Position', [500,800,1200,650],'Color',BackGroundColor); + 'Position', [500,800,1200,650],'Color',BackGroundColor); sizeFig = [10 26 800 600]; @@ -40,7 +40,7 @@ CONFIG.plot_main = zeros(1,4); plot_pos = [0.51,0.05,0.45,1; - 0.01,0.05,0.45,1]; + 0.01,0.05,0.45,1]; for ii=1:2 diff --git a/examples/rigidBodyDynamics.m b/examples/rigidBodyDynamics.m index 31ef156..86cda1b 100644 --- a/examples/rigidBodyDynamics.m +++ b/examples/rigidBodyDynamics.m @@ -17,11 +17,11 @@ %% we set the state to some random values, just to show how to %% get the dynamics quantities w_R_b = eye(3,3); % rotation matrix that transforms a vector in the base frame to the world frame -x_b = [1;2;3]; % position of the link frame origin wrt to the world frame -qj = zeros(0,1); % joint positions -dqj = zeros(0,1); % joint velocities -grav = [0;0;-9.8]; % gravity in world frame -dx_b = [0.4;0.5;0.6]; % derivative in the position of the link frame origin wrt to the world frame +x_b = [1;2;3]; % position of the link frame origin wrt to the world frame +qj = zeros(0,1); % joint positions +dqj = zeros(0,1); % joint velocities +grav = [0;0;-9.8]; % gravity in world frame +dx_b = [0.4;0.5;0.6]; % derivative in the position of the link frame origin wrt to the world frame omega_W = [0.4;0.5;0.2]; % angular velocity of base frame %% Set the state diff --git a/mex-wholebodymodel/matlab/utilities/QPSolver.m b/mex-wholebodymodel/matlab/utilities/QPSolver.m index 4ddaa59..40d86ad 100644 --- a/mex-wholebodymodel/matlab/utilities/QPSolver.m +++ b/mex-wholebodymodel/matlab/utilities/QPSolver.m @@ -47,7 +47,7 @@ forceFrictionCoefficient = 1; torsionalFrictionCoefficient = 2/150; footSize = [-0.1 0.1; % xMin, xMax - -0.1 0.1]; % yMin, yMax + -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 @@ -78,7 +78,7 @@ gradientQP1Foot = -A'*(HDotDes-f_grav); [fcDes, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP1Foot, gradientQP1Foot, ConstraintsMatrixQP1Foot,... - [], [], [], bVectorConstraintsQP1Foot); + [], [], [], bVectorConstraintsQP1Foot); elseif sum(feet_on_ground) == 2 @@ -88,7 +88,7 @@ gradientQP2Feet = SigmaNA'*(tauModel + Sigma*fcHDot); [f0, ~, exitFlag, iter, lambda, auxOutput] = qpOASES(HessianMatrixQP2Feet, gradientQP2Feet, ConstraintsMatrixQP2Feet,... - [], [], [], bVectorConstraintsQp2Feet); + [], [], [], bVectorConstraintsQp2Feet); fcDes = fcHDot + Nullfc*f0; end diff --git a/mex-wholebodymodel/matlab/utilities/frictionCones.m b/mex-wholebodymodel/matlab/utilities/frictionCones.m index 5981f06..a71af4c 100644 --- a/mex-wholebodymodel/matlab/utilities/frictionCones.m +++ b/mex-wholebodymodel/matlab/utilities/frictionCones.m @@ -43,15 +43,15 @@ end 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(2,2), 1, 0, 0]; + 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(2,2), 1, 0, 0]; CFoot = [ Aineq ; - CFoot ]; + CFoot ]; dConstraints = [zeros(size(Aineq,1), 1); zeros(7,1)]; diff --git a/mex-wholebodymodel/matlab/utilities/parametrization.m b/mex-wholebodymodel/matlab/utilities/parametrization.m index 536f843..3c8f806 100644 --- a/mex-wholebodymodel/matlab/utilities/parametrization.m +++ b/mex-wholebodymodel/matlab/utilities/parametrization.m @@ -19,7 +19,7 @@ %angles T_bar = [1 0 -sin(theta); - 0 cos(phi) sin(phi)*cos(theta) ; - 0 -sin(phi) cos(phi)*cos(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/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/visualizeForwardDynamics.m b/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m index 23e3d15..b99ffe0 100644 --- a/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m +++ b/mex-wholebodymodel/matlab/utilities/visualizeForwardDynamics.m @@ -172,18 +172,18 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) % 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]; + 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 the lines depicting the links for jj=1:n_lin @@ -204,24 +204,24 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) % 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)]; + 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)]; 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]; + 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); + 'FaceAlpha',0.2,'FaceColor',COLOR); end @@ -238,17 +238,17 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 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)]; + 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',COLOR); + 'FaceColor',COLOR); % left foot patch jj = n_lin+2; @@ -263,16 +263,16 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 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)]; + 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',COLOR); + 'FaceColor',COLOR); %% PLOT THRUSTS if params.plotThusts @@ -430,13 +430,13 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 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)]; + 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); @@ -458,13 +458,13 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 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)]; + 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); @@ -483,13 +483,13 @@ function visualizeForwardDynamics(xout,params,jetsIntensities) 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)]; + 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); diff --git a/mex-wholebodymodel/matlab/utilities/wpinv.m b/mex-wholebodymodel/matlab/utilities/wpinv.m deleted file mode 100644 index df35e45..0000000 --- a/mex-wholebodymodel/matlab/utilities/wpinv.m +++ /dev/null @@ -1,23 +0,0 @@ -function [G,F] = wpinv(A,W) -% WPINV - Compute weighted pseudoinverse. -% -% [G,F] = wpinv(A,W) -% -% Computes the optimal solution x = Gy + Fx0 to the least squares -% problem -% -% min ||W(x-x0)|| subj. to Ax = y -% x -% -% See also PINV. - -[~,n] = size(A); - -if nargin < 2 - W = eye(n); -end - -% Thesis, Lemma B.1 -G = W\pinvDamped(A/W,1e-4); -F = eye(n) - G*A; -end \ No newline at end of file diff --git a/tests/baseLinkFrameTest.m b/tests/baseLinkFrameTest.m index 1765e32..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; @@ -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 c1d4f97..c244d08 100644 --- a/tests/kinEnergyConservationTest.m +++ b/tests/kinEnergyConservationTest.m @@ -1,6 +1,5 @@ function [] = kinEnergyConservationTest( params ) rng(0) - clear wholeBodyModel; %% initialise mexWholeBodyModel @@ -21,10 +20,10 @@ %% random initial conditions inside deltaJl = jlMax - jlMin; -qjInit=jlMin + rand(params.ndof,1) .* deltaJl ; -maxVel = 10; +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; @@ -45,7 +44,7 @@ [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]; + params.dx_bInit;params.omega_bInit;params.dqjInit]; %% contact constraints (no constraint, free floating system) params.constraintLinkNames = {};