MATLAB script to analyze shoulder's cable length guards

!analyze-shoulder-cable-length.md

analyze-shoulder-cable-length

MATLAB script to analyze shoulder's cable length guards (see robotology/community#74).

analyze.m

function analyze(filename)
% Copyright: (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
% Author: Ugo Pattacini <ugo.pattacini@iit.it>
% CopyPolicy: Released under the terms of the GNU GPL v2.0.

c=1.7105;
A=[ c, -c,  0; ...
   -1,  1,  0; ...
   -c,  c,  0; ...
    1, -1,  0; ...
    0, -1,  0; ...
    0,  1,  0; ...  
    0,  1,  0; ...
    0, -1,  0; ...
    c, -c, -c; ...
   -1,  1,  1; ...
   -c,  c,  c; ...
    1, -1, -1; ...
    0,  1,  1; ...
    0, -1, -1];

b=[ 404.0; ...
     54.3; ...
     46.0; ...
    305.7; ...
    215.7; ...
    150.0; ...
     54.3; ...
    210.0; ...
    431.0; ...
    101.7; ...
    109.0; ...
    258.3; ...
     71.7; ...
    228.3];

% actually implemented in the software
% from the solution of a linear
% programming problem
Asw=[ 1.71, -1.71,  0.0;  ...
      1.71, -1.71, -1.71; ...
     -1.71,  1.71,  1.71; ...
      0.0,   1.0,   1.0;  ...
      0.0,  -1.0,  -1.0];

bsw=[347.00; ...
     366.57; ...
     112.42; ...
      66.60; ...
     213.30];

% single joint min/max bounds (iCubGrenoble01)
theta_min=[-95.5   0.0 -30.0];
theta_max=[ 10.0 161.0  75.0];

figure('color','white');

theta=load(filename);
theta=theta(:,1:3);
shape=ones(1,length(theta));

lg=A*theta'+b*shape;
lgsw=Asw*theta'+bsw*shape;

subplot(211); hold('on');
title(filename);
stairs(theta(:,1));
stairs(theta(:,2));
stairs(theta(:,3));
legend({'\theta_0','\theta_1','\theta_2'});
ylabel('\theta [deg]');

for i=1:3
    idx=find((theta(:,i)<theta_min(i))|(theta(:,i)>theta_max(i)));
    if ~isempty(idx)
        plot(idx,theta(idx,i),'r.');
    end
end

subplot(212); hold('on');
stairs(min(lg));
stairs(min(lgsw));
legend({'min(A\theta+b)','min(A_{sw}\theta+b_{sw})'});
lim=axis; axis([lim(1:2) 0 lim(4)]);
ylabel('cable length guards');
xlabel('samples [#]');