%% Control a UR10 Manipulator with Simulink and Gazebo
% This example demonstrates how to simulation control of a robotics manipulator using Simulink and Gazebo. The example uses several Gazebo Co-simulation features to ensure a tight connection between Simulink and Gazebo:
% The Gazebo Pacer block is used to ensure that Simulink drives the stepping, allowing control to be executed accurately in Simulink while simulating the physical dynamics in Gazebo.
% The Gazebo Read and Gazebo Apply blocks are used for communication between MATLAB and Gazebo.
% A variety of MATLAB commands, including gzlink, gzjoint, and gzworld provide easy access to model parameters and queries.
% This example will review each of these components and their configurations in detail using a Simulink model that controls the positions of a Universal Robotics UR10 manipulator. The model uses inverse kinematics to relate desired end effector pose to joint positions, then applies a joint-space PD controller with dynamic compensation feedforward terms to govern the motion.
% For more information about setting up the environment for this example, refer to Gazebo Cosimulation for Robotic Arm - Set Joint States.
%
%% Open the model in Gazebo
% To run this example, you must have access to a machine with Gazebo with the plug-in for Gazebo co-simulation installed and the provided world. If you are using the provided Virtual Machine [LINK] [TODO: Update that machine so the steps below are reflected], the next steps may be skipped.
%% Add the model and set up Gazebo
% To execute this example, you must either be able to connect to a machine with Linux installed (such as the supplied Virtual Machine) or you must be executing the example from a Linux environment that has been configured accordingly:
% [FORMATTING NOTE: I tried to use numbered lists, but I can't break up the last number in a meaningful way because I am only allowed one paragraph per number -- please direct as needed].
% 1) Set up the Gazebo environment and add the plug-in.
% Follow the instructions at [this link][LINK]. In this example, it is assumed that the plug-in is located in /home/user/src/GazeboPlugIn.
% 2) Add the provided world file to the directory /home/user/worlds
% 3) Add the robot mesh files to the virtual machine.
% The mesh files are Collada (*.DAE files) that are required to visualize the robot. This robot was created using loadrobot, so the repository containing all associated information -- including meshes -- may be found in the "Source" property of second output of loadrobot('universalUR10'):

[~, info] = loadrobot('universalUR10');
disp(info.Source)

% In this case, that points to V1.2.1 of a ROS-Industrial repository. These files are made available when the associated ROS package is installed, but It is sufficient to simply clone the repository, which will include the ur_description folder that contains the meshes. This may be done by executing the following commands on the machine where Gazebo is installed:
% cd /home/user/catkin_ws/src
% git clone -b 1.2.1 https://github.com/ros-industrial/universal_robot/
% Once these steps are completed, the following statements on directories should be true:
% The Gazebo plug-in has been unzipped and installed to /home/user/src/GazeboPlugIn
% The world file is located at /home/user/worlds/Ur10BasicWithPlugin.world
% The robot mesh files are located at /home/user/catkin_ws/src/universal_robot/ur_description/.
% As noted above, if using the provided virtual machine, these changes have already been configured.
%% Open the world file in Gazebo
% Open the world by running these commands in the terminal:
% cd /home/user/src/GazeboPlugin/export
% export SVGA_VGPU10=0
% gazebo /home/user/worlds/Ur10BasicWithPlugin.world --verbose
% Gazebo should open with the following visual.
%
%
%% Initialize connection to Gazebo
% In order to communicate between MATLAB/Simulink and Gazebo, it is necessary to initialize the connection in MATLAB.

ipGazebo = '192.168.116.161';
gzinit(ipGazebo,14581);

% Here, ipGazebo is the IP address of the machine hosting Gazebo. On the shipped virtual machine, this address randomly assigned and is displayed on the desktop.
% See [DOCUMENTATION] for further information on establishing a connection to Gazebo.
%
%% Load the Simulink model
% Open the model using the following command.

open_system('gazeboCosimControl');

%%
%% Set the sample time

