%% Set up initial conditions for demo
% We're not using symbol math because we can't easily take the pseudoinverse.
wx = 0;
wy = 0;
rx = 0;
ry = 1;
dwx = 0;
dwy = 0;
drx = 0;
dry = 0;
fwx = 0;
fwy = 0;
frx = 0;
fry = 0;

%% System parameters
mw = 1;
mr = 1;
g = 10;


%% Mass matrix
MM = diag([mw mw mr mr]);

%% Constraint matrices
A = [     0,     1,     0,     0;
      wx-rx, wy-ry, rx-wx, ry-wy ];
b = [ 0;
      -(dwx-drx)^2-(dwy-dry)^2];

%% External forces
F = [ fwx; fwy-g*mw; frx; fry-g*mr];

%% Unconstrained acceleration
a = inv(MM)*F;

%% Constraint force
Fc = MM^(1/2)*pinv(A*MM^(-1/2))*(b-A*a);

%% True acceleration
ddx = a + inv(MM) * Fc;

%ddwx = ddx(1);
%ddwy = ddx(2);
%ddrx = ddx(3);
%ddry = ddx(4);

%function [ddwx,ddwy,ddrx,ddry] = ...
%    udwadiademo(wx,wy,rx,ry,dwx,dwy,drx,dry,fwx,fwy,frx,fry)

% wx,wy,rx,ry,dwx,dwy,drx,dry,fwx,fwy,frx,fry
% 0,0,1,0,1,0,0,0,0,0,0,0