added example with simpleRobot

+dynasors/interpolateData.m

@@ -1,37 +1,72 @@
-function [w_H_b_dec, jointPos_dec, time_dec] = interpolateData(w_H_b, jointPos, time, tStep)
+function varargout = interpolateData(varargin)
 
-    % interpolate and decimate data so that they can be visualized real
+    % Interpolate and decimate data so that they can be visualized real
     % time with the Visualizer class. JointPos is a matrix of size
     % ndofxlength(time), while w_H_b is of size 4x4xlength(time).
+    %
+    % Floating base:
+    %
+    %  [w_H_b_dec, jointPos_dec, time_dec] = interpolateData(w_H_b, jointPos, time, tStep)
+    %
+    % Fixed base:
+    %
+    %  [jointPos_dec, time_dec] = interpolateData(jointPos, time, tStep)
+    %
+    switch nargin
+        case 4
+            w_H_b    = varargin{1};
+            jointPos = varargin{2};
+            time     = varargin{3};
+            tStep    = varargin{4};
+        case 3
+            jointPos = varargin{1};
+            time     = varargin{2};
+            tStep    = varargin{3};
+        otherwise
+            error('[interpolateData]: wrong number of inputs!')
+    end
+
     time_dec = time(1):tStep:time(end);
 
-    % it is necessary to convert w_H_b in position + rpy  before proceeding
-    % with interpolation
-    pos = zeros(3, length(time));
-    rpy = zeros(3, length(time));
+    if nargin == 4
+        % it is necessary to convert w_H_b in position + rpy  before
+        % proceeding with the interpolation
+        pos = zeros(3, length(time));
+        rpy = zeros(3, length(time));
 
-    for k = 1:length(time)
+        for k = 1:length(time)
 
-        w_H_b_k   = w_H_b(:,:,k);
-        pos(:, k) = w_H_b_k(1:3, 4);
-        rpy(:, k) = dynasors.rollPitchYawFromRotation(w_H_b_k(1:3, 1:3));
+            w_H_b_k   = w_H_b(:,:,k);
+            pos(:, k) = w_H_b_k(1:3, 4);
+            rpy(:, k) = dynasors.rollPitchYawFromRotation(w_H_b_k(1:3, 1:3));
+        end
+        state = [pos; rpy; jointPos];
+    else
+        state = jointPos;
     end
 
-    state_rpy = [pos; rpy; jointPos];
-
     % interpolate the state
-    state_dec = interp1(time, transpose(state_rpy), time_dec);
+    state_dec = interp1(time, transpose(state), time_dec);
     state_dec = transpose(state_dec);
 
     % now that signals are decimated, we regenerate w_H_b and jointPos
-    basePosRpy   = state_dec(1:6, :);
-    jointPos_dec = state_dec(7:end, :);
-    w_H_b_dec    = zeros(4, 4, length(time_dec));
+    if nargin == 4
+        basePosRpy   = state_dec(1:6, :);
+        jointPos_dec = state_dec(7:end, :);
+        w_H_b_dec    = zeros(4, 4, length(time_dec));
 
-    for k = 1:length(time_dec)
+        for k = 1:length(time_dec)
 
-        w_H_b_dec(1:3, 4, k) = basePosRpy(1:3, k);
-        w_H_b_dec(1:3, 1:3, k) = dynasors.rotationFromRollPitchYaw(basePosRpy(4:6, k));
-        w_H_b_dec(4, 1:4, k) = [0 0 0 1];
+            w_H_b_dec(1:3, 4, k) = basePosRpy(1:3, k);
+            w_H_b_dec(1:3, 1:3, k) = dynasors.rotationFromRollPitchYaw(basePosRpy(4:6, k));
+            w_H_b_dec(4, 1:4, k) = [0 0 0 1];
+        end
+        varargout{1} = w_H_b_dec;
+        varargout{2} = jointPos_dec;
+        varargout{3} = time_dec;
+    else
+        jointPos_dec = state_dec;
+        varargout{1} = jointPos_dec;
+        varargout{2} = time_dec;
     end
 end

