%% Visualization of rigidBodyTree Objects
%
% This example shows visualization of manipulator robots that are available
% as Robotics System Toolbox(TM) rigidBodyTree objects.
%
% After clicking on the user-defined button on the left edge of the F3D
% Viewer, a dialog box appears that allows you to select from the
% available manipulator robot models. The selected robot is then
% displayed in both MATLAB figure and F3D Viewer.
%
% The structure of the selected robot is displayed in the Command window
% using the rigidBodyTree method showdetails. Robot parts can be then
% accessed using the findBy(FW.Root, ) or findByID(FW.Root, ) methods. The
% robot part names in the F3D Viewer correspond to those in the rigidBodyTree
% object, their IDs correspond to their rigidBodyTree Idx numbers.
%
% You can select one or more robot parts using Ctrl + left mouse button and
% then click on the "C" user button to make them colored.

%   Copyright 1998-2021 HUMUSOFT s.r.o.

%%
% Initialize the 3D Foundation object
FW = F3D.World(mfilename);
initialize(FW);

% Set the viewport properties
VP = FW.Viewports.Main;
VP.Title = mfilename;
Path = fileparts(which(mfilename));
setLogo(VP, fullfile(Path, 'data', 'MatlabLogo.png'));
VP.OnClose = 'close(findall(0, ''tag'', ''F3D_RBT''));';

% Assign user buttons
VP.Button5 = F3D.Type.Button([0 0 0], '->', 'Load robot', @importRobot);
VP.Button3 = F3D.Type.Button([0 0 0], 'C', 'Colorize selected', @colorizeSel);

if isInteractive
    message(VP, ['Click on the [->] button to select and import a robot.' ,...
        newline newline 'Select robot part(s) using Ctrl + left mouse button,' , ...
        newline 'then click on the [C] button to color the selected parts.'
        ]);
else
    importRobot(FW, -1);
end

function importRobot(World, ButtonID)
    RobotNames = robotics.manip.internal.RobotList.allRobots;
    if ButtonID >= 0
        label(World.Viewports.Main, 'Please select a robot from the list in the MATLAB dialog box.')
        id = listdlg('ListString', RobotNames, 'SelectionMode', 'single', 'ListSize', [200 400]);
        label(World.Viewports.Main, '')
    else
        id = ceil(rand*numel(RobotNames));
    end
    if ~isempty(id)
        Robot = addChild(World.Root, 'Robot');
        removeChildren(Robot);
        RBT = loadrobot(RobotNames(id));
        show(RBT);
        set(gcf, 'Tag', 'F3D_RBT');
        showdetails(RBT);
        load(Robot, RBT, 10);
        propagate(Robot, 'Flat', 1);
    end
end

function colorizeSel(World, ~)
    Sel = findBy(World.Root, 'Selected', true);
    for i = 1:length(Sel)
        c = rand(1, 3);
        Sel(i).Color = c/max(c);
        Sel(i).Selected = false;
    end
end