Is it possible to call a rigidBodyTree robot model in a Matlab function in Simulink?

Hi everybody,

I am trying to use the Robotic System Toolbox functions inside a Matlab function in Simulink.

I am having troubles in importing the rigidBodyTree model in the Matlab function.

How can I manage this issue?

Thank you very much in advance!

[Aastav Sen]

I second this question! (https://www.mathworks.com/matlabcentral/answers/533538-how-to-load-rigidbodytree-from-workspace-into-simulink)

I am currently solving this problem by recreating the ridigbodyTree in the MATLAB Function block itself. However, this means that it is created in every loop and that is not efficient or desireable at all. Although it is a workaround, I am unsure if directly obtaining a 'ridigbodyTree' type from the workspace is possible and if anybody knows about this I would really appreaciate it if you helped us out.

[Lorenzo Scalera]

I have solved the problem by creating the RigidBodyTree in the Matlab function itself. You can create the RigidBodyTree by initializing a persistent variable, in which you save your RigidBodyTree. In this way, you can create it only once and not at every loop.

[Valeria Leto]

Hi Lorenzo! could you post your code? I have the same problem but I didn't understand how to fix it.

[Lorenzo Scalera]

Ciao Valeria, I have created the robot RigidBodyTree in a way similar to what Yiping did.

I have defined a persistent variable, which is initialized only once when the code is run at the first time. Then, "robot" is automatically kept in memory during code execution.

"createRigidBodyTree" is a custom function in which I have built the manipulator RigidBodyTree starting from the DH parameters, as explained in this link:

https://it.mathworks.com/help/robotics/ref/rigidbodytree.html

I hope it can help!

persistent robot
if isempty(robot)
    robot=createRigidBodyTree;
end

function robot = createRigidBodyTree

dhparams = [...
    
robot = rigidBodyTree("MaxNumBodies",8,"DataFormat","row");

body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');
... (please see the link)
    
end

Answer 1

[Yiping Liu]

Lorenzo, the persistent variable is the correct way.

You should also check out the rigid body tree algorithm Simulink blocks in RST:

Manipulator Algorithm Design — Blocks

An example of using the persistent variable (NOTE to get a robot inside a function, starting in 21a, you can also use loadrobot command or the new API on rigidBodyTree class called writeAsFunction, both supporting codegen)

function [q, solInfo] = computeIK(T, qini, solTol, gradTol)
%COMPUTEIK
%  input:
%  Q - 4x4 homogenerous matrix representing end-effector pose
%  qini - initial guess for joint configurations (a vector)
%  solTol - Solution Toleraance
%  gradTol - Gradient Tolerance
persistent ik
if isempty(ik)
    robot = robotics.manip.internal.robotplant.puma560DH(); % Since 21a, there are better/cleaner ways to create a robot inside a function.
    robot.DataFormat = 'column';
    ik = inverseKinematics('RigidBodyTree', robot);
    ik.SolverParameters.MaxIterations = 100; 
    ik.SolverParameters.AllowRandomRestart = false;
end

ik.SolverParameters.SolutionTolerance = solTol;
ik.SolverParameters.GradientTolerance = gradTol;
[q, solInfo] = ik('L6', T, ones(1,6), qini);

end

% codegen command:
% codegen computeIK.m -args {eye(4), ones(1,13), 0, 0}

[Valeria Leto]

Hi! could you explain it better giving an example?

[Yiping Liu]

I have updated the answer to give you an example.

[Karsh Tharyani]

Valeria, you can also refer to this documented example in R2021a