184 lines
9.6 KiB
Mathematica
184 lines
9.6 KiB
Mathematica
|
function debugPlot(robot, benchmarkSettings, experimentDataStruct, Beta_0, estimatedBeta, options, expNb, algName, Betas, it, varargin)
|
||
|
|
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
%
|
||
|
|
% Generate a set of debug plots allowing to visualize the relevance of a
|
||
|
|
% given set of parameters along a given trajectory.
|
||
|
|
|
||
|
|
if nargin < 9
|
||
|
|
Betas = estimatedBeta;
|
||
|
|
it = 0;
|
||
|
|
end
|
||
|
|
|
||
|
|
if options.debug == true
|
||
|
|
[estimatedBeta benchmarkSettings.Beta_obj]
|
||
|
|
options.filter = 'butterworth';
|
||
|
|
[~, Tau, Qpp, Qp, Q, ~, ~, ~, ~, ~, ~] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, options, expNb, algName);
|
||
|
|
|
||
|
|
jointNb = 3;
|
||
|
|
|
||
|
|
tau = Tau(jointNb,:)';
|
||
|
|
q = Q(jointNb,:)';
|
||
|
|
qp = Qp(jointNb,:)';
|
||
|
|
qpp = Qpp(jointNb,:)';
|
||
|
|
|
||
|
|
% Simulation of the robot using the computed parameters:
|
||
|
|
t_control = linspace(benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples); % Control epochs
|
||
|
|
augmentedDesiredState = benchmarkSettings.trajectoryData.getTrajectoryData(t_control, benchmarkSettings.interpolationAlgorithm); % augmentedState = [Qpp; Qp; Q];
|
||
|
|
|
||
|
|
% Simulation of the robot:
|
||
|
|
if strcmp(benchmarkSettings.codeImplementation,'optim')
|
||
|
|
% Simulation of the robot using the current parameters:
|
||
|
|
if Beta_0 ~= estimatedBeta % In case the algorithm requires an initial estimate (e.g. CLIE, CLOE, IV, DIDIM...)
|
||
|
|
[~, State_0, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, Beta_0, robot.name, robot.numericalParameters.Geometry, ...
|
||
|
|
robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
|
||
|
|
robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
|
||
|
|
robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
|
||
|
|
robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
|
||
|
|
end
|
||
|
|
[~, State_it, ~] = integrateClosedLoopDynamics_mex(augmentedDesiredState, estimatedBeta, robot.name, robot.numericalParameters.Geometry, ...
|
||
|
|
robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
|
||
|
|
robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
|
||
|
|
robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
|
||
|
|
robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
|
||
|
|
elseif strcmp(benchmarkSettings.codeImplementation,'classic')
|
||
|
|
% Simulation of the robot using the current parameters:
|
||
|
|
if Beta_0 ~= estimatedBeta % In case the algorithm requires an initial estimate (e.g. CLIE, CLOE, IV, DIDIM...)
|
||
|
|
[~, State_0, ~] = integrateClosedLoopDynamics(augmentedDesiredState, Beta_0, robot.name, robot.numericalParameters.Geometry, ...
|
||
|
|
robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
|
||
|
|
robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
|
||
|
|
robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
|
||
|
|
robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
|
||
|
|
end
|
||
|
|
[~, State_it, ~] = integrateClosedLoopDynamics(augmentedDesiredState, estimatedBeta, robot.name, robot.numericalParameters.Geometry, ...
|
||
|
|
robot.numericalParameters.Gravity, benchmarkSettings.t_i, benchmarkSettings.t_f, benchmarkSettings.nbCtrlSamples, benchmarkSettings.nbSamples, ...
|
||
|
|
robot.controlParameters.Kp, robot.controlParameters.Ki, robot.controlParameters.Kd, robot.controlParameters.Ktau, robot.controlParameters.antiWindup, ...
|
||
|
|
robot.physicalConstraints.limQ_L, robot.physicalConstraints.limQ_U, robot.physicalConstraints.limQp_L, robot.physicalConstraints.limQp_U, ...
|
||
|
|
robot.physicalConstraints.limQpp_L, robot.physicalConstraints.limQpp_U, robot.physicalConstraints.limTau_L, robot.physicalConstraints.limTau_U, benchmarkSettings.integrationAlgorithm);
|
||
|
|
else
|
||
|
|
error('%s: unknown implementation option', algName);
|
||
|
|
end
|
||
|
|
|
||
|
|
|
||
|
|
% Compute the observation matrix:
|
||
|
|
W = observationMatrix(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q, Qp, Qpp);
|
||
|
|
|
||
|
|
referenceTau = W*benchmarkSettings.Beta_obj;
|
||
|
|
recomputedTau = W*estimatedBeta;
|
||
|
|
recomputedTau0 = W*Beta_0;
|
||
|
|
|
||
|
|
figure('Name',sprintf('Torque %s',options.debugPlot));
|
||
|
|
plot1 = plot(tau,'b','LineWidth',0.5);
|
||
|
|
hold on
|
||
|
|
plot(referenceTau(jointNb:robot.nbDOF:end),'g', 'LineWidth',2);
|
||
|
|
hold on
|
||
|
|
plot(recomputedTau0(jointNb:robot.nbDOF:end),'r--','LineWidth',1);
|
||
|
|
hold on
|
||
|
|
plot(recomputedTau(jointNb:robot.nbDOF:end),'m','LineWidth',1);
|
||
|
|
legend({'real noisy signal', 'reference parameters', 'initial parameters',sprintf('%s parameters',algName)})
|
||
|
|
plot1(1,1).Color(4)=0.25;
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Tau');
|
||
|
|
title('Recomputed torque')
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
|
||
|
|
if Beta_0 ~= estimatedBeta % In case the algorithm requires an initial estimate (e.g. CLIE, CLOE, IV, DIDIM...), plot both the initial estimate and the final estimate trajectories.
|
||
|
|
Err=(q-State_it(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)');
|
||
|
|
Err0=(q-State_0(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)');
|
||
|
|
figure('Name',sprintf('%s',options.debugPlot));
|
||
|
|
subplot(4,1,1)
|
||
|
|
plot(State_0(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)','--','LineWidth',2)
|
||
|
|
hold on
|
||
|
|
plot(q,'LineWidth',1)
|
||
|
|
legend(sprintf('q%s simulated',jointNb),sprintf('q%s robot',jointNb))
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Q');
|
||
|
|
title('Trajectory initial parameter set')
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
subplot(4,1,2)
|
||
|
|
plot(100*Err0)
|
||
|
|
legend('100 x Error')
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Joint Error');
|
||
|
|
title('Trajectory Error initial parameter set')
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
subplot(4,1,3)
|
||
|
|
plot(State_it(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)','--','LineWidth',2)
|
||
|
|
hold on
|
||
|
|
plot(q,'LineWidth',1)
|
||
|
|
legend(sprintf('q%s simulated',jointNb),sprintf('q%s robot',jointNb))
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Q');
|
||
|
|
title(sprintf('Trajectory %s',algName))
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
subplot(4,1,4)
|
||
|
|
plot(100*Err)
|
||
|
|
legend('100 x Error')
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Joint Error');
|
||
|
|
title(sprintf('Trajectory Error %s',algName))
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
else % Otherwise just plot both the final estimate trajectory...
|
||
|
|
Err=(q-State_it(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)');
|
||
|
|
figure('Name',sprintf('%s',algName));
|
||
|
|
subplot(2,1,1)
|
||
|
|
plot(State_it(robot.nbDOF+jointNb,benchmarkSettings.samplingBorder+1:end-benchmarkSettings.samplingBorder)','--','LineWidth',2)
|
||
|
|
hold on
|
||
|
|
plot(q,'LineWidth',1)
|
||
|
|
legend(sprintf('q%s simulated',jointNb),sprintf('q%s robot',jointNb))
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Q');
|
||
|
|
title(sprintf('Trajectory %s', algName))
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
subplot(2,1,2)
|
||
|
|
plot(100*Err)
|
||
|
|
legend('100 x Error')
|
||
|
|
xlabel('Samples');
|
||
|
|
ylabel('Joint Error');
|
||
|
|
title(sprintf('Trajectory Error %s',algName))
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
end
|
||
|
|
|
||
|
|
if strcmp(algName,'ANN') || strcmp(algName,'HTRNN')
|
||
|
|
if options.debug == true
|
||
|
|
cc=parula(robot.paramVectorSize);
|
||
|
|
figure('Name',sprintf('%s',options.debugPlot));
|
||
|
|
for i=1:robot.paramVectorSize
|
||
|
|
plot(Betas(i,:)','LineWidth',2 ,'color',cc(i,:))
|
||
|
|
hold on
|
||
|
|
plot(repmat(benchmarkSettings.Beta_obj(i),1,it-1)','--','LineWidth',2,'color',cc(i,:))
|
||
|
|
end
|
||
|
|
hold off
|
||
|
|
legend('Identified Parameters','True Parameters')
|
||
|
|
xlabel('Iterations');
|
||
|
|
ylabel('Beta');
|
||
|
|
title(sprintf('Parameter Convergence of %s Parameter Estimation',algName))
|
||
|
|
ylim([-5,5])
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
|
||
|
|
err = Betas-repmat(benchmarkSettings.Beta_obj,1,it-1);
|
||
|
|
err2 = sum(err.^2,1);
|
||
|
|
|
||
|
|
figure('Name','Convergence');
|
||
|
|
plot(err2,'LineWidth',2 )
|
||
|
|
legend('Squared Error')
|
||
|
|
xlabel('Iterations');
|
||
|
|
ylabel('Error');
|
||
|
|
title(sprintf('Convergence of %s Parameter Estimation',algName))
|
||
|
|
grid on
|
||
|
|
grid minor
|
||
|
|
end
|
||
|
|
end
|
||
|
|
pause
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|