Apparently the following lines:
%Setting State to random values
q = rand(32,1);dq = rand(32,1);dxb = rand(6,1);
needs to be changed into:
%Setting State to random values
q = rand(25,1);dq = rand(25,1);dxb = rand(6,1);
to complete the test successfully.
Apparently the following lines: %Setting State to random values q = rand(32,1);dq = rand(32,1);dxb = rand(6,1); needs to be changed into: %Setting State to random values q = rand(25,1);dq = rand(25,1);dxb = rand(6,1); to complete the test successfully.