%% Kinova Robot in a Work Cell
%
% This example reads a virtual model of a work cell and the Kinova(R) Gen3
% robot and displays it in the F3D Viewer. The models of the cell and robot
% had been previously imported from URDF models using 3D World Editor
% import feature.
%
% There is an OnActivate callback defined for the control panel. Double
% click on it to animate the robot.
%
% This model is suitable for exploring various virtual world environment
% and visual properties. Select the World Inspect option from the context
% menu that is available on the right mouse button click.
%
% The work cell model is based on the 2017 ARIAC competition organized by
% the NIST Institute:
% https://www.nist.gov/el/intelligent-systems-division-73500/agile-robotics-industrial-automation-competition
%
% Kinova Gen3 URDF model Copyright 2018 Kinova inc.
% Kinova is a registerted trademark of Kinova inc.

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

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

% Units and coordinate system
FW.AngleUnits = 'deg';
FW.CoordinateSystem = 'vrml';

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

FW.load('f3dex_workcell.f3d');

% Add callback to control panel
Ctrl = findBy(FW.Root, 'Name', 'Actor22');
Ctrl.Color = 0;
Ctrl.OnActivate = @ActivateRobot;

function ActivateRobot(Actor, ~)
    L0 = findBy(Actor.ParentWorld.Root, 'Name', 'robot_robot_Base_Link', 'first');
    L1 = findBy(Actor.ParentWorld.Root, 'Name', 'robot_HalfArm1_Link', 'first');
    Actor.TextureMapping.Blend = [10 1 1];
    interpolate(L1, 'Rotation(3)', 90, 'sweep');
    interpolate(L0, 'Rotation(2)', 90, 'hill', 2.0);
    interpolate(L1, 'Rotation(3)', 0, 'sweep');
    Actor.TextureMapping.Blend = 1;
end