43 lines
2.3 KiB
Mathematica
43 lines
2.3 KiB
Mathematica
|
function [W] = observationMatrix(robotName, paramVectorSize, Geometry, Gravity, Q, Qp, Qpp) %#codegen
|
||
|
|
|
||
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
||
|
|
%
|
||
|
|
% Building of the observation matrix W for the current iteration of Beta
|
||
|
|
|
||
|
|
[nbDOF,nbSamples]=size(Q);
|
||
|
|
W = zeros(nbSamples*nbDOF, paramVectorSize);
|
||
|
|
|
||
|
|
for i=1:nbSamples
|
||
|
|
% Compute the observation matrix:
|
||
|
|
switch robotName % [toyRobot, SCARA, UR3, UR5, UR10, TX40, TX40_uncoupled, RX90, PUMA560, REEMC_right_arm, ICUB_right_arm, NAO_right_arm]
|
||
|
|
% case 'toyRobot'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_toyRobot(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'SCARA'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_SCARA(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'UR3'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_UR3(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'UR5'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_UR5(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'UR10'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_UR10(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
case 'TX40'
|
||
|
|
W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_TX40(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
case 'TX40_uncoupled'
|
||
|
|
W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_TX40_uncoupled(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'RX90'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_RX90(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'PUMA560'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_PUMA560(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'REEMC_right_arm'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_REEMC_right_arm(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
case 'RV2SQ'
|
||
|
|
W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_RV2SQ(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
% case 'NAO_right_arm'
|
||
|
|
% W(nbDOF*(i-1)+1:nbDOF*i,:) = Regressor_Y_NAO_right_arm(Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity);
|
||
|
|
otherwise
|
||
|
|
W(nbDOF*(i-1)+1:nbDOF*i,:) = feval(sprintf('Regressor_Y_%s',robotName),Q(:,i), Qp(:,i), Qpp(:,i), Geometry, Gravity); % Much slower when compiled !
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|