%% Gazebo Cosimulation for Robotic Arm - Setup Environment
% This example provides a step by step instructions for setting up Robotic arm in Simulink model to run Gazebo Cosimulation.
% For an introduction to Gazebo co-simulation and getting connected for the first time, see Perform Co-Simulation between Simulink and Gazebo. Download the virtual machine from Virtual Machine with ROS 2 Melodic and Gazebo. Gazebo 9 is used in this example.
% The robot arm used in the example is Universal Robot UR10. It has 6 degrees of freedom. For simplicity, this example does not include a joint controller. Joint angles are directly fed to the robot by setting the joint position with 'Gazebo Apply Command'. Therefore the robot in this example will interact stiffly with the environment and care must be taken.
%% 1) Set up Gazebo world
% This example uses a world given in the VM, robotArmBasic.world. The world contains Universal Robot's UR10 with its base joint fixed to the ground and an object.
% Open the world by running these commands in the terminal:
% cd /home/user/src/GazeboPlugin/export
% gazebo ../world/robotArmBasic.world --verbose
%
% This world was made by 1) Inserting model from GUI and 2) adding fixed joint to the world. Add a fixed joint by adding the following lines of code before the </model> line.
% <joint name='world_base' type='fixed'>
%   <child>base</child>
%   <parent>world</parent>
% </joint>
% 3)  Set up Gazebo plugin at the beginning of the file after the <world> line:
% <plugin name='GazeboPlugin' filename='lib/libGazeboCoSimPlugin.so'>
%   <portNumber>14581</portNumber>
% </plugin>
% To learn more about building a world and adding a fixed joint refer to the documentation from Gazebo.
%
%% 2) Set up Simulink Model
% This model sets the joint states of the robot model by taking inputs from sliders and applying the joint positions in Gazebo. The joint positions are set as using the Gazebo Apply Command block for each link state. The model also reads those joint states using the Gazebo Read block from the Gazebo simulation to verify.
% Open the model and set the sample time:

open_system('gazeboCosimSetJoint');
Ts = 0.01;

% The model is divided into a few areas. User Input for interactively setting the joint angles; Transmit to Gazebo where 'Gazebo Apply Command' block are used to set joint positons; Receive to Gazebo where 'Gazebo Read' blocks are used to get joint angles.
%
% To apply joint position in Gazebo, use the 'Bus Assignment' like the following. For 'Gazebo Apply Command', select SetJointPosition message type to set the joint position. For 'Gazebo Blank Message', select SetJointPosition to set the bus assignment for setting joint position. For 'Gazebo Select Entity', select the associated joint for each of the subsystem blocks to apply joint position for all six joints of the robot.
%
% Set the duration by adjusting values for 'seconds' and 'nano_seconds'. If a duration is 1.005 seconds, it would be 1 second and 5,000,000 nanoseconds as a bus. They are separated out to increase precision.
% To read joint position from Gazebo, use 'Gazebo Read'. Select the associated joints to read joint positions from all six joints of the robot.
%
%
%% 3) Customize Bus Objects for Simulation
% Gazebo bus objects are generated when the cosimulation blocks are introduced in a model. Signals from Gazebo are read through these bus objects. By default dimensions are set to Variable size since joints can have multiple degrees of freedom. Since all joints in this example are revolute joint, change the bus dimension mode to fixed and size to 1. This step is optional, but it is useful to overcome variable-size signal limitations.
% Load the custom bus objects.

load('custom_busobjects_basic'); 

%
% To do so manually, go to bus editor by double clicking the variable in Workspace > for Gazebo_SL_Bus_gazebo_msgs_JointState->joint_position and joint_velocity > change Dimension to 1 and DimensionMode to 'Fixed'.
%
%
%% 4) Set initial position of an object
% Gazebo cosimulation blocks can be used to change the environment parameters. Let's take a look at how the initial position of an object can be changed.
%
% In this subsystem, link is selected to set the world pose. See how coke_can/link is selected in 'Gazebo Select Entity'. In order to set the position of the object (or orientation), select SetLinkWorldPose from 'Gazebo Blank Message'. Change the sample time of 'Gazebo Apply Command' to 1000 seconds or something big so that the pose is set only once during a simulation. We do not want to set the position of the object at every simulation step; that would make it impossible for us to interact with it. Setting a slow sample time would essentially set the position once in the beginning of the simulation.
%
%
%% 5) Interactively Set Joint Position
% The interactive dashboard allows changing the joint reference while the simulation is running. See if the robot model moves as expected.
%
% Run the simulation by clicking the Run button in Simulink:

set_param('gazeboCosimSetJoint','SimulationCommand','start')

%
%
% Copyright 2020 The MathWorks, Inc.