examples/simpleRobot/init/init_control.m

@@ -0,0 +1,38 @@
+% INIT_CONTROL control-related settings and parameters.
+%
+% Author: Gabriele Nava ([email protected])
+% Genova: Mar. 2024
+%
+
+%% Controller gains
+
+% joint space gains
+Config.gains.KP = diag([15; 15]);
+Config.gains.KD = 2*sqrt(Config.gains.KP);
+
+%% Weights QP tasks
+
+% cost function
+Config.weightsQP.joints  = 1;
+Config.weightsQP.torques = 0.01;
+
+% Hessian matrix regularization
+Config.tasksQP.reg_HessianQP = 1e-4;
+
+% QP bounds
+Config.inequalitiesQP.maxJointTorque = [20; 20];
+Config.inequalitiesQP.minJointTorque = [-20; -20];
+
+%% Initialize QP problem with OSQP
+
+% initialize problem
+ndof   = Config.ndof;
+var.H  = eye(ndof);
+var.g  = zeros(ndof,1);
+var.A  = eye(ndof);
+var.lb = Config.inequalitiesQP.minJointTorque;
+var.ub = Config.inequalitiesQP.maxJointTorque;
+
+% create the solver object
+Config.opti = dynasors.OptiQP('osqp');
+Config.opti.setup(var);

examples/simpleRobot/init/init_robot.m

@@ -0,0 +1,29 @@
+% INIT_ROBOT robot-related settings and parameters.
+%
+% Author: Gabriele Nava ([email protected])
+% Genova: Mar. 2024
+%
+
+% specify the list of joints that are going to be considered in the reduced model
+Config.model.jointList = {'joint_1','joint_2'};
+
+% select the link that will be used as base link
+Config.model.baseLinkName = 'root_link';
+
+% model name and path
+Config.model.modelName = 'simpleRobot.urdf';
+Config.model.modelPath = '../../../models/';
+DEBUG                  = false;
+
+% set the initial robot position and velocity [deg] and gravity vector
+Config.initCond.jointPos_init = [10; 20]*pi/180;
+Config.ndof                   = length(Config.initCond.jointPos_init);
+Config.initCond.jointVel_init = zeros(Config.ndof,1);
+Config.gravityAcc             = [0;0;-9.81];
+
+% load the reduced model
+KinDynModel = iDynTreeWrappers.loadReducedModel(Config.model.jointList, Config.model.baseLinkName, ...
+    Config.model.modelPath, Config.model.modelName, DEBUG);
+
+% set the initial robot position
+iDynTreeWrappers.setRobotState(KinDynModel, Config.initCond.jointPos_init, Config.initCond.jointVel_init, Config.gravityAcc);

examples/simpleRobot/init/init_simulator.m

@@ -0,0 +1,36 @@
+% INIT_SIMULATOR defines simulation options.
+%
+% Author: Gabriele Nava ([email protected])
+% Genova: Mar. 2024
+%
+
+% integrator class options
+Config.integr_opt.t_init      = 0;
+Config.integr_opt.t_step      = 0.01;
+Config.integr_opt.t_end       = 10;
+Config.integr_opt.max_n_iter  = 1000000;
+Config.integr_opt.solver_type = 'ode15s'; % 'ode23t'; % 'ode45'; % 'euler';
+Config.solver_opt             = odeset('RelTol',1e-4,'AbsTol',1e-4,'Stats','off');
+
+% visualizer class options
+Config.viz_opt.color              = [0.9,0.9,0.9];
+Config.viz_opt.material           = 'metal';
+Config.viz_opt.meshesPath         = '';
+Config.viz_opt.transparency       = 0.7;
+Config.viz_opt.debug              = false;
+Config.viz_opt.view               = [-92.9356 22.4635];
+Config.viz_opt.groundOn           = true;
+Config.viz_opt.groundColor        = [0.5 0.5 0.5];
+Config.viz_opt.groundTransparency = 0.5;
+Config.interpolation.tStep        = 1/30; % data decimation for visualizer
+
+% initialize the Logger class
+Config.logger                     = dynasors.Logger();
+
+% plot options for jointTorques
+Config.plot_options_jointTorques.yLabel   = 'jointTorques';
+Config.plot_options_jointTorques.xLabel   = 'time [s]';
+Config.plot_options_jointTorques.title    = 'Joint Torques';
+Config.plot_options_jointTorques.lineSize =  3;
+Config.plot_options_jointTorques.fontSize =  12;
+Config.plot_options_jointTorques.legend   = {'t_1','t_2'};

