classdef robotPlatform < handle
%robotPlatform Create robot platform in scenario
%
%   The robotPlatform object represents a robot platform in a given robot
%   scenario. Use the platform to define and track the trajectory of an
%   object in the scenario.
%
%   PLATFORM = ROBOTPLATFORM(NAME, SCENARIO) creates a platform with a
%   specified name NAME and adds it to the scenario, specified as a
%   robotScenario object. Specify the NAME argument as a string scalar. The
%   NAME argument sets the Name property.
%
%   PLATFORM = ROBOTPLATFORM(___, Name=Value) specifies options using one
%   or more name-value arguments. You can specify properties as name-value
%   arguments as well.
%
%   Name-Value Arguments:
%
%      BaseTrajectory              - Trajectory for robot platform base
%                                    motion, specified as a
%                                    waypointTrajectory or
%                                    polynomialTrajectory object. By
%                                    default, the platform is assumed to be
%                                    stationary and at the scenario origin.
%                                    To move the platform at each
%                                    simulation step of the scenario, use
%                                    the move object function.
%                                     Note: The robotPlatform object must
%                                           specify the same ReferenceFrame
%                                           property as specified in the
%                                           trajectory object.
%                                    Default: []
%      IsBinaryOccupied            - Occupied state of binary occupancy
%                                    map, specified as true or false. Set
%                                    the value as true if robot platform is
%                                    incorporated in the binary occupancy
%                                    map.
%                                    Default : False
%      InitialBaseAcceleration     - Initial acceleration of robot platform
%                                    base, specified as a vector of the
%                                    form [ax ay az]. Only specify this
%                                    name-value argument if not specifying
%                                    the BaseTrajectory property.
%                                    Default: [0 0 0]
%      InitialBaseAngularVelocity  - Initial angular velocity of robot
%                                    platform base, specified as a vector
%                                    of the form [wx wy wz]. The magnitude
%                                    of the vector defines the angular
%                                    speed in radians per second. The
%                                    xyz-coordinates define the axis of
%                                    clockwise rotation. Only specify this
%                                    name-value argument if not specifying
%                                    the BaseTrajectory property.
%                                    Default: [0 0 0]
%      InitialBaseOrientation      - Initial robot platform base
%                                    orientation, specified as a vector of
%                                    the form [w x y z], representing a
%                                    quaternion. Only specify this
%                                    name-value argument if not specifying
%                                    the BaseTrajectory property.
%                                    Default: [1 0 0 0]
%      InitialBasePosition         - Initial robot platform base position,
%                                    specified as a vector of the form [x y
%                                    z]. Only specify this name-value
%                                    argument if not specifying the
%                                    BaseTrajectory property.
%                                    Default: [0 0 0]
%      InitialBaseVelocity         - Initial velocity of robot platform
%                                    base, specified as a vector of the
%                                    form [vx vy vz]. Only specify this
%                                    name-value argument if not specifying
%                                    the BaseTrajectory property.
%                                    Default: [0 0 0]
%      InitialJointConfiguration   - Initial joint configuration of the
%                                    rigidBodyTree-based robot platform,
%                                    specified as an N-element vector. N is
%                                    the total number of joints associated
%                                    with the rigidBodyTree object.
%                                    Default: homeConfiguration
%      ReferenceFrame              - Reference frame for computing robot
%                                    platform motion, specified as "ENU" or
%                                    "NED", which matches any reference
%                                    frame in the robotScenario. All
%                                    platform motion is computed relative
%                                    to this inertial frame.
%                                    Default: "ENU"
%      RigidBodyTree               - Rigid body tree robot platform,
%                                    specified as a rigidBodyTree object.
%                                    Default: []
%      StartTime                   - Initial time of the platform
%                                    trajectory, specified as a scalar in
%                                    seconds.
%                                    Default: 0
%      Collision                   - Add collision object to platform mesh,
%                                    specified as false, "default", "mesh",
%                                    "capsule", or externally created
%                                    collision object. The rigidBodyTree
%                                    based platform accepts externally
%                                    created collision mesh only for base
%                                    body.
%                                     - false  - Keeps platform mesh
%                                     collision object free. Clear existing
%                                     collision for rigidBodyTree-based
%                                     platform.
%                                     - "default" - Keeps existing
%                                     collision mesh for
%                                     rigidBodyTree-based platform. For
%                                     other platforms, keeps platform mesh
%                                     collision free.
%                                     - "mesh" - Fits collision object such
%                                     as collisionBox, collisionCylinder,
%                                     collisionSphere, and collisionMesh
%                                     object based on platform mesh type.
%                                     - "capsule" - Fits collision capsule
%                                     object with platform mesh.
%                                     - Externally created collision object
%                                     of type: collisionCapsule,
%                                     collisionBox, collisionCylinder,
%                                     collisionSphere, and collisionMesh.
%                                    Default: "default"
%      CollisionOffset             - Transformation of collision mesh
%                                    relative to the platform mesh,
%                                    specified as a 4-by-4 homogeneous
%                                    transformation matrix. Use the
%                                    CollisionOffset input for
%                                    rigidBodyTree-based platforms only
%                                    when the Collision input is specified
%                                    as an externally created collision
%                                    object.
%                                    Default: eye(4)
%
%   robotPlatform properties:
%      Name                - Identifier for robot platform
%      ReferenceFrame      - Reference frame for computing robot platform
%                            motion
%      BaseTrajectory      - Trajectory for base of robot platform
%      BaseMesh            - Robot platform base body mesh
%      BaseMeshColor       - Robot platform base body mesh color
%      BaseMeshTransform   - Transform between robot platform base body and
%                            mesh frames
%      RigidBodyTree       - Rigid body tree robot platform
%      IsBinaryOccupied    - Status of robot platform in binary occupancy
%                            map
%      Sensors             - Sensors mounted on robot platform
%      CollisionMesh       - Robot platform collision mesh
%      CollisionMeshOffset - Transformation between robot platform body
%                            mesh and collision mesh
%
%   robotPlatform methods:
%      move                - Move robot platform in scenario at current
%                            simulation step
%      read                - Read robot platform motion vector
%      updateMesh          - Update robot platform body mesh
%      attach              - Attach target robot platform to source robot
%                            platform
%      detach              - Detach target robot platform from source robot
%                            platform
%      checkCollision      - Check collision between robot platform and
%                            target bodies
%
%   Example:
%
%      % Create scenario with ground plane.
%      scenario = robotScenario(UpdateRate=1, StopTime=5);
%      addMesh(scenario,"Plane", Size=[5 3], Position=[2 0 0],...
%                         Color=[0.2 0.2 0.2]);
%
%      % Create waypoint trajectory.
%      traj = waypointTrajectory(...
%                         SampleRate=10, ...
%                         TimeOfArrival=[0 2 5],...
%                         Waypoints=[1 0 0; 2 0 0; 3 0 0],...
%                         ReferenceFrame="ENU");
%
%      % Create rigidBodyTree with loadrobot.
%      robot = loadrobot("amrPioneer3AT");
%
%      % Create robot platform with trajectory and rigidBodyTree.
%      platform = robotPlatform("TurtleBot", scenario, ...
%                         BaseTrajectory=traj, RigidBodyTree=robot);
%
%      % Setup the scenario and gather initial platform pose.
%      setup(scenario);
%      motion = read(platform);
%
%      % Visualize the scene.
%      ax = show3D(scenario);
%
%      % Create a rate object running at 1 Hz.
%      r = rateControl(1);
%
%      % Advance the scene, gather platform's motion and visualize.
%      while advance(scenario)
%           motion = read(platform);
%           % visualize collision mesh along with visual mesh.
%           show3D(scenario, Parent=ax, Collisions="on");
%           % Pause Simulation for visualization.
%           waitfor(r);
%      end
%
%   See also robotScenario, robotSensor

