Passing a rigidBodyTree object as a parameter in simulink MATLAB Function Block

Hi,

I'm trying to pass a rigidBodyTree object in MATLAB Function Block (simulink) so that th function looks like the following:

function out = fcn(var1, q0, robot)
''' 
.......
'''
T = getTransform(robot, q0, robot.BodyNames{end}, robot.BaseName);

''' 
.......
'''
end

I have defined robot in the Matlab Workspace

I have tried to change in the Block Explore the Scope of the robot to parameter. But i still get Error

Expression 'robot' for initial value of data 'robot' must evaluate to a logical or supported numeric type.

What should I do?

If that is not possible, how can I create a Matlab function block that takes parameters from the workspace

as 'Forward Dynamics' block from the Robotics System ToolBox?

[Lorenzo Scalera]

Hi, I am having the same problem.

Did you solve this issue?

Thank you very much in advance.

Answer 1

[Francesco Ciriello]

An easy way to make a rigidBodyTree compatible with codegen, so that it can be used a MATLAB Function block in Simulink, is to run the writeAsFunction function on to the rigidBodyTree in the command window:

writeAsFunction(myRigidBodyTree, "rbtForCodegen")

This creates a function in your current directory called rbtForCodegen.

You can then call the rbtForCodegen function to load your robot in a MATLAB Function block, for example,

function tform = myGetTransform(config) %#codegen
% A codegenable function that computes forward kinematics on a
% rigidBodyTree. 

persistent robot configState

% setup for loading RBT
if isempty(robot)
    robot = rbtForCodegen();
    configState = robot.homeConfiguration;
end

for i = 1:length(configState)
    configState(i).JointPosition = config(i);
end

linkNumber = 5;   % an index for the link in robot.BodyNames
tform = getTransform(robot, configState, robot.BodyNames{linkNumber});

end

You can use many other functions from the Robotic System Toolbox in a similar way.

Answer 2

[Shravista Kashyap]

I have tried the same way as you did that is passing rigidBodyTree object. But, we cannot pass rigidBodyTree objects for many reasons as it is not the data type that is supported by Simulink. To check the data types that are supported by Simulink, check this URL. https://www.mathworks.com/help/simulink/ug/data-types-supported-by-simulink.html

We can't even load this rigidBodyTree from .mat type file. Or even we cannot use masks to pass such arguments. The reason is the same that this data type is not supported. The same reason for giving inputs in MATLAB function block. The data types that can be specified through model explorer, are the same that specified at the URL above. In addition to this, we can specify bus objects and Enum. but still, problems.

One way to solve this problem is to use MATLAB system objects. This way you can use classes that depend on MATLAB.system and other system objects. refer https://www.mathworks.com/help/simulink/ug/system-design-in-simulink-using-system-objects.html. In Simulink, this is possible using the MATLAB System block.

I will show here how I adopted this method to pass on rigidBodyTree objects to the class. But, remember that these are imported, handle classes.

See this a way that I adopted to implement my function.

But Remember, Simulation cannot run this code with Code generation. If you need to simulate with code generation, then you may need other methods where you do not import handle classes that cannot use for code generation. Or else staff from MathWorks can help us.

classdef derivativeJacobian < matlab.System & matlab.system.mixin.Propagates
    % derivativeJacobian Block evaluates the time derivative of jacobian

    % Copyright 2018 The MathWorks, Inc.

    %#codegen
    
    properties(Nontunable)
        %robot - Rigid Body Tree
        robot = 0;
    end
        
    methods
        ..........................................................
    end
    methods(Access = protected)
        
        function setupImpl(obj)
            %setupImpl Perform one-time calculations, such as computing constants
            
            % Assign the robot internal property by instantiating from the structure      
            obj.robot.DataFormat = 'column';
        end
        function Jdt = stepImpl(obj,q,qdt)
            Tf = cell(7,1);
            pos = zeros(3,7);
            for i1 = 1:7
                Tf{i1} = obj.robot.getTransform(q,obj.robot.BodyNames{i1});
                pos(:,i1) = Tf{i1}(1:3,4);
            end
            ..............................
        end
        
        ..........................................
    end
    methods(Access = protected, Static)
        function header = getHeaderImpl
            ............................................
        end
    end