examples/simpleRobot/runSimpleRobot.m

@@ -0,0 +1,72 @@
+% Example of usage of DynaSoRS library with simpleRobot.
+%
+% Author: Gabriele Nava ([email protected])
+% Genova, Mar. 2024
+%
+clear variables
+close all
+clc
+
+% add path to local functions
+addpath(genpath('./src'))
+
+% run scripts for initializing robot, simulator and control params.
+run('./init/init_robot.m');
+run('./init/init_simulator.m');
+run('./init/init_control.m');
+
+%-------------------------------------------------------------------------%
+
+% set initial conditions
+init_state     = [Config.initCond.jointPos_init; Config.initCond.jointVel_init];
+u_init         = jointsController(Config.integr_opt.t_init, init_state, KinDynModel, Config);
+
+% numerical integration
+Config.waitbar = waitbar(0,'Integration in progress...');
+int            = dynasors.Integrator(init_state, [], Config.integr_opt, Config.solver_opt);
+
+% set initial control input and fwdyn function
+control_fcn    = @(t, x) jointsController(t, x, KinDynModel, Config);
+int.input_ctrl = u_init;
+int.integr_fcn = @(t,x) forwardDynamicsFixedBase(t, x, int.input_ctrl, KinDynModel, Config);
+
+tic;
+[time, state] = int.solve();
+t_solver      = toc;
+
+disp('Integration done.')
+disp(['Integration time: ', num2str(t_solver), ' [s]'])
+
+if ~isempty(Config.waitbar)
+    close(Config.waitbar);
+end
+
+disp('Press any key...')
+pause();
+
+%-------------------------------------------------------------------------%
+% demux and interpolate the state
+jointPos = state(:,1:2)';
+[jointPos_dec, time_dec] = dynasors.interpolateData(jointPos, time, Config.interpolation.tStep);
+
+% setup visualizer
+disp('Visualizing the robot...')
+viz = dynasors.Visualizer(eye(4), jointPos_dec, time_dec, KinDynModel, Config.viz_opt);
+
+% run visualizer
+tic;
+viz.run();
+t_viz = toc;
+
+disp('Visualization completed.')
+disp(['Visualizer real-time factor: ', num2str(time(end)/t_viz)])
+
+%-------------------------------------------------------------------------%
+% plot data collected with the logger
+Config.plot_options_jointTorques.xValue   = Config.logger.data.time;
+Config.logger.plotData('jointTorques', Config.plot_options_jointTorques);
+
+%-------------------------------------------------------------------------%
+% remove local paths
+rmpath(genpath('../../src'))
+rmpath(genpath('./src'))

examples/simpleRobot/src/forwardDynamicsFixedBase.m

