You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
48 lines
1.1 KiB
48 lines
1.1 KiB
function [J,arm,elbow,flip] = LBRinvKinMinChange(M,J,delta,robot)
|
|
|
|
possible_configs = 2^3;
|
|
joints = size(J,2);
|
|
|
|
J_new = zeros(possible_configs,joints);
|
|
J_arm = zeros(possible_configs,1);
|
|
J_elbow = zeros(possible_configs,1);
|
|
J_flip = zeros(possible_configs,1);
|
|
|
|
% calculate for each config posibility
|
|
i=1;
|
|
for arm = [-1,1]
|
|
for elbow = [-1,1]
|
|
for flip = [-1,1]
|
|
|
|
J_arm(i) = arm;
|
|
J_elbow(i) = elbow;
|
|
J_flip(i) = flip;
|
|
|
|
arg = [J_arm(i),J_elbow(i),J_flip(i),delta];
|
|
try
|
|
J_new(i,:) = LBRinvKin(M,arg,robot);
|
|
catch
|
|
% config is not valid dectemten to write
|
|
% at same position again
|
|
i = i - 1;
|
|
end
|
|
|
|
i = i + 1 ;
|
|
end
|
|
end
|
|
end
|
|
|
|
% cut unused space
|
|
J_new = J_new(1:i-1,:);
|
|
|
|
% get the minimum change index
|
|
J_changes = sum((J_new - repmat(J,possible_configs,1)).^2,2);
|
|
[~,index] = min(J_changes);
|
|
|
|
% set return values
|
|
J = J_new(index,:);
|
|
arm = J_arm(index);
|
|
elbow = J_elbow(index);
|
|
flip = J_flip(index);
|
|
|
|
|