How to use a MATLAB Function with Robotic System Toolbox in Simulink?

Hi everyone.

I'm designing a backstepping control of a 7 DoF robotic arm which I created and imported in MATLAB as RigidBody Tree object. Considering the control law, there is the term so I computed the Coriolis matrix in real-time knowing the current configuration of my robot q and the Mass matrix . I did it by the Christoffel parametrization considering numerical data that comes from sequential integrations but it is very slow.

As alternative approach to see if it is lighter than the previous one, I thought to solve the problem using the Corke's method (of the RTB) which considers to compute single termes of due to and the pair product (further information here). Instead of set several Inverse Dynamics Simulink and MATLAB Function Blocks, can I define an external MATLAB function which contains the inverse dynamics command in a for loop? I think it should be more efficienty and elegant. In other words, how can I use this MATLAB Function below in Simulink? Is "Interpreted MATLAB Function" block the proper one?

function C = computeCoriolisMatrix(q, dq, robot_free)
N = length(q);
C = zeros(N, N);
Csq = C;
    for j = 1:N
        QD = zeros(1,N);
        QD(j) = 1;
        tau = inverseDynamics(robot_free, q, QD);
        Csq(:,j) = Csq(:,j) + tau';
    end
    
    for j = 1:N
        for k = j+1:N
            % find a product term  qd_j * qd_k
            QD = zeros(1,N);
            QD(j) = 1;
            QD(k) = 1;
            tau = inverseDynamics(robot_free, q, QD);
            C(:,k) = C(:,k) + (tau' - Csq(:,k) - Csq(:,j)) * dq(j);
        end
    end
    C = C + Csq * diag(dq);
    
end

Thanks eveyone, Valerio

[Gabriele Gualandi]

I would use the "Matlab function" block (not the Interpreted one). Then, I would compile it as an S-function (that is, C++ code).

Note that this process involves all the constraints deriving from writing S-Functions (e.g., you need to initialize your data structures with zeros). In particular, I would avoid that "inverseDynamics" invocation and past all your code in your Matlab function block (to have a monolithic block).

[Valerio Novelli]

Thank you for answering me.

Searching for a solution, I implemented it by the "Interpreted MATLAB Fcn" defining the robot_free object as a permanent variable inside the function's filed. I know that is not the best solution and that implementing an 2-Level MATLAB S-Function should be the best way to proceed, but I'n not so expert. Thus, I'm gathering information to learn the basics about this last way (in particular how to insert the RigidBody Tree Object and use the inverseDynamics() function of RST, which both have C/C++ Code Generation support) but, could this method increase significantly the efficienty in the real-time computation?

Meanwhile, here below I posted my last version of the code to embed in the "Interpreted MATLAB Fnc" Simulink block. It works for me but, as I told, it's not so fast as I would want.

function C = computeCoriolisMatrix_v2(qdq)
% Definizione della dell'oggetto RigidBody Tree come variabile persistente
% per non doverla ricreare ogni voltea che viene richiamata la function
persistent robot_free
defaultScene_fullfilepath = strcat('C:\Users\valer\MATLAB Drive\', ...
    'Published\Controllo dei Robot\Canadarm2_dh_dragonPlaced_v4\urdf\', ...
    'Canadarm2_dh_dragonPlaced_v4.urdf');
robot_free = importrobot(defaultScene_fullfilepath);
robot_free.DataFormat = 'row';

% Demux della variabile in ingresso
q = qdq(1:length(qdq)/2);
dq = qdq(length(qdq)/2+1:end);

% Calcolo della matrice dei termini centripeti di Coriolis con il metodo di
% Corke
N = length(q);
C = zeros(N, N);
Csq = C;
    for j = 1:N
        QD = zeros(1,N);
        QD(j) = 1;
        tau = inverseDynamics(robot_free, q, QD);
        Csq(:,j) = Csq(:,j) + tau';
    end
    
    for j = 1:N
        for k = j+1:N
            QD = zeros(1,N);
            QD(j) = 1;
            QD(k) = 1;
            tau = inverseDynamics(robot_free, q, QD);
            C(:,k) = C(:,k) + (tau' - Csq(:,k) - Csq(:,j)) * dq(j);
        end
    end
    C = C + Csq * diag(dq);
end

Answer 1

[siyong xu]

I think that you can use the 'MATLAB system ' block, you can define a system object to compute the coriolis matrix.

There are two advatages:

Firstly, more efficient. You can invoke the internal funtions of Robotics System Toolbox which is more efficient than the outer function. And you can invoke the internal properities of rigidbodyTree object, which is convenient for coding.

Secondly, you can import the rigidbodyTree Object of a parameter of the block, which avoids invoking the importrobot function.

But the 'MATLAB system ' block is more complicate and you may pay some time to learn how to use it.

By the way, if you find a more efficient algorithm to compute the Coriolis Matrix, please let me know, and I will be appreciate. My email is [e-mail]