@@ -0,0 +1,44 @@
+function xDot = forwardDynamicsFixedBase(t, x, ~, KinDynModel, Config)
+
+    % FORWARDDYNAMICS computes the forward dynamics of a fixed-based robot.
+    %
+    % System state:    [jointPos, jointVel];
+    % Time derivative: [jointVel, jointAcc];
+    %
+    % Robot dynamics:
+    %
+    %   M*jointAcc + h(jointPos, jointVel) = tau
+    %
+    % with tau = joint torques (control input).
+    %
+    % Author : Gabriele Nava ([email protected])
+    % Genova, Mar. 2024
+
+    %% ------------Initialization----------------
+
+    % update the waitbar
+    if ~isempty(Config.waitbar)
+        waitbar(t/Config.integr_opt.t_end, Config.waitbar);
+    end
+
+    % demux the system state
+    [jointPos, jointVel] = wbc.vectorDemux(x, [Config.ndof, Config.ndof]);
+
+    % update the current system state
+    iDynTreeWrappers.setRobotState(KinDynModel, jointPos, jointVel, Config.gravityAcc);
+
+    % demux the control input
+    jointTorques = jointsController(t, x, KinDynModel, Config);
+
+    % compute the joints acceleration and state derivatives
+    M        = iDynTreeWrappers.getFreeFloatingMassMatrix(KinDynModel);
+    h        = iDynTreeWrappers.generalizedBiasForces(KinDynModel);
+    jointAcc = M(7:end, 7:end)\(jointTorques - h(7:end));
+    xDot     = [jointVel; jointAcc];
+
+    % ------------------------------------------------------------------- %
+
+    % log data from the forward dynamics simulation
+    Config.logger.logData(t, 'time');
+    Config.logger.logData(jointTorques, 'jointTorques');
+end

examples/simpleRobot/src/getJointsControlQPQuantities.m

@@ -0,0 +1,61 @@
+function [Hessian, gradient, lowerBound, upperBound] = getJointsControlQPQuantities(KinDynModel, jointPos, ...
+        jointVel, jointPosRef, Config)
+
+    % ----------------------------------------------------------------------- %
+    % QP tasks:
+    %
+    % 1) desired joints dynamics:
+    %
+    %    jointAccStar = g -KD*jointVel -KP*(jointPos - jointPosRef)
+    %
+    % 2) joint torques regularization:
+    %
+    %    jointTorques = 0
+    % ----------------------------------------------------------------------- %
+    %
+    g = iDynTreeWrappers.generalizedGravityForces(KinDynModel);
+
+    % reference joints dynamics
+    jointAccStar = g(7:end) -Config.gains.KD*jointVel -Config.gains.KP*(jointPos - jointPosRef);
+
+    %% QP TASKS
+
+    % ----------------------------------------------------------------------- %
+    % Tasks to be added to the QP cost function
+
+    % JOINTS DYNAMICS TASK
+    H_joints =  eye(Config.ndof);
+    g_joints = -jointAccStar;
+
+    % add weights
+    H_joints = Config.weightsQP.joints * H_joints;
+    g_joints = Config.weightsQP.joints * g_joints;
+
+    % TORQUES REGULARIZATION TASK
+    H_torques = eye(Config.ndof);
+    g_torques = zeros(Config.ndof, 1);
+
+    % add weights
+    H_torques = Config.weightsQP.torques * H_torques;
+    g_torques = Config.weightsQP.torques * g_torques;
+
+    % ----------------------------------------------------------------------- %
+    % Build the QP cost function (Hessian matrix and gradient)
+    Hessian  = H_joints + H_torques;
+
+    % regularization of the Hessian. Enforce symmetry and positive definiteness
+    Hessian  = 0.5 * (Hessian + transpose(Hessian)) + eye(size(Hessian,1)) * Config.tasksQP.reg_HessianQP;
+
+    % compute the gradient
+    gradient = g_joints + g_torques;
+
+    %% QP BOUNDARIES
+
+    % ----------------------------------------------------------------------- %
+    % QP input boundaries. They are of the form:
+    %
+    % lb <= u <= ub
+    %
+    upperBound = Config.inequalitiesQP.maxJointTorque;
+    lowerBound = Config.inequalitiesQP.minJointTorque;
+end