%   Copyright 2021-2022 The MathWorks, Inc.

    properties (Dependent, SetAccess = private)
        %Name Identifier for robot platform
        %   Identifier for the robot platform, specified as a string scalar
        %   or character vector. The name must be unique within the
        %   scenario.
        Name

        %ReferenceFrame Reference frame for computing robot platform motion
        %   Reference frame for computing robot platform motion, specified
        %   as "ENU" or "NED", which matches any reference frame in the
        %   robotScenario. The object computes all platform motion relative
        %   to this inertial frame.
        %
        %   Default: "ENU"
        ReferenceFrame

        %BaseTrajectory Trajectory for robot platform base motion
        %   Trajectory for robot platform base motion, specified as a
        %   waypointTrajectory or polynomialTrajectory object. By default,
        %   the object assumes the base of the platform is stationary and
        %   at the scenario origin. When specified as a waypointTrajectory
        %   or polynomialTrajectory object, base of the platform is moved
        %   along the trajectory during the scenario simulation. To move
        %   the platform at each simulation step of the scenario, use the
        %   move object function.
        %
        %   Default: []
        BaseTrajectory

        %BaseMesh Robot platform base body mesh
        %   Robot platform base body mesh, specified as an
        %   extendedObjectMesh object. The body mesh describes the 3-D
        %   model of the platform for visualization purposes. The body mesh
        %   is used to generate 3-D point cloud. The default mesh is a
        %   cuboid of the form [xlength ylength zlength] in meters.
        %
        %   Default: [1 0.5 0.3]
        BaseMesh

        %BaseMeshColor Robot platform base body mesh color
        %   Robot platform base body mesh color when displayed in the
        %   scenario, specified as an RGB triplet
        %
        %   Default: [1 0 0]
        BaseMeshColor

        %BaseMeshTransform Transform between robot platform base body and
        %mesh frames
        %   Transform between robot platform base body and mesh frame,
        %   specified as a 4-by-4 homogeneous transformation matrix that
        %   maps points in the platform mesh frame to points in the body
        %   frame.
        %
        %   Default: eye(4)
        BaseMeshTransform

        %RigidBodyTree Rigid body tree robot platform
        %   Rigid body tree robot platform, specified as a rigidBodyTree
        %   object.
        %
        %   Default: []
        RigidBodyTree

        %IsBinaryOccupied Status of robot platform in binary occupancy map
        %   Status of robot platform in binary occupancy map, specified as
        %   true or false.
        %
        %   Default: false
        IsBinaryOccupied

        %CollisionMesh Robot platform collision mesh
        %   Collision mesh associated with robot platform body mesh,
        %   specified as a collision object. The supported collision
        %   object types are: collisionBox, collisionCapsule,
        %   collisionCylinder, collisionMesh, and collisionSphere.
        %
        %   Default: []
        CollisionMesh

        %CollisionMeshOffset Transform between robot platform mesh and
        %collision mesh
        %   Transform between robot platform mesh and collision mesh,
        %   specified as a 4-by-4 homogeneous transformation matrix.
        %
        %   Default: eye(4)
        CollisionMeshOffset
    end

    properties (SetAccess = {?robotPlatform, ?robotSensor})
        %Sensors Sensors mounted on robot platform
        %   Sensors mount on robot platform, specified as an array of
        %   robotSensor objects.
        %
        %   Default: []
        Sensors
    end

    properties (Access={?robotPlatform, ...
                        ?robotSensor,...
                        ?robotics.SensorAdaptor, ...
                        ?robotics.internal.scenario.Sensor , ...
                        ?uav.internal.scenario.SensorInterface , ...
                        ?robotics.internal.scenario.sensor.SensorAdaptor, ...
                        ?uav.internal.scenario.sensor.utils.PointCloudGeneratorUtils,...
                        ?matlab.unittest.TestCase})
        %ScenarioImpl Implementation of the robot scenario
        ScenarioImpl

        %PlatformImpl Implementation of the robot platform
        PlatformImpl
    end

    methods

        function traj = get.BaseTrajectory(obj)
            traj = obj.PlatformImpl.BaseTrajectory;
        end

        function name = get.ReferenceFrame(obj)
            name = obj.PlatformImpl.ReferenceFrame;
        end

        function name = get.Name(obj)
            name = obj.PlatformImpl.Name;
        end

        function mesh = get.BaseMesh(obj)
            mesh = obj.PlatformImpl.BaseMesh.Mesh;
        end

        function color = get.BaseMeshColor(obj)
            color = obj.PlatformImpl.BaseMesh.Color;
        end

        function transform = get.BaseMeshTransform(obj)
            transform = obj.PlatformImpl.BaseMesh.Transform;
        end

        function robot = get.RigidBodyTree(obj)
            robot = obj.PlatformImpl.RigidBodyTree;
        end

        function isBinaryOccupied = get.IsBinaryOccupied(obj)
            isBinaryOccupied = obj.PlatformImpl.OccupancyMapDetails.IsBinaryOccupied;
        end

        function collisionMesh = get.CollisionMesh(obj)
            collisionMesh = obj.PlatformImpl.CollisionMeshDetails.Mesh;
        end

        function offset = get.CollisionMeshOffset(obj)
            offset = obj.PlatformImpl.CollisionMeshDetails.Offset;
        end
    end

    methods
        function obj = robotPlatform(name, scenario, varargin)
        %robotPlatform

        % validate inputs
            validateattributes(scenario, "robotScenario", "scalar", "robotPlatform", "scenario");
            name = convertCharsToStrings(name);
            validateattributes(name, ["char", "string"], "scalartext", "robotPlatform", "name");
            if any(strcmp(name, [scenario.Platforms.Name]))
                error(message("robotics:robotscenario:scenario:PlatformNameNotUnique", name));
            end

            % parse inputs
            p = inputParser;
            p.addParameter("ReferenceFrame", uav.internal.scenario.Scene.ENUFrameName, ...
                           @(x)validateattributes(x, ["char", "string"], "scalartext", "robotPlatform", "ReferenceFrame"));
            p.addParameter("BaseTrajectory", [],...
                           @(x)validateattributes(x, ["waypointTrajectory", "polynomialTrajectory"], "scalar", "robotPlatform", "BaseTrajectory"));
            p.addParameter("InitialBasePosition", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "robotPlatform", "InitialBasePosition"));
            p.addParameter("InitialBaseOrientation", [1 0 0 0]);
            p.addParameter("InitialBaseVelocity", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "robotPlatform", "InitialBaseVelocity"));
            p.addParameter("InitialBaseAcceleration", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "robotPlatform", "InitialBaseAcceleration"));
            p.addParameter("InitialBaseAngularVelocity", [0 0 0], ...
                           @(x)validateattributes(x, "numeric", {"nrows", 1, "ncols", 3, "2d"}, "robotPlatform", "InitialBaseAngularVelocity"));
            p.addParameter("InitialJointConfiguration", []);
            p.addParameter("StartTime", scenario.ScenarioImpl.StartTime, ...
                           @(x)validateattributes(x, "numeric", {">=", scenario.ScenarioImpl.StartTime}, "robotPlatform", "StartTime"));
            p.addParameter("RigidBodyTree", {},...
                           @(x)validateattributes(x, "rigidBodyTree", "scalar", "robotPlatform", "RigidBodyTree"));
            p.addParameter("IsBinaryOccupied", false, ...
                           @(x)validateattributes(x,["logical", "numeric"], ["scalar", "binary"], "robotPlatform", "IsBinaryOccupied"));
            p.addParameter("Collision", "default");
            p.addParameter("CollisionOffset", eye(4),...
                           @(x)validateattributes(x, "numeric", {"finite","2d", "nrows", 4, "ncols", 4, "real"}, "robotPlatform", "CollisionOffset"));
            p.parse(varargin{:});

            if scenario.IsRunning
                error(message("robotics:robotscenario:scenario:CannotAddPlatformToRunningScenario"));
            end

            obj.ScenarioImpl = scenario.ScenarioImpl;
            traj = p.Results.BaseTrajectory;

            % validate and get valid collision input
            collisionMeshInput = ...
                robotics.internal.scenario.collision.CollisionObjectUtil.getValidCollisionInput(...
                p.Results.Collision, true);

            collisionInput.Mesh = collisionMeshInput;
            collisionInput.Offset = p.Results.CollisionOffset;

            % validate initial joint configuration
            jointConfig = ...
                robotics.internal.scenario.rigidbody.RigidBodyTreeUtil.getInitialJointConfiguration(...
                p.Results.RigidBodyTree, p.Results.InitialJointConfiguration);

            if isempty(traj)
                % validate initial base orientation
                q = robotics.internal.validation.validateQuaternion(...
                    p.Results.InitialBaseOrientation, ...
                    "robotPlatform", "InitialBaseOrientation");

                % initial motion vector
                motion = [double(p.Results.InitialBasePosition), ...
                          double(p.Results.InitialBaseVelocity), ...
                          double(p.Results.InitialBaseAcceleration), ...
                          double(q), ...
                          double(p.Results.InitialBaseAngularVelocity)];

                obj.PlatformImpl = robotics.internal.scenario.Platform(...
                    obj.ScenarioImpl, ...
                    p.Results.ReferenceFrame, name, ...
                    double(p.Results.StartTime), motion, p.Results.RigidBodyTree,...
                    p.Results.IsBinaryOccupied, collisionInput, jointConfig);
            else
                % error-out if platform and trajectory reference frames are
                % mismatched
                if ~strcmp(p.Results.ReferenceFrame, traj.ReferenceFrame)
                    error(message("robotics:robotscenario:scenario:MismatchTrajectoryReferenceFrame", ...
                                  traj.ReferenceFrame, p.Results.ReferenceFrame));
                end
                obj.PlatformImpl = robotics.internal.scenario.Platform(...
                    obj.ScenarioImpl, p.Results.ReferenceFrame, name, ...
                    double(p.Results.StartTime), traj, p.Results.RigidBodyTree,...
                    p.Results.IsBinaryOccupied, collisionInput, jointConfig, varargin);
            end
            obj.ScenarioImpl.addPlatform(obj.PlatformImpl);
            obj.Sensors = robotSensor.empty;
            scenario.Platforms(end+1) = obj;
        end

        function updateMesh(obj, type, varargin)
        %updateMesh Update body mesh of robot platform
        %   UPDATEMESH(PLATFORM,TYPE,NAME=VALUE) updates the body mesh of
        %   the robot platform by specifying the mesh type as "Cuboid",
        %   "Custom", "GroundVehicle" or "RigidBodyTree" and specifies
        %   additional options using name-value arguments.
        %
        %   Name-Value arguments:
        %
        %       Color            - Robot platform body mesh color,
        %                          specified as a RGB triplet, except for
        %                          RigidBody mesh.
        %                          Default : [1 0 0]
        %       Faces            - Faces of the custom robot platform mesh,
        %                          specified as an N-by-3 matrix of
        %                          positive integers. The three elements in
        %                          each row are the indices of the three
        %                          points in the vertices forming a
        %                          triangle face. N is the number of faces.
        %       IsBinaryOccupied - Occupied state of binary occupancy map,
        %                          specified as true or false. Set the
        %                          value as true if robot platform is
        %                          incorporated in the binary occupancy
        %                          map.
        %                          Default : False
        %       Object           - Rigid body tree robot platform,
        %                          specified as a rigidBodyTree object.
        %                          Default : []
        %       Offset           - Transformation of mesh relative to the
        %                          body frame, specified as a 4-by-4
        %                          homogeneous transformation matrix. The
        %                          matrix maps points in the platform mesh
        %                          frame to points in the body frame.
        %       Orientation      - Relative mesh orientation in the body
        %                          frame, specified as a quaternion vector
        %                          of the form [w x y z] or a quaternion
        %                          object.
        %                          Default : [1 0 0 0]
        %       Position         - Relative mesh position in the body
        %                          frame, specified as a vector of the form
        %                          [x y z] in meters.
        %                          Default : [0 0 0]
        %       Scale            - Scale of the GroundVehicle robot
        %                          platform mesh, specified as a scalar.
        %                          Scale is unitless.
        %                          Default : 1
        %       Size             - Size of the cuboid robot platform mesh,
        %                          specified as a three-element vector of
        %                          form [LENGTHX, LENGTHY, LENGTHZ] in
        %                          meters.
        %                          Default : [1 0.5 0.3]
        %       Vertices         - Vertices of the custom robot platform
        %                          mesh, specified as an N-by-3 matrix of
        %                          real scalars. The first, second, and
        %                          third element of each row represents the
        %                          x-, y-, and z-position of each vertex,
        %                          respectively. N is the number of
        %                          vertices.
        %      Collision         - Add collision object to platform mesh,
        %                          specified as false, "default", "mesh",
        %                          "capsule", or externally created
        %                          collision object. The
        %                          rigidBodyTree-based platform accepts
        %                          externally created collision mesh only
        %                          for base body.
        %                           - false  - Keeps platform mesh
        %                             collision object free. Clear existing
        %                             collision for rigidBodyTree-based
        %                             platform.
        %                           - "default" - Keeps existing collision
        %                             mesh for rigidBodyTree-based
        %                             platform. For other platforms, keeps
        %                             platform mesh collision free.
        %                           - "mesh" - Fits collision object such
        %                             as collisionBox, collisionCylinder,
        %                             collisionSphere, and collisionMesh
        %                             object based on platform mesh type.
        %                           - "capsule" - Fits collision capsule
        %                             object with platform mesh.
        %                           - Externally created collision object
        %                             of type: collisionCapsule,
        %                             collisionBox, collisionCylinder,
        %                             collisionSphere, and collisionMesh.
        %                          Default: "default"
        %      CollisionOffset   - Transformation of collision mesh
        %                          relative to the platform mesh, specified
        %                          as a 4-by-4 homogeneous transformation
        %                          matrix. Use the CollisionOffset input
        %                          for rigidBodyTree-based platforms only
        %                          when the Collision input is specified as
        %                          an externally created collision object.
        %                          Default: eye(4)
        %
        %   Example:
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1,StopTime=10);
        %
        %      % Create three platforms.
        %      platformA = robotPlatform("RobotA", scenario);
        %      platformB = robotPlatform("RobotB", scenario);
        %      platformC = robotPlatform("RobotC", scenario);
        %
        %      % Create a rigid body tree.
        %      robot = loadrobot("amrPioneer3AT");
        %
        %      % Update platform with external collision mesh.
        %      externalCollisionObj = collisionBox(1,1,0.5);
        %      externalCollisionObj.Pose = trvec2tform([2 3 0]);
        %      updateMesh(platformA,"GroundVehicle",...
        %                           Collision=externalCollisionObj,...
        %                           Scale=2, Position=[2 3 0]);
        %
        %      % Update platform with capsule fit collision.
        %      updateMesh(platformB,"Cuboid", Size=[1.5 1 0.5],...
        %                           Collision="capsule",...
        %                           Color=[1 0.5 0],Position=[4 5 0]);
        %
        %      % Update platform with mesh fit collision
        %      updateMesh(platformC,"RigidBodyTree", Object=robot,...
        %                           Collision="mesh", Position=[1 1 0]);
        %
        %      % Visualize the visual mesh along with collision mesh.
        %      show3D(scenario, Collisions="on");

            type = validatestring(type, uav.internal.scenario.Constants.RobotBodyMeshType, "updateMesh", "type");

            p = inputParser;
            addParameter(p,'Position',[0 0 0],...
                         @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 3, "real", "finite"},"updateMesh", "Position"));
            addParameter(p,'Orientation',[1 0 0 0],...
                         @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 4, "real", "finite"},"updateMesh", "Orientation"));
            addParameter(p,'IsBinaryOccupied', {}, ...
                         @(x)validateattributes(x,["logical", "numeric"], ["scalar", "binary"], "updateMesh", "IsBinaryOccupied"));
            addParameter(p,"Collision", "default");
            addParameter(p,"CollisionOffset", eye(4),...
                         @(x)validateattributes(x, "numeric", {"finite","2d", "nrows", 4, "ncols", 4, "real"}, "updateMesh", "CollisionOffset"));
            addParameter(p,'Offset',{});

            switch type
              case "GroundVehicle"
                addParameter(p,'Color', [1 0 0],...
                             @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 3, "real", "finite",">=", 0, "<=", 1},"updateMesh", "Color"));
                addParameter(p, 'Scale', 1,...
                             @(x)validateattributes(x,"numeric",["scalar", "positive", "finite"], "updateMesh", "Scale"));
              case "Cuboid"
                addParameter(p,'Color', [1 0 0],...
                             @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 3, "real", "finite",">=", 0, "<=", 1},"updateMesh", "Color"));
                addParameter(p,'Size', ...
                             uav.internal.scenario.Constants.DefaultCuboidSize, ...
                             @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 3, "positive", "finite"},"updateMesh", "Size"));
              case "Custom"
                addParameter(p,'Color', [1 0 0],...
                             @(x)validateattributes(x,"numeric",{"2d", "nrows", 1, "ncols", 3, "real", "finite",">=", 0, "<=", 1},"updateMesh", "Color"));
                addParameter(p,'Vertices', {},...
                             @(x)validateattributes(x,"numeric",{"2d","finite", "real", "ncols", 3, "nonempty"},"updateMesh", "Vertices"));
                addParameter(p,'Faces', {},...
                             @(x)validateattributes(x,"numeric",{"2d","finite", "integer", "ncols", 3},"updateMesh", "Faces"));
              case "RigidBodyTree"
                addParameter(p,'Object', {},...
                             @(x)validateattributes(x,"rigidBodyTree", ["scalar", "nonempty"], "updateMesh", "Object"));
            end
            parse(p,varargin{:});

            % validate and get valid collision input
            collisionMeshInput = ...
                robotics.internal.scenario.collision.CollisionObjectUtil.getValidCollisionInput(...
                p.Results.Collision, true);

            collisionInput.Mesh = collisionMeshInput;
            collisionInput.Offset = p.Results.CollisionOffset;

            switch type
              case "GroundVehicle"
                color = double(p.Results.Color);
                geometries{1} = p.Results.Scale;
                geometries = cellfun(@double, geometries, "UniformOutput", false);
              case "Cuboid"
                color = double(p.Results.Color);
                geometries{1} = p.Results.Size;
                geometries = cellfun(@double, geometries, "UniformOutput", false);
              case "Custom"
                color = double(p.Results.Color);
                % validate input vertices and faces
                validateattributes(p.Results.Vertices,"numeric", {"2d","finite", "real", "ncols", 3}, "updateMesh", "Vertices");
                validateattributes(p.Results.Faces, "numeric",{"2d","finite", "integer", "ncols", 3}, "updateMesh", "Faces");
                geometries{1} = double(p.Results.Vertices);
                geometries{2} = p.Results.Faces;
                geometries = cellfun(@double, geometries, "UniformOutput", false);
              case "RigidBodyTree"
                color = [1 0 0];
                if isempty(p.Results.Object)
                    error(message("robotics:robotscenario:scenario:MissingRBTObject"));
                else
                    geometries{1} = p.Results.Object;
                end
            end

            if ~isempty(p.Results.Offset)
                % validate input offset
                offset = double(p.Results.Offset);
                validateattributes(offset, "numeric", {"2d", "nrows", 4, "ncols", 4, "real", "finite"}, "updateMesh", "offset");
            else
                % validate input position and orientation
                position = p.Results.Position;
                orientation = p.Results.Orientation;
                validateattributes(norm(orientation), ["quaternion", "numeric"], {'nonzero'}, "updateMesh", "Orientation norm");
                orientation = robotics.internal.validation.validateQuaternion(orientation, "updateMesh", "Orientation");
                offset = trvec2tform(double(position))*quat2tform(double(orientation));
            end

            if ~isempty(p.Results.IsBinaryOccupied)
                % change only if updateMesh contains occupancy status
                % otherwise preserve existing status
                obj.PlatformImpl.OccupancyMapDetails = ...
                    uav.internal.scenario.map.OccupancyMapInfo(p.Results.IsBinaryOccupied);
            end

            obj.PlatformImpl.setMesh(lower(type), geometries, color, offset, collisionInput);

        end

        function motion = read(obj, type)
        %READ Read robot platform motion vector or joint configuration
        %   MOTION = READ(PLATFORM,'base') reads the latest motion of the
        %   robot platform base in the scenario. The robot platform motion
        %   at the current instance in the scenario is returned as a
        %   16-element vector including position, velocity, acceleration,
        %   orientation, and angular velocity as [x y z vx vy vz ax ay az
        %   qw qx qy qz wx wy wz].
        %
        %   CONFIG = READ(PLATFORM,'joint') reads the latest joint
        %   configuration of the rigidBodyTree-based robot platform in the
        %   scenario. The robot platform joint configuration at the current
        %   instance in the scenario is returned as an N-element vector,
        %   where N is the total number of joints associated with the
        %   rigidBodyTree object.
        %
        %   Example 1 : Read motion vector from platform base
        %   -------------------------------------------------
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1, StopTime=10);
        %
        %      % Create platform at specific initial position.
        %      platform = robotPlatform("Robot", scenario,...
        %                               InitialBasePosition=[2 0 0]);
        %
        %      % Setup scenario and get initial motion vector.
        %      setup(scenario);
        %      motion = read(platform);
        %
        %   Example 2 : Read platform joint configuration
        %   -------------------------------------------------
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1, StopTime=10);
        %
        %      % Create rigidBodyTree with loadrobot.
        %      robot = loadrobot("universalUR10");
        %
        %      % Get initial joint configuration.
        %      initConfig = robot.randomConfiguration;
        %
        %      % Create platform with initial joint configuration.
        %      platform = robotPlatform("Robot", scenario,...
        %                   RigidBodyTree=robot,...
        %                   InitialJointConfiguration=initConfig);
        %
        %      % Setup scenario and get initial joint configuration.
        %      setup(scenario);
        %      config = read(platform,"joint");

            if nargin < 2
                type = 'base'; % preserve previous release support
            else
                type = validatestring(type, uav.internal.scenario.Constants.RobotPlatformBodyName, "read", "type");
            end

            if isempty(obj.RigidBodyTree) && strcmp(type,'joint')
                % only rigidBodyTree-based platform has support for 'joint'
                % type
                error(message("robotics:robotscenario:scenario:OnlySupportRBTPlatform"));
            end

            motion = obj.PlatformImpl.read(type, obj.ScenarioImpl.CurrentTime);

        end

        function move(obj, type, motion)
        %MOVE Move robot platform in scenario
        %   MOVE(PLATFORM,TYPE,MOTION) moves the robot platform of TYPE
        %   "base" in the scenario according to the specified motion
        %   MOTION. The robot platform motion at the current instance in
        %   the scenario is specified as a 16-element vector including
        %   position, velocity, acceleration, orientation, and angular
        %   velocity as [x y z vx vy vz ax ay az qw qx qy qz wx wy wz].
        %
        %   MOVE(PLATFORM,TYPE,CONFIG) moves the joints of
        %   rigidBodyTree-based robot platform for input TYPE "joint" in
        %   the scenario according to the specified joint configuration
        %   CONFIG. The robot platform joint configuration at the current
        %   instance in the scenario is specified as an N-element vector,
        %   where N is the total number of joints associated with the
        %   rigidBodyTree object.
        %
        %   Example 1 : Move robot platform base
        %   -------------------------------------------------
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1, StopTime=10);
        %
        %      % Create platform at specific initial position.
        %      platform = robotPlatform("Robot", scenario,...
        %                               InitialBasePosition=[0 3 0]);
        %
        %      % Setup scenario and move platform with motion vector.
        %      setup(scenario);
        %      move(platform,"base",[0 5 0 0 1 0 0 0 0 1 0 0 0 0 0 0])
        %
        %   Example 2 : Move robot platform joint
        %   -------------------------------------------------
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1, StopTime=10);
        %
        %      % Create rigidBodyTree with loadrobot.
        %      robot = loadrobot("universalUR10");
        %
        %      % Get initial joint configuration.
        %      initConfig = robot.randomConfiguration;
        %
        %      % Create platform with initial joint configuration.
        %      platform = robotPlatform("Robot", scenario,...
        %                   RigidBodyTree=robot,...
        %                   InitialJointConfiguration=initConfig);
        %
        %      % Setup scenario
        %      setup(scenario);
        %
        %      %  Move platform with new joint configuration.
        %      newConfig = [-4.3880, 3.3246, 3.0228,...
        %                   -4.2049, 1.4036, 3.0177];
        %      move(platform,"joint", newConfig)
        %

            narginchk(3,3);
            type = validatestring(type, uav.internal.scenario.Constants.RobotPlatformBodyName, "move", "type");

            if strcmp(type,'base')
                validateattributes(motion, "numeric", {"vector", "numel", 16, "finite", "real"}, "move", "motion");
                validateattributes(norm(motion(10:13)), "numeric", "nonzero", "move", "orientation norm");
                robotics.internal.validation.validateQuaternion(motion(10:13), "move", "orientation");

                obj.PlatformImpl.move(type, double(reshape(motion, 1, 16)), obj.ScenarioImpl.CurrentTime);
            else
                if isempty(obj.RigidBodyTree)
                    % only rigidBodyTree-based platform has support for
                    % 'joint' type
                    error(message("robotics:robotscenario:scenario:OnlySupportRBTPlatform"));
                end
                validateattributes(motion, "numeric", {"vector", "finite", "real"}, "move", "motion");
                config = robotics.internal.scenario.rigidbody.RigidBodyTreeUtil.validateConfiguration(obj.RigidBodyTree, motion);
                obj.PlatformImpl.move(type, config', obj.ScenarioImpl.CurrentTime);
            end

        end

        function attach(obj, targetName, attachingBodyName, varargin)
        %ATTACH Attach target robot platform to source robot platform
        %   ATTACH(PLATFORM, TARGETNAME, ATTACHINGBODYNAME, NAME=VALUE)
        %   attaches the non-rigidBodyTree-based target platform TARGETNAME
        %   to the body ATTACHINGBODYNAME of the rigidBodyTree-based source
        %   platform PLATFORM. You can also specify additional options
        %   using name-value arguments.
        %
        %   Name-Value arguments:
        %
        %       ChildToParentTransform - Transformation of attached target
        %                                platform body relative to the
        %                                source platform attaching body,
        %                                specified as a 4-by-4 homogeneous
        %                                transformation matrix.
        %                                Default : eye(4)
        %       Collision              - Add collision object to target
        %                                platform mesh, specified as a
        %                                scalar externally created
        %                                collision object such as
        %                                collisionCapsule, collisionBox,
        %                                collisionCylinder,
        %                                collisionSphere, and
        %                                collisionMesh. This is required if
        %                                target platform needs other than
        %                                default collision mesh.
        %                                Default : Existing Platform
        %                                Collision Mesh
        %
        %   Example :
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1,StopTime=10);
        %
        %      % Create a rigid body tree.
        %      robot = loadrobot("frankaEmikaPanda");
        %
        %      % Create platform with initial joint configuration.
        %      sourcePlatform = robotPlatform("Manipulator", scenario,...
        %           RigidBodyTree=robot);
        %
        %      % Add target platform in the scenario and update dimensions
        %      targetPlatform = robotPlatform("Box",scenario,...
        %           Collision="mesh",InitialBasePosition=[0.5 0.15 0.278]);
        %      updateMesh(targetPlatform, "Cuboid", Collision="mesh",...
        %           Size=[0.06,0.06,0.1])
        %
        %      % Visualize Scenario
        %      ax = show3D(scenario,Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Define goal joint configuration
        %      goalConfig = [0.2371,-0.0200,0.0542,-2.2272,0.0013,...
        %                    2.2072,-0.9670,0.0400,0.0400];
        %
        %      % Setup Scene
        %      setup(scenario)
        %
        %      % Move source "Manipulator" near to target "box"
        %      move(sourcePlatform, "joint", goalConfig)
        %
        %      % Visualize Scenario before picking box
        %      show3D(scenario,Parent=ax,Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Pick-up target "box" with source "Manipulator"
        %      attach(sourcePlatform, "Box", "panda_hand",...
        %            ChildToParentTransform=trvec2tform([0 0 0.1]))
        %
        %      % Move source "Manipulator" with target "box" to random
        %      % position
        %      move(sourcePlatform, "joint", ...
        %            sourcePlatform.RigidBodyTree.randomConfiguration)
        %
        %      % Visualize Scenario after picking box
        %      show3D(scenario,Parent=ax,Collisions="on");
        %      light

            if isempty(obj.RigidBodyTree)
                % error-out if source platform is not RBT based
                error(message("robotics:robotscenario:scenario:OnlySupportRBTPlatform"));
            end

            % validate input target platform name
            targetName = convertCharsToStrings(targetName);
            validateattributes(targetName, ["char", "string"], "scalartext",...
                               "attach", "targetName");

            if strcmp(targetName,obj.Name)
                % error-out if target and source platform are same
                error(message("robotics:robotscenario:scenario:SameSourceTargetPlatform"));
            end

            if obj.PlatformImpl.BodyAttachmentDetails.IsTargetAttached
                % error-out if source platform already contains the
                % target platform
                error(message("robotics:robotscenario:scenario:SourcePlatformCotainsTarget",...
                              obj.Name, obj.PlatformImpl.BodyAttachmentDetails.AttachedChildPlatformName));
            end

            % validate input attaching platform body name
            attachingBodyName = convertCharsToStrings(attachingBodyName);
            validateattributes(attachingBodyName, ["char", "string"], "scalartext", "attach", "platformBodyName");
            validBodyList = robotics.internal.scenario.rigidbody.getPlatformBodyList(obj, 'excludePlatform');
            if ~any(strcmp(validBodyList, attachingBodyName))
                % error-out if input attaching body name is invalid
                error(message("robotics:robotscenario:scenario:InvalidAttachingBodyName",...
                              attachingBodyName, strjoin(validBodyList,' , ')));
            end

            % parse input
            p = inputParser;
            addParameter(p,"Collision", []);
            addParameter(p,"ChildToParentTransform", eye(4),...
                         @(x)validateattributes(x, "numeric", {"finite","2d", "nrows", 4, "ncols", 4, "real"}, "updateMesh", "CollisionOffset"));
            parse(p,varargin{:});

            if ~isempty(p.Results.Collision)
                if ~isa(p.Results.Collision, "robotics.core.internal.CollisionGeometryBase")
                    % error-out if collision input is object but not any
                    % type from collision objects
                    error(message("robotics:robotscenario:scenario:InvalidExternalCollision"));
                end
            end

            % validate target platform body name
            validPlatNames = robotPlatform.getSceneEntityNames(obj,'allPlatform');
            targetPlatIndx = strcmp(validPlatNames, targetName);
            if any(targetPlatIndx)
                % get target platform object
                targetPlatform = obj.ScenarioImpl.Platforms(targetPlatIndx);
            else
                error(message("robotics:robotscenario:scenario:TargetPlatformNotFound", targetName, strjoin(validPlatNames,' , ')));
            end

            if targetPlatform.BodyAttachmentDetails.IsAttachedToSource
                % error-out if target is already attached to the source
                % platform or attached source platform is different
                error(message("robotics:robotscenario:scenario:TargetPlatformIsAttached",...
                              targetPlatform.Name, ...
                              targetPlatform.BodyAttachmentDetails.AttachedParentPlatformName));
            end

            if ~isempty(targetPlatform.RigidBodyTree)
                error(message("robotics:robotscenario:scenario:TargetNonRBTPlatform", targetPlatform.Name));
            end

            % attach target platform
            obj.PlatformImpl.attachPlatform(targetPlatform, attachingBodyName,...
                                            p.Results.ChildToParentTransform, p.Results.Collision, ...
                                            obj.ScenarioImpl.CurrentTime);

        end

        function detach(obj)
        %DETACH Detach target robot platform from source robot platform
        %   DETACH(PLATFORM) detaches the attached target platform from the
        %   rigidBodyTree-based source robot PLATFORM.
        %
        %   Example :
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1,StopTime=10);
        %
        %      % Create a rigid body tree.
        %      robot = loadrobot("frankaEmikaPanda");
        %
        %      % Create platform with initial joint configuration.
        %      sourcePlatform = robotPlatform("Manipulator", scenario,...
        %           RigidBodyTree=robot);
        %
        %      % Add target platform in the scenario and update dimensions
        %      targetPlatform = robotPlatform("Box", scenario,...
        %           Collision="mesh",InitialBasePosition=[0.5 0.15 0.278]);
        %      updateMesh(targetPlatform, "Cuboid", Collision="mesh",...
        %           Size=[0.06,0.06,0.1])
        %
        %      % Visualize Scenario
        %      ax = show3D(scenario,Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Define goal joint configuration
        %      goalConfig = [0.2371,-0.0200,0.0542,-2.2272,0.0013,...
        %                    2.2072,-0.9670,0.0400,0.0400];
        %
        %      % Setup Scene
        %      setup(scenario)
        %
        %      % Move source "Manipulator" near to target "box"
        %      move(sourcePlatform, "joint", goalConfig)
        %
        %      % Visualize Scenario before picking box
        %      show3D(scenario,Parent=ax,Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Pick-up target "box" with source "Manipulator"
        %      attach(sourcePlatform, "Box", "panda_hand",...
        %            ChildToParentTransform=trvec2tform([0 0 0.1]))
        %
        %      % Move source "Manipulator" with target "box" to random
        %      % position
        %      move(sourcePlatform, "joint", ...
        %            sourcePlatform.RigidBodyTree.randomConfiguration)
        %
        %      % Visualize Scenario after picking box
        %      show3D(scenario,Parent=ax,Collisions="on");
        %      light
        %
        %      % Place target "box" at random position and move to another
        %      % random position
        %      detach(sourcePlatform);
        %      move(sourcePlatform, "joint", ...
        %            sourcePlatform.RigidBodyTree.randomConfiguration)
        %
        %      % Visualize Scenario after placing box
        %      show3D(scenario,Parent=ax,Collisions="on");
        %      light

            narginchk(1,1);

            if isempty(obj.RigidBodyTree)
                % error-out if source platform is not RBT based
                error(message("robotics:robotscenario:scenario:OnlySupportRBTPlatform"));
            end

            if ~obj.PlatformImpl.BodyAttachmentDetails.IsTargetAttached
                % error-out if source platform does not contain any target
                % platform or attached target is different from input
                % target
                error(message("robotics:robotscenario:scenario:SourcePlatformNotCotainsTarget",obj.Name));
            end

            targetName = obj.PlatformImpl.BodyAttachmentDetails.AttachedChildPlatformName;

            % get target platform object
            validPlatNames = robotPlatform.getSceneEntityNames(obj,'allPlatform');
            targetPlatIndx = strcmp(validPlatNames, targetName);
            targetPlatform = obj.ScenarioImpl.Platforms(targetPlatIndx);

            obj.PlatformImpl.detachPlatform(targetPlatform, obj.ScenarioImpl.CurrentTime);

        end

        function [isColliding, separationDist, witnessPts] = checkCollision(obj, targetBody, varargin)
        %   CHECKCOLLISION Check collision between robot platform and target bodies
        %   [ISCOLLIDING, SEPARATIONDIST, WITNESSPTS] = checkCollision(PLATFORM, TARGETBODY)
        %   check if the rigidBodyTree-based robot platform PLATFORM is
        %   colliding with a set of target bodies TARGETBODY. Specify the
        %   TARGETBODY as a scalar, string array containing
        %   non-RigidBodyTree-based platform names, scenario mesh names, or
        %   cell array of externally created collision objects.
        %
        %   OUTPUT ARGUMENTS:
        %
        %       ISCOLLIDING - Logical scalar indicating both self-collision
        %       and collision with a set of TARGETBODY respectively as a
        %       two-element vector. If "IgnoreSelfCollision" is "on",
        %       ISCOLLIDING is a logical scalar indicating collision with
        %       target bodies.
        %
        %       SEPARATIONDIST - Separation distance between target bodies
        %       and the robot platform bodies including the base, returned
        %       as a (N+1)-by-(N+M+1) square matrix, where N is the number
        %       of robot platform bodies, and M is the number of target
        %       bodies. If "IgnoreSelfCollision" is "on", SEPARATIONDIST is
        %       an (N+1)-by-M matrix with column indices corresponding to
        %       the target bodies.
        %
        %       WITNESSPTS - Witness points between bodies of the robot
        %       platform including the base, returned as a
        %       3(N+1)-by-2(N+1) matrix, where N is the number of bodies.
        %       The matrix has the format:
        %
        %       [Wr1_1       Wr1_2        ...     Wr1_(N)    Wr1_(N+1);
        %        Wr2_1       Wr2_2        ...     Wr2_(N)    Wr2_(N+1);
        %        .           .            .       .         .
        %        .           .            .       .         .
        %        .           .            .       .         .
        %        Wr(N+1)_1   Wr(N+1)_2    ...     Wr(N+1)_N  Wr(N+1)_(N+1)]
        %
        %       Each element, Wr, in the above matrix is a 3-by-2 matrix
        %       that gives the nearest [x y z] points on the two
        %       corresponding bodies in the robot platform. The final row
        %       and column correspond to the robot platform base.
        %
        %   [ISCOLLIDING, SEPARATIONDIST, WITNESSPTS] = checkCollision(__, Name=Value)
        %   specifies additional options using one or more name-value
        %   arguments.
        %
        %       Exhaustive            - Exhaustively check for all
        %                               collisions, specified as "on" or
        %                               "off". Setting this to "off" will
        %                               interrupt the check when the first
        %                               collision is found. If returned
        %                               early, separation distance and
        %                               witness points values are returned
        %                               as Inf for incomplete checks.
        %                               Setting this to 'on' will compute
        %                               the separation distance and witness
        %                               points for all pairs.
        %                               Default: "off"
        %
        %       IgnoreSelfCollision   - Skip checking for collisions
        %                               between robot bodies and base,
        %                               specified as "on" or "off".
        %                               Default: "off"
        %
        %       SkippedSelfCollisions - Body pairs skipped for checking
        %                               self-collisions, specified as
        %                               "parent" or "adjacent".
        %                                 - "parent" -Skip collision
        %                                 checking between child and parent
        %                                 bodies.
        %                                 - "adjacent" - Skip collision
        %                                 checking between bodies on
        %                                 adjacent indices.
        %                               Default: "parent"
        %
        %   Example :
        %
        %      % Create scenario.
        %      scenario = robotScenario(UpdateRate=1, StopTime=10);
        %      addMesh(scenario,"Cylinder",Name="Can",Collision="mesh",...
        %           Position=[0.52 0 0.278],Size=[0.03, 0.1]);
        %
        %      % Create a rigid body tree.
        %      robot = loadrobot("frankaEmikaPanda");
        %
        %      % Create platform with initial joint configuration.
        %      sourcePlatform = robotPlatform("Manipulator", scenario,...
        %           RigidBodyTree=robot);
        %
        %      % Add target platform in the scenario and update dimensions
        %      targetPlatform = robotPlatform("Box", scenario,...
        %           Collision="mesh",InitialBasePosition=[0.5 0.15 0.278]);
        %      updateMesh(targetPlatform, "Cuboid", Collision="mesh",...
        %           Size=[0.06,0.06,0.1])
        %
        %      % Visualize Scenario
        %      ax = show3D(scenario, Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Define goal joint configuration
        %      goalConfig = [0.2371,-0.0200,0.0542,-2.2272,0.0013,...
        %                    2.2072,-0.9670,0.0400,0.0400];
        %
        %      % Setup Scene
        %      setup(scenario)
        %
        %      % Check collision before "Manipulator" reaches at "Box"
        %      isColliding = checkCollision(sourcePlatform, ...
        %           ["Box","Can"], IgnoreSelfCollision="on");
        %
        %      % Move source "Manipulator" near to target "box"
        %      move(sourcePlatform, "joint", goalConfig)
        %
        %      % Visualize Scenario once "Manipulator" reaches near to box
        %      show3D(scenario, Parent=ax, Collisions="on");
        %      view(79,36)
        %      light
        %
        %      % Check collision after "Manipulator" reaches at "Box"
        %      isColliding = checkCollision(sourcePlatform, ...
        %           ["Box","Can"], IgnoreSelfCollision="on");


            if isempty(obj.RigidBodyTree)
                % error-out if platform is not RBT based
                error(message("robotics:robotscenario:scenario:OnlySupportRBTPlatform"));
            end

            % validate target body input and get valid collision objects
            collisionObj = ...
                robotics.internal.scenario.collision.CollisionObjectUtil.getValidCheckCollisionObjects(...
                obj, obj.ScenarioImpl.Platforms, obj.ScenarioImpl.Meshes, targetBody);

            % process check collision
            [isColliding, separationDist, witnessPts] = ...
                obj.PlatformImpl.checkPlatformCollision(collisionObj, obj.ScenarioImpl.CurrentTime, varargin{:});
        end

    end

    methods(Static, Access = {?robotPlatform,...
                              ?robotics.internal.scenario.Platform})

        function sceneElementNames = getSceneEntityNames(platform, type)
        %getSceneEntityNames returns valid scene mesh and platform
        %name list from scene

            switch(type)
              case 'allPlatform'
                sceneElementNames = [platform.ScenarioImpl.Platforms.Name];
              case 'validPlatform'
                sceneElementNames = [platform.ScenarioImpl.Platforms.Name];
                % remove current platform name from list
                sceneElementNames(strcmp(sceneElementNames,platform.Name)) = [];
              case 'scenarioMesh'
                sceneElementNames = platform.ScenarioImpl.MeshNameList;
              case 'allSceneEntity'
                validPlatformNames = [platform.ScenarioImpl.Platforms.Name];
                % remove current platform name from list
                validPlatformNames(strcmp(validPlatformNames,platform.Name)) = [];
                sceneElementNames = [platform.ScenarioImpl.MeshNameList, validPlatformNames];
            end
        end

    end

end
