Add files via upload

InitializeSimulationParameters.m

@@ -0,0 +1,28 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                          INITIALIZE ALL                                 %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Initialize all the neccessary simulation data.
+initializeValvesData
+initializeLinearActuatorsData
+initializeFrictionData
+initializeHosingData
+initializeConstantGeometricData
+initializeConstantPositionVectors
+initializeConstantRotationMatrices
+initializeInertiaProperties
+initializeAuxiliaryQuantities
+pres.supply = 190e5;
+pres.return = 10e5;
+oil.B = 800e6;
+initializeInitialValuesInSimulation
+initializeControllerParameters
+initializeControlModes
+initializeLowPassFilters
+initializeParameterUpdate
+revJoint1.damp = 0.1;
+revJoint2.damp = 0.1;
+gravity.Wg = [0; 9.8066; 0];
+D22F = [0;0;0;0;0;0];
+D22Fr = [0;0;0;0;0;0];
+Bc1_V = [0;0;0;0;0;0];
+Bc1_Vr = [0;0;0;0;0;0];

RUN_ME.m

@@ -0,0 +1,93 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Supplementary material for the open-access publication:                 %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Goran R. Petrović and Jouni Mattila, Mathematical modelling and virtual %
+% decomposition control of heavy-duty parallel–serial hydraulic manipula- %
+% tors, Mechanism and Machine Theory, Volume 170, 2022, 104680, ISSN 0094-%
+% 114X, https://doi.org/10.1016/j.mechmachtheory.2021.104680.             %
+% (https://www.sciencedirect.com/science/article/pii/S0094114X21004092)   %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% The CAD files used were kindly donated by Dr Janne Koivumäki. This fact %
+% and feedback provision are highly appreciated by the simulation author. % 
+% Check Dr Koivumäki's publications at:                                   %            
+% https://scholar.google.com/citations?user=Llrx-nsAAAAJ                  %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% The supplementary material creation process has been supported by prof. %
+% Jouni Mattila and is acknowledged here.                                 %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% The simulation file author: Goran R. Petrović, (goran.petrovic@tuni.fi) %                       
+% January 2023, Tampere University / Tampereen Yliopisto                  %                         
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+clear
+clc
+close all
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Demonstrated operating modes have following numeric identifiers:
+% 0 - Cartesian position control,
+% ANY VALUE - Joint-space control.
+MODE = 1;
+% Choose whether the friction force should be exerted to linear actuator
+% pistons on simulation. (No reason not to.)
+settings.actuatorFrictionON = 1;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Controller feedback gains and sample time.                              %           
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% controller sample time
+controller.dT = 0.001;
+% feedback gains in revolute segment 1 
+controller.kx1 = 5e-3;
+controller.kf1 = 2e-8;
+% feedback gains in revolute segment 2 
+controller.kx2 = 5e-3;
+controller.kf2 = 2e-8;
+% feedback gains in prismatic segment 2
+controller.kxt2 = 1e-3;
+controller.kft2 = 8e-9;
+controller.kVgain = 5;
+% lambda gains in joint-space control
+controller.lamTh1 = 3;
+controller.lamTh2 = 3;
+controller.lamxT2 = 3;
+% lambda gains in Cartesian control
+controller.lamX = 6;
+controller.lamY = 6;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Encoder and pressure measurements are filtered with 2nd order low-pass  %
+% filters. Their cut-off frequencies are defined here. Similar happens    %
+% for filters after numerical derivatives of required velocities and      %
+% required piston forces. More details can be found from the              %
+% 'init2ndOrderLowPassFilter.m' function.                                 %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% adds simulated noise to encoder meaasurements
+encoder.measNoisePower = 1e-11;
+% adds simulated noise to pressure measurements
+pres.measNoisePower = 5e5;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+filtEnc.cutOffFrequency = 24*2*pi;
+filtPres.cutOffFrequency = 24*2*pi;
+filtDV.cutOffFrequency = 8*2*pi;
+filtDF.cutOffFrequency = 8*2*pi;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Uncertainty factor allow supplying non-ideal values of physical         %
+% quantities to the VDC controller. Setting them to 1 and the controller  %
+% will 'know' all the parameters ideally. More details can be found from  %
+% the supplied 'initializeParameterUpdate.m' file.                        %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+update.inertiaUncertainty = 0.9;
+update.cyl1Uncertainty = 0.95;
+update.cyl2Uncertainty = 1.05;
+update.cylT2Uncertainty = 1.1;
+update.valve1Uncertainty = 0.97;
+update.valve2Uncertainty = 1.05;
+update.valveT2Uncertainty = 1;
+update.frictionRS1Uncertainty = 0.9;
+update.frictionRS2Uncertainty = 1.1;
+update.frictionPS2Uncertainty = 0.9;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Initialize all the rest.
+InitializeSimulationParameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           RUN SIMULATION                                %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Run the simulation.
+sim('VDCHyd2022a.slx',[0 simulationEndTime])

WrWD22.m

@@ -0,0 +1,14 @@
+function r = WrWD22(theta1, theta2, xt2)
+
+    % TCP forward kinematics. For more details, please see the 
+    % supplementary 'ForwardKinematicsXS031.pdf'
+    r = [-0.189261 + (1.99949 +  xt2) .*cos(theta1 + theta2) + ...
+         (-0.100002 + (-0.00152855 - 0.000928646 *xt2) .*cos(theta2)).* sin(theta1) + ...
+         cos(theta1).* (1.59687 + (-0.00152855 - 0.000928646 *xt2).* sin(theta2)) - ...
+         0.152638 *sin(theta1 + theta2), 0.85 + ...
+         sin(theta1).* (1.59687 + (1.99949 + xt2).* cos(theta2) + ...
+         (-0.154167 - 0.000928646 *xt2).* sin(theta2)) + ... 
+         cos(theta1).*(0.100002 + (0.154167 + 0.000928646* xt2).* cos(theta2) + ...
+         (1.99949 +  xt2).* sin(theta2)), -0.0092303*ones(size(xt2))];
+
+end

init2ndOrderLowPassFilter.m

@@ -0,0 +1,22 @@
+function filt = init2ndOrderLowPassFilter(cof, damp, dT)
+
+    % complex variable s
+    s = tf('s');
+    % cut-off frequency of the filter [rad/s]
+    filt.cof = cof;
+    % filter damping ratio
+    filt.damp = damp;
+    % filter sampling time
+    filt.dT = dT;
+    % discretize the filter transfer function and extract the coefficients
+    Wf = 1/((s/filt.cof)^2 + 2*filt.damp/filt.cof*s + 1);
+    [numfd,denfd] = tfdata(c2d(Wf, filt.dT, 'tustin'), 'v');
+    denfd = denfd/denfd(1);
+    numfd = numfd/denfd(1);
+    filt.a1 = denfd(2);
+    filt.a0 = denfd(3);
+    filt.b2 = numfd(1);
+    filt.b1 = numfd(2);
+    filt.b0 = numfd(3);
+
+end

initInertiaUpdate.m

@@ -0,0 +1,29 @@
+function [thetaXr, thetaXrLB, thetaXrUB] = initInertiaUpdate(m, r, I, UB, LB)
+
+    % Form thetaAr vector per Eq. (A.43) - (A.55) from Zhu, W. H. (2010). 
+    % Virtual decomposition control: toward hyper degrees of freedom robots
+    % (Vol. 60). Springer Science & Business Media.
+    rx = r(1); ry = r(2); rz = r(3);    
+    Ixx = I(1,1); Iyy = I(2,2); Izz = I(3,3);
+    Iyz = I(2,3); Ixz = I(1,3); Ixy = I(1,2);    
+    t1 = rx*rx;
+    t2 = ry*ry;
+    t3 = rz*rz;
+    t4 = rx*m;    
+    thetaXr = [m; rx*m; ry*m; rz*m; t1*m; t2*m; t3*m;...
+                 t4*ry - Ixy; t4*rz - Ixz; m*ry*rz - Iyz; Ixx; Iyy; Izz];
+    % Establish lower and upper bound to be used in the parameter 
+    % adapatation part as:
+    % upperBound = nominal*UB
+    % lowerBound = nominal*LB
+    thetaXrUB = thetaXr.*UB;
+    thetaXrLB = thetaXr.*LB;
+    for i = 1:length(thetaXrUB)
+        if thetaXrUB(i) < thetaXrLB(i)
+            aux = thetaXrLB(i);
+            thetaXrLB(i) = thetaXrUB(i);
+            thetaXrUB(i) = aux;
+        end
+    end  
+
+end

initializeAuxiliaryQuantities.m

@@ -0,0 +1,9 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                           AUXILIARY QUANTITIES                          %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+aux.xf =   [1;0;0;0;0;0];
+aux.xf =   [0;1;0;0;0;0];
+aux.zf =   [0;0;1;0;0;0];
+aux.xtau = [0;0;0;1;0;0];
+aux.ytau = [0;0;0;0;1;0];
+aux.ztau = [0;0;0;0;0;1];

initializeConstantGeometricData.m

@@ -0,0 +1,20 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                       CONSTANT GEOMETRIC DATA                           %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% lengths [m]
+geom.L1 = 1.0071227324212;
+geom.L11 = 0.37610632101293;
+geom.L2 = 1.132185817991;
+geom.L21 = 0.30901132665325;
+geom.lc1 = 0.745;
+geom.lc01 = 0.085;
+geom.lc2 = 0.745;
+geom.lc02 = 0.085;
+geom.lc3 = 1.2;
+% angles [rad]
+geom.beta1 = atan2d(374.261,935)*pi/180;
+geom.beta2 = atan2d(45.757,373.313)*pi/180;
+geom.beta3 = atan2d(139.50,1132.19)*pi/180;
+geom.beta4 = atan2d(88.061,296.198)*pi/180;
+geom.alpha1 = 1.9515432018109;
+geom.alpha2 = 0.24548563270531;

initializeConstantPositionVectors.m

@@ -0,0 +1,20 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                       CONSTANT POSITION VECTORS                         %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% lengths in [m]
+posVec.P32_r_P32D22 = [0.40499744871392; -0.13130517281345; -0.013385243295654];
+posVec.B52_r_B52P32 = [geom.lc3; 0; 0];
+posVec.P22_r_P22P22p = [0.041; 0; 0];
+posVec.Tc2_r_Tc2P22 = [-0.048858374920227;0.36587359346146;0.035276938723102];
+posVec.B12_r_B12Tc2 = [geom.L21;0;0];
+posVec.Bc2_r_Bc2B12 = [geom.L2;0;0];
+posVec.B42_r_B42Tc2 = [geom.lc2; 0; 0];
+posVec.B32_r_B32B32p = [geom.lc01;0;0];
+posVec.Bc2_r_Bc2B32 = [0;0;0];
+posVec.Tc1_r_Tc1Bc2 = [0.098495302558553; 0.01838181329939; 0];
+posVec.B11_r_B11Tc1 = [geom.L11; 0; 0];
+posVec.Bc1_r_Bc1B11 = [geom.L1; 0; 0];
+posVec.Bc1_r_Bc1B31 = [0;0;0];
+posVec.B31_r_B31B31p = [geom.lc02;0;0];
+posVec.B41_r_B41Tc1 = [geom.lc1;0;0];
+posVec.W_r_WBc1 = [0.185;-0.085;0];

initializeConstantRotationMatrices.m

@@ -0,0 +1,21 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                      CONSTANT ROTATION MATRICES                         %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+rotMat.W_Rot_Bc1 = [cos(geom.alpha1) -sin(geom.alpha1) 0;...
+             sin(geom.alpha1)  cos(geom.alpha1) 0;...
+             0            0           1];
+rotMat.B31_Rot_B41 = eye(3);
+rotMat.B11_Rot_Tc1 = eye(3);
+rotMat.Tc1_Rot_Bc2 = [cos(geom.alpha2) -sin(geom.alpha2) 0;...
+                      sin(geom.alpha2)  cos(geom.alpha2) 0;...
+                                0            0           1];
+rotMat.B32_Rot_B42 = eye(3);  
+rotMat.B12_Rot_Tc2 = eye(3);
+rotMat.Tc2_Rot_P22 = [0.95853460763586 -0.27664713597974  0.0683942111482;...
+                      0.28497614981675  0.93051946315663 -0.23004808784757;...
+                      0                 0.23999977258511  0.9707729441837];
+rotMat.P22_Rot_B52 = eye(3);
+rotMat.B52_Rot_P32 = eye(3);
+rotMat.P32_Rot_D22 = [1               0                0;...
+                      0 0.9707729441837 0.23999977258512;...
+                      0 -0.23999977258512 0.9707729441837];

initializeControlModes.m

@@ -0,0 +1,107 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%                             CONTROL MODES                               %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Examples provided here have the purpose of illustrating how Cartesian and
+% joint references can are formed in VDC. These implemenent core ideas as 
+% presented in Section (3.3.6) of Zhu, Wen-Hong. Virtual decomposition 
+% control: toward hyper degrees of freedom robots. Vol. 60. Springer 
+% Science & Business Media, 2010. using specific examples (diamond path
+% and sine wave command for joint velocities).
+%  
+% NB! The contents of simulation subsystems providing Cartesian and 
+% joint-space references can be entirely changed per user's wishes,
+% but taking care that Eq. (3.20) and Eq. (3.21)-(3.22) hold.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% MODE 0 = Cartesian control (Eq. (3.21) - (3.22))                        %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% In the present demonstration the TCP is asked to follow a diamond path
+% defined below. Starting from the initial positon 0 the diamond path is
+% point-to-point A-B-C-D-A path and the last step is returning to 0.
+%                          B^
+%                         A/ \C
+%                         /\ /
+%                        0  .D
+% Get the initial TCP position (0).
+r0 = WrWD22(initVals.theta10, initVals.theta20, initVals.xt20); 
+x0 = r0(1); y0 = r0(2); z0 = r0(3); 
+% Define waypoints (0-A-B-C-D-A-0) and form 5th order point-to-point path.
+pathPlanner.waypoints = [x0 2.5 3.0 3.5 3.0 2.5  x0;...
+                         y0 1.0 1.5 1.0 0.5 1.0  y0;...
+                         z0  z0  z0  z0  z0  z0  z0];
+clear r0 x0 y0 z0
+% NB! A flexible path planner can be implemented inside the simulation 
+% instead of using the beforehand generated path below, but this is not the
+% main topic and solutions are infinite. For the illustration purposes here
+% a quintic polynomial path is generated beforehand.
+
+% The amount of seconds required from point-to-point.
+pathPlanner.pointToPointTime = 5;
+% End the simulation after returning to the starting point.
+simulationEndTime = pathPlanner.pointToPointTime*(length(pathPlanner.waypoints(1,:))-1);
+% Time points at which a particular path point should be reached.
+pathPlanner.timePoints = 0:pathPlanner.pointToPointTime:simulationEndTime;
+% Time vector as independent variable for the desired path.
+pathPlanner.timeVector = 0:controller.dT:simulationEndTime;
+% Use off-the-shelf MATLAB function here for path generation.
+[pathPlanner.posDesCart, pathPlanner.velDesCart, ~, ~] = ...
+    quinticpolytraj(pathPlanner.waypoints, pathPlanner.timePoints, pathPlanner.timeVector);
+% Create point cloud for path illustration inside the Mechanics explorer.
+% Adding 1e-7*rand(...) is to avoid having the same points twice, which is
+% seen as error in the block providing visualization.
+% NB! Values 1e-7, 500 and 1000 are heuristically hardcoded. These affect 
+% only thevisualization and can be freely changed.
+pointCloudMatrix0 = pathPlanner.posDesCart(:,1:500/(controller.dT*1000):simulationEndTime/controller.dT)' + ...
+    1e-7*rand(size(pathPlanner.posDesCart(:,1:500/(controller.dT*1000):simulationEndTime/controller.dT)'));
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% MODE 1 = joint-space control (Eq. (3.20))                               %
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% For all the three joints, sine-wave references are generated for the
+% illustration purposes. This part can also be subject to any desired
+% change if validity of Eq. (3.20) is preserved and if the care is taken
+% that the desired velocity is differentiable.
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Th1 = Th1B + Th1A*sin(Th1f*t) [rad]
+pathPlanner.theta1A = 10*pi/180;
+pathPlanner.theta1B = 0;
+pathPlanner.theta1f = 0.15*2*pi;
+% Th2 = Th2B + Th2A*sin(Th2f*t) [rad]
+pathPlanner.theta2A = 15*pi/180;
+pathPlanner.theta2B = 0;
+pathPlanner.theta2f = 0.25*2*pi;
+% xt2 = xt2B + xt2A*sin(xt2f*t) [m]
+pathPlanner.xt2A = 0.1;
+pathPlanner.xt2B = 0;
+pathPlanner.xt2f = 0.20*2*pi;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% Calculations below only served the purpose of generating a point cloud
+% used for the illustration purposes in Mechanics explorer. The anticipated
+% TCP positions in the Cartesian workspace are calculated and displayed.
+pathPlanner.theta1Des = zeros(length(pathPlanner.timeVector),1);
+pathPlanner.theta1Des(1,1) = initVals.theta10;
+for i = 2:length(pathPlanner.timeVector)
+    pathPlanner.theta1Des(i,1) = pathPlanner.theta1Des(i-1,1) + (pathPlanner.theta1B + ...
+        pathPlanner.theta1A*sin(pathPlanner.theta1f*pathPlanner.timeVector(i)))*controller.dT;
+end
+pathPlanner.theta2Des = zeros(length(pathPlanner.timeVector),1);
+pathPlanner.theta2Des(1,1) = initVals.theta20;
+for i = 2:length(pathPlanner.timeVector)
+    pathPlanner.theta2Des(i,1) = pathPlanner.theta2Des(i-1,1) + (pathPlanner.theta2B + ...
+        pathPlanner.theta2A*sin(pathPlanner.theta2f*pathPlanner.timeVector(i)))*controller.dT;
+end
+pathPlanner.xt2Des = zeros(length(pathPlanner.timeVector),1);
+pathPlanner.xt2Des(1,1) = initVals.xt20;
+for i = 2:length(pathPlanner.timeVector)
+    pathPlanner.xt2Des(i,1) = pathPlanner.xt2Des(i-1,1) + (pathPlanner.xt2B + ...
+        pathPlanner.xt2A*sin(pathPlanner.xt2f*pathPlanner.timeVector(i)))*controller.dT;
+end
+pathPlanner.posDesJoint = WrWD22(pathPlanner.theta1Des,pathPlanner.theta2Des,pathPlanner.xt2Des)';
+% Create point cloud for path illustration inside the Mechanics explorer.
+pointCloudMatrix1 = pathPlanner.posDesJoint(:,1:100/(controller.dT*1000):end)' + ...
+    1e-7*rand(size(pathPlanner.posDesJoint(:,1:100/(controller.dT*1000):end)'));
+% Illustrate only relevant for the chosen operating mode.
+if MODE == 0
+    pointCloudMatrix = pointCloudMatrix0;
+else 
+    pointCloudMatrix = pointCloudMatrix1;
+end
+clear i pointCloudMatrix0 pointCloudMatrix1