Code Generated Blocks for Seriallink return error "Index expression out of bounds" - Peter Corke Robotics Toolbox

Hi, I am trying to simulate a Seriallink manipulator in Simulink, and am attempting to solve for the coriolis matrix at a current configuration and velocity. For this I have decided to use the Peter Corke Robotics toolbox as I saw in the docs that it supports code generation. In fact, it has the capability to generate blocks for direct use in simulink (as stated in the following link: https://www.petercorke.com/RTB/r9/html/CodeGenerator.html):

% The Simulink block library containing robot-specific blocks can be
% opened by
open robot/robotslib.slx
% and the blocks dragged into your own models.

This is the Seriallink manipulator and code generation .m file.

%mod DH
% Peter corkes: theta, d, a, alpha
dhparams = [0   0	 0   pi/2;
            0	0    0   pi/2;
            0   0    0.5 -pi/2;
            0   0    0.5  0];
L(1) = Link(dhparams(1,:), 'modified');
L(1).m = 0.5; %mass
L(1).r = [0 0 0]; %COM
L(1).I = [1 0 0; 
          0 1 0;
          0 0 1];
L(1).Jm = 0;  % this is motor inertia
      
L(2) = Link(dhparams(2,:), 'modified');
L(2).m = 1; %mass
L(2).r = [0.325 0 0]; %COM
L(2).I = [1 0 0; 
          0 1 0;
          0 0 1];
L(2).Jm = 0;
      
L(3) = Link(dhparams(3,:), 'modified');
L(3).m = 1; %mass
L(3).r = [0.2175 0 0]; %COM
L(3).I = [1 0 0; 
          0 1 0;
          0 0 1];
L(3).Jm = 0; 

T_tool = [1  0  0  0.5;
          0  1  0  0;
          0  0  1  0;
          0  0  0  1];

myrobot2 = SerialLink(L, 'name', 'myrobot2', 'gravity',[0,0,-9.81], 'tool', T_tool); %gravity compensated
myrobot2 = myrobot2.nofriction('all');

% and to generate all the Simulink blocks
cg = CodeGenerator(myrobot2);
cg.geneverything();

This generates a .slx (Simulink) file which contains blocks such as inertia, coriolis, etc.

When using these blocks however, I am obtaining the following error (specifically for the coriolis block in this case):

Index expression out of bounds. Attempted to access element 2. The valid range is 1-1.
More information

Function 'coriolis/coriolis_row_3' (#1334.164.165), line 7, column 12:
"2"
Launch diagnostic report.

As well as more errors concerning the functions to calculate the coriolis matrix (coriolis_row_1, coriolis_row_2, coriolis_row_3).

I could not find any examples where these code generated blocks are used in Simulink.

NOTE: In Matlab the functions do work. For example doing the following returns a result (where q and qd are defined):

myrobot2.coriolis(q,qd)

Does anyone have a solution? Any input would be much appreciated. Thank you in advance.

Answer 1

[Aastav Sen]

I found a work around!

Unfortunately I am unable to use the generated Simulink blocks directly (with the drag and drop simplicity) or to load the symbolic expressions to calculate the inertia, coriolis, etc. However, by loading the symbolic expressions into the workspace and opening the variable (which is a struct) I can copy the generated symbolic expressions directly. Thus, making a custom Matlab Function block and computing my copied symbolic expressions I can compute the unknown matrices.

So to calculate the coriolis matrix I have a custom Matlab Function block as follows (the symbolic expressions will differ for your robot):

function C = compute_C_matrix(q1, q2, q3, qd1, qd2, qd3)
%cor1 = load('coriolis_row_1')
%cor2 = load('coriolis_row_2')
%cor3 = load('coriolis_row_3')
c11 = (7569*qd3*cos(q3)*sin(q3))/320000 - (87*qd3*sin(q3))/1600 - (7569*qd2*sin(2*q2)*cos(q3)^2)/320000 - (569*qd2*sin(2*q2))/3200 - (87*qd2*sin(2*q2)*cos(q3))/800 - (87*qd3*cos(2*q2)*sin(q3))/1600 - (7569*qd3*cos(2*q2)*cos(q3)*sin(q3))/320000;
c12 = (87*qd2*cos(q2)*sin(q3))/800 - (167569*qd3*sin(q2))/320000 - (7569*qd1*cos(2*q3)*sin(2*q2))/640000 - (121369*qd1*sin(2*q2))/640000 - (87*qd1*sin(2*q2)*cos(q3))/800 + (7569*qd2*sin(2*q3)*cos(q2))/320000 + (7569*qd3*cos(2*q3)*sin(q2))/320000;
c13 = (7569*qd2*cos(q3)^2*sin(q2))/160000 - (87*qd3*cos(q2)*sin(q3))/800 - (87*qd1*cos(q2)^2*sin(q3))/800 - (87569*qd2*sin(q2))/160000 + (7569*qd1*cos(q3)*sin(q2)^2*sin(q3))/160000;
c21 = (sin(q2)*(80000*qd3 + 7569*qd3*cos(q3)^2 + 56900*qd1*cos(q2) + 17400*qd3*cos(q3) + 34800*qd1*cos(q2)*cos(q3) + 7569*qd1*cos(q2)*cos(q3)^2))/160000;
c22 = -(87*qd3*sin(q3)*(87*cos(q3) + 200))/160000;
c23 = (qd1*sin(q2))/2 - (87*qd2*sin(q3))/800 + (87*qd1*cos(q3)*sin(q2))/800 - (7569*qd2*cos(q3)*sin(q3))/160000 + (7569*qd1*cos(q3)^2*sin(q2))/160000;
c31 = (87*qd1*cos(q2)^2*sin(q3))/800 - (7569*qd1*cos(q3)*sin(q3))/160000 - (87*qd2*cos(q3)*sin(q2))/800 - (qd2*sin(q2))/2 - (7569*qd2*cos(q3)^2*sin(q2))/160000 + (7569*qd1*cos(q2)^2*cos(q3)*sin(q3))/160000;
c32 = (87*qd2*sin(q3))/800 - (qd1*sin(q2))/2 - (87*qd1*cos(q3)*sin(q2))/800 + (7569*qd2*cos(q3)*sin(q3))/160000 - (7569*qd1*cos(q3)^2*sin(q2))/160000;
c33 = 0;

C = [c11 c12 c13;
     c21 c22 c23;
     c31 c32 c33];

I obtain the symbolic expressions by loading them from their respective .mat files as done in the following code (run in the Command Window), these files are generated by the code generation procedure outlined in the question above:

cor1 = load('coriolis_row_1')
cor2 = load('coriolis_row_2')
cor3 = load('coriolis_row_3')

Then I double click on cor1, cor2 and cor3 (structures corresponding to rows of the coriolis matrix) to find expressions for each term in the coriolis matrix.

Hope this helps =)