Ts = 0.01;
Ts_trajectory = 0.05;

% The sample time of the controller is short to ensure good performance. The sample time of trajectory is longer to ensure simulation speed. The latter is a sample time for high level tasks like camera sensor, inverse kinematics, and trajectory generation.
%% Load the rigid body tree
% This model uses a universal UR10 robot created using loadrobot. As detailed in [LINK TO PREVIOUS EXAMPLE], the Gazebo model has been created from the same source repository so that they match.

robot = loadrobot('universalUR10','Gravity',[0 0 -9.81],'DataFormat','column');
showdetails(robot)

% Set the initial configuration of the robot.

q0 = [0 -70 140 0 0 0]' * pi/180; 

%
%% Understand the model construction
% The Simulink model consists of four areas:
% User input: Allows the user to provide inputs to the desired robot end effector position using interactive sliders.
% Control: Maps the end effector position to joint position using inverse kinematics and controls joint position using a computed torque controller
% Gazebo Pacer: Maintains the connection to Gazebo to ensure that the simulink model drives co-simulation speed
% Gazebo Model: Sends data to and from the Gazebo world
% These sections are outlined in greater detail below.
%% User Input
% The user input has 6 sliders that control the end effector position: three to control the X, Y, and Z position in space, and three to control the orientation.
%% Control
%
% The control section translates the desired end effector pose to joint torques. First the Inverse Kinematics subsystem computes joint positions that satisfy a target end effector pose. Next, the Joint Controller subsystem produces actuator input torques given the joint reference position, velocity, and acceleration, and the present state of the simulated robot -- the measured position and velocity output from Gazebo.
%% Inverse Kinematics subsystem
% Inverse kinematics calculates the joint position for the corresponding end effector pose (position and orientation) given an initial guess. For fast convergence, set the initial condition of the unit delay. In this subsystem, the weights are uniform, implying that the solution should place equal importance on achieving all positions and orientations in the desired pose.
%
%% Joint Controller subsystem
% In this example, a joint controller with feedback and feedforward term is used. A Proportional-Derivative (PD) controller is used for the feedback term. Gravity and velocity product is fed as the feedforward term. For more details about joint controllers, refer to Perform Safe Trajectory Tracking Control Using Robotics Manipulator Blocks.
%
%% Gazebo Pacer
% The Gazebo Pacer is used to ensure co-simulation with Gazebo: the Simulink model controls the Gazebo simulation stepping, ensuring that the Gazebo world and the Gazebo model are always in sync. This is important for applications like this model, where a controller is used to directly govern the positin of a robot in the Gazebo world. It is critical that the joint measurements and torque applications be synced to ensure the simulation is reflective of the actual behavior.
%
% Open the block parameters on the Gazebo pacer block and set the sample time.
%% Gazebo Model
% The Gazebo Model subsystem contains the blocks used to communicate with the Gazebo world. In this case, the controller applies a torque to each joint and reads back the joint position and velocity. Therefore, for each joint, it is necessary to send the computed torque to the appropriate joint in the Gazebo model, and to read back the measured joint states.
%
%% Apply Joint Torque
% For each joint, the torque must be applied. This is achieved in the "Set <Joint name>" subsystems in the Transmit to Gazebo area of the Gazebo model subsystem. For example, for the shoulder pan joint, the tools to send the shoulder plan subsystem are contained in the Set Shoulder Pan subsystem.
%
% Each of these subsystems uses a Bus Assignment block with inputs dictating the type of bus, the details of the model and duration, and the resultant command.
% The Gazebo Blank Message block defines the bus type to be a Gazebo message format of the appropriate type. In the block parameters dialog, select "ApplyJointTorque" from the list of command types. The block is required to ensure that the correct bus template is used, i.e. that the port is the Bus Assignment block have the correct values.
% The Gazebo Select Entity block selects the object to which this message will be applied. In this case, since a torque is being applied, the "entity" is a joint. Since this subsystem will apply torque to the shoulder joint, select the corresponding joint in the Gazebo world from the list of entity types. As long as a connection to the Gazebo world has been established, this list will be automatically populated. If the connection is lost, click the "Configure Gazebo network and simulation settings" link to reestablish, or use the gzinit command line interface. The Gazebo world must be open for this operation to succeed.
%
% The Gazebo Apply Command block takes the contents of the message sends it to the Gazebo server. Open the block dialog parameters, select the Command type parameter, and select ApplyJointTorque to send the corresponding command to Gazebo.
%
% The remaining inputs govern the amount and duration of the applied force. The effort inport specifies the joint torque quantity. The index input dictates the axis to which the torque should be applied. Since each of the revolute joints is just 1 degree of freedom, this value should be set to uint32(0) to indicate the first axis. Finally, the duration inputs specify the duration of the applied torque, divided into seconds and nanoseconds. For example, 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. In this case, the controller applies the torque for the length of the sample time, previously specified to 0.01 seconds, or 1e7 nanoseconds. Therefore the first input is zero (0 seconds) and the second input is 1e7.
%
% The Gazebo Model subsystem includes 5 other subsystems corresponding to the other five joints, which have been configured in the same fashion.
%% Measure Joint State
% The second part of the Gazebo Model interface requires reading values from Gazebo. This is achieved using Gazebo Read blocks. For example, the block for the shoulder pan joint is shown below.
%
% The Gazebo Read block reads messages from the Gazebo server. In the block dialog, click "Select.." next to the topic to choose the right topic to read from, and select the corresponding measured joint value, /gazebo/ground_truth/joint_state/ur10/shoulder_pan_joint. This message selects the precise ground truth value, though it is also possible to place sensors in Gazebo and read from those instead.
% Use the Bus Selector to only choose the relevant values from the message. In this case, only the position is measured from the Gazebo model, which is ultimately fed back to the controller.
% This section also reads back the value of the red box in Gazebo. While it is possible to also set the position of this box using a format similar to that used above, it is simpler to use the gzlink command line interface to update the block at discrete instants during the model execution.
%
%% 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 joints, they are 1-dimensional and fixed in size, which can be specified as a property of the bus signals. 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, open the Bus Editor in Simulink. This can be found under Modeling > Design > Bus Editor.
%
% Select the bus used to read joint state, then elect the corresponding element name on the right-hand side. For Gazebo_SL_Bus_gazebo_msgs_JointState, choose joint_position and joint_velocity and change Dimension to 1 and DimensionMode to 'Fixed'.
%
%
%% Run the simulation and observe performance
% While it is also possible to have the Gazebo Pacer block reset the scene with each run, in this model, only time is reset. This means that the model can be updated ahead of simulation runs using MATLAB commands.

gzworld("reset"); % Reset the world to its initial state
gzlink("set","redBox","link","Position",[0.5 -0.4 .3]); % Move the box to a new location

% Commands like these can be more directly incorporated into the Simulink model as a StartFcn callback to ensure that they are executed at each Simulation run.
% Run the simulation and test different reference poses for the end effector to verify the inverse kinematics and joint controller. The following command runs the simulation for 20 seconds.

simoutput = sim('gazeboCosimControl','StopTime','20');

%
% The following data inspector shows example behavior where the manipulator command positions have been changed several times over the course of the simulation.
%
%
%% View Performance
% Joint measurements and references are logged. They can be viewed during the simulation using tools like the Data Inspector (above), or once a simulation is complete, by plotting the logged outputs:

measuredPosition = simoutput.logsout{1}.Values; 
referencePosition = simoutput.logsout{2}.Values; 
figure
plot(measuredPosition.Time, measuredPosition.Data, '-', referencePosition.Time, referencePosition.Data, '--')
legend({'Meas1','Meas2','Meas3','Meas4','Meas5','Meas6','Ref1','Ref2','Ref3','Ref4','Ref5','Ref6'})

%
% Copyright 2020 The MathWorks, Inc.