Enforce the kinematics structure underlying the skeleton's limbs

!enforce-skeleton-kinematics.md

💡 Idea

This MATLAB script runs nonlinear constrained optimization to find the underlying kinematic configuration of a skeleton arm whose projection in a 2D image plane can be observed through e.g. OpenPose.

See robotology/assistive-rehab#188.

solve.m

function x=solve(joints)
% Input: 5D vector of joints used to construct the desired configuration
% Output: 5D vector of solved joints
%
% Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT)
% All Rights Reserved.
%
% Author: Ugo Pattacini <ugo.pattacini@iit.it>

    % limb lengths
    global lengths;
    lengths=[0.3 1 1];

    % camera's parameters
    w=320; h=240;
    fov=50*pi/180;
    f=0.5*h*1/tan(fov/2);
    K=[f 0 w/2 0;...
       0 f h/2 0;...
       0 0   1 0];
    Hcam=[-1  0  0  0;...
           0  0 -1  5;...
           0 -1  0 -1;...
           0  0  0  1];

    figure('color','white');
    
    % display 3D rest (dashed red) and desired (blue) configurations
    hax_3d=subplot(121);
    title('3D'); view(3); axis('equal');
    grid('on'); hold('on');
    xlim([-3 3]); xlabel('x');
    ylim([-3 3]); ylabel('y');
    zlim([-3 3]); zlabel('z');
    p0=fkin([0 0 0 0 0]);
    pt=fkin(joints);
    plot_points(hax_3d,p0,'ro--');
    plot_points(hax_3d,pt,'bo-');

    % display rest (dashed red) and desired (blue) configurations
    % in the image plane
    hax_2d=subplot(122);
    title('2D'); axis('equal');
    grid('on'); hold('on');
    xlim([0 320]); xlabel('u');
    ylim([0 240]); ylabel('v');
    hax_2d.YDir='reverse';
    px0=cell(1,length(p0));
    pxt=cell(1,length(pt));
    for i=1:length(p0)
        px0{i}=K*(Hcam\p0{i});
        px0{i}=px0{i}/px0{i}(3);
        px0{i}(3)=[];

        pxt{i}=K*(Hcam\pt{i});
        pxt{i}=pxt{i}/pxt{i}(3);
        pxt{i}(3)=[];
    end
    plot_points(hax_2d,px0,'ro--');
    plot_points(hax_2d,pxt,'bo-');
    drawnow;
    
    % compute the cost and constraint function symbolically
    global x1 x2 x3 x4 x5;
    syms x1 x2 x3 x4 x5 real;
    
    R{1}=[cos(x1) -sin(x1) 0;...
          sin(x1)  cos(x1) 0;...
                0        0 1];
    R{2}=[1       0        0;...
          0 cos(x2) -sin(x2);...
          0 sin(x2)  cos(x2)];
    R{3}=[ cos(x3) 0 sin(x3);...
                 0 1       0;...
          -sin(x3) 0 cos(x3)];
    R{4}=[cos(x4) -sin(x4) 0;...
          sin(x4)  cos(x4) 0;...
                0        0 1];
    R{5}=[1       0        0;...
          0 cos(x5) -sin(x5);...
          0 sin(x5)  cos(x5)];

    orig=[0 0 0 1]';
    p{1}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*orig;
    p{2}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*...
         [R{2} [0 0 0]'; 0 0 0 1]*...
         [R{3} [0 0 0]'; 0 0 0 1]*...
         [R{4} [0 0 -lengths(2)]'; 0 0 0 1]*orig;
    p{3}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*...
         [R{2} [0 0 0]'; 0 0 0 1]*...
         [R{3} [0 0 0]'; 0 0 0 1]*...
         [R{4} [0 0 -lengths(2)]'; 0 0 0 1]*...
         [R{5} R{5}*[0 0 -lengths(3)]'; 0 0 0 1]*orig;
     
    px=cell(1,length(p));
    for i=1:length(p)
        px{i}=K*(Hcam\p{i});
        px{i}=px{i}/px{i}(3);
        px{i}(3)=[];
    end
    
    pxt(1)=[];  % remove origin
    global cost dcost constr dconstr;
    cost=(dot(pxt{1}-px{1},pxt{1}-px{1})+...
          dot(pxt{2}-px{2},pxt{2}-px{2})+...
          dot(pxt{3}-px{3},pxt{3}-px{3}))/3;
    dcost=gradient(cost,[x1 x2 x3 x4 x5]);
    
    constr=dot(pt{end}-p{end},pt{end}-p{end});
    dconstr=gradient(constr,[x1 x2 x3 x4 x5]);
    
    % run the optimization
    options=optimoptions('fmincon','Algorithm','interior-point',...
                         'SpecifyObjectiveGradient',true,...,
                         'SpecifyConstraintGradient',true,...,
                         'Display','iter',...
                         'OptimalityTolerance',1e-3,...
                         'ObjectiveLimit',1,...
                         'ConstraintTolerance',1e-4);
    lb=[-pi -pi/2  -pi -pi/2  0];
    ub=[ pi    pi pi/2  pi/2 pi];
    x=fmincon(@fun,(lb+ub)/2,[],[],[],[],lb,ub,@nonlcon,options);
    
    % display results (green)
    p=fkin(x);
    plot_points(hax_3d,p,'go-');
    
    px=cell(1,length(p));
    for i=1:length(p)
        px{i}=K*(Hcam\p{i});
        px{i}=px{i}/px{i}(3);
        px{i}(3)=[];
    end
    plot_points(hax_2d,px,'go-');

end


%--------------------------------------------------------------------------
function plot_points(hax,p,mode)

    for i=2:length(p)
        if length(p{i})>=3
            plot3(hax,[p{i-1}(1) p{i}(1)],[p{i-1}(2) p{i}(2)],...
                      [p{i-1}(3) p{i}(3)],mode);
        else
            plot(hax,[p{i-1}(1) p{i}(1)],[p{i-1}(2) p{i}(2)],mode);
        end
    end
    
end


%--------------------------------------------------------------------------
function p=fkin(joints)

    global lengths;

    R{1}=axang2rotm([0 0 1 joints(1)]);
    R{2}=axang2rotm([1 0 0 joints(2)]);
    R{3}=axang2rotm([0 1 0 joints(3)]);
    R{4}=axang2rotm([0 0 1 joints(4)]);
    R{5}=axang2rotm([1 0 0 joints(5)]);

    p{1}=[0 0 0 1]';
    p{2}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*p{1};
    p{3}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*...
         [R{2} [0 0 0]'; 0 0 0 1]*...
         [R{3} [0 0 0]'; 0 0 0 1]*...
         [R{4} [0 0 -lengths(2)]'; 0 0 0 1]*p{1};
    p{4}=[R{1} R{1}*[lengths(1) 0 0]'; 0 0 0 1]*...
         [R{2} [0 0 0]'; 0 0 0 1]*...
         [R{3} [0 0 0]'; 0 0 0 1]*...
         [R{4} [0 0 -lengths(2)]'; 0 0 0 1]*...
         [R{5} R{5}*[0 0 -lengths(3)]'; 0 0 0 1]*p{1};

end



%--------------------------------------------------------------------------
function [f,g]=fun(x)

    global x1 x2 x3 x4 x5 cost dcost;

    f=double(subs(cost,[x1 x2 x3 x4 x5],[x(1) x(2) x(3) x(4) x(5)]));
    if nargout>1
        g=double(subs(dcost,[x1 x2 x3 x4 x5],[x(1) x(2) x(3) x(4) x(5)]));
    end
    
end



%--------------------------------------------------------------------------
function [c,ceq,dc,dceq]=nonlcon(x)

    global x1 x2 x3 x4 x5 constr dconstr;

    c=[];
    ceq=double(subs(constr,[x1 x2 x3 x4 x5],[x(1) x(2) x(3) x(4) x(5)]));
    if nargout>2
        dc=[];
        dceq=double(subs(dconstr,[x1 x2 x3 x4 x5],[x(1) x(2) x(3) x(4) x(5)]));
    end
    
end