%% Gazebo Cosimulation for Robotic Arm (INTERNAL USE ONLY)
% This example sets the ground work for the 3 other examples: Gazebo Cosim for Robotic Arm - Set Joint States, Control and IK, Pick and Place.
% 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.
%% 1) Set Up Rigid Body Tree and Read Joint Position
% Now that we checked Simulink's communication with Gazebo is working fine with basic controller for simple gripper, let's set up a robot arm in Gazebo and read joint positions. We will work with Universal Robot UR10 for this example.
%% 1-1) Open Gazebo world
% This example uses a world given in the VM, robotArm.world. The world contains Universal Robot's UR10 with its base joint fixed to the ground. There is one red object next to the robot. This file is also available in this folder.
% Open the world by running these commands in the terminal:
% cd /home/user/src/GazeboPlugin/export
% gazebo ../world/robotArm.world --verbose
%
%
%% 1-2) Open Simulink Model
% Open the model:

open_system('gazeboCosimRobotArm')

% Overall, the model consists of two loops: low level feedback loop and high level feedback loop. The low level feedback loop is responsible for following the joint trajectory. This is where sensors and controllers like the Computed Torque Controller would come in. This loop usually run at a faster pace. The high level feedback loop is where trajectory is generated to perform a meaningful task like picking an object. High bandwidth sensors like cameras would be here to estimate the position of target object. This loop usually runs at a slower pace.
% Set the sample times for the low level feedback loop and high level feedback loop.

Ts = 0.01; % gazebo sample time
Ts_trajectory = 0.05; % trajectory and camera sample time 

%
%
%% 1-3) Rigid Body Tree
% Add two gripper bodies--'gripper1' and 'gripper2'--to the rigid body tree to match with the model in the simulator. Also add another massless body that would serve as an end effector pose: 'end_effector'. For simplicity, grippers are added as fixed joints.

clear robot 
robot = loadrobot('universalUR10','Gravity',[0 0 -9.81]);

body0 = rigidBody('end_effector'); 
body1 = rigidBody('gripper1'); 
body2 = rigidBody('gripper2'); 
body0.Mass = 0.0; 
body1.Mass = 0.01; 
body2.Mass = 0.01; 
body0.Inertia = [0 0 0 0 0 0]; 
body1.Inertia = [0.01 0.01 0.01 0 0 0]; 
body2.Inertia = [0.01 0.01 0.01 0 0 0]; 

fxjnt0 = rigidBodyJoint('fixedjnt0','fixed'); 
fxjnt1 = rigidBodyJoint('fixedjnt1','fixed');  
fxjnt2 = rigidBodyJoint('fixedjnt2','fixed'); 

setFixedTransform(fxjnt0, [1 0 0 0;0 1 0 0;0 0 1 0.035;0 0 0 1]);
setFixedTransform(fxjnt1, [1 0 0 0;0 1 0 0.03;0 0 1 0;0 0 0 1]); 
setFixedTransform(fxjnt2, [1 0 0 0;0 1 0 -0.03;0 0 1 0;0 0 0 1]); 

body0.Joint = fxjnt0; 
body1.Joint = fxjnt1; 
body2.Joint = fxjnt2; 
addBody(robot, body0, 'tool0'); 
addBody(robot, body1, 'tool0'); 
addBody(robot, body2, 'tool0'); 

robot.showdetails(); 

%% 1-4) Initial Conditions
% Visualize the robot on MATLAB side. This way you can compare to see if robot joints are connected as expected.
% Set the initial configuration of the robot and find the corresponding end effector transformation matrix. The initial joint positions in Simulink model are [0 -70 140 0 0 0]. They are used in constant blocks (as callback) for joints and inverse kinematic solver. Corresponding end effector transform matrix is fed to the Stateflow chart.

q0 = robot.homeConfiguration; 
q0vec = [0 -70 140 0 0 0]' * pi/180; 
len = length(q0); 
for idx = 1:len
    q0(idx).JointPosition = q0vec(idx);  
end 
tform0 = getTransform(robot, q0, 'end_effector'); 
show(robot, q0);

%% 1-5) Customize Bus Objects for Simulation
% Gazebo signals need some processing before we can read the signals properly.
% Load the custom bus objects.

load('custom_busobjects'); 

%
% To do so manually, go to bus editor: for Gazebo_SL_Bus_gazebo_msgs_JointState->joint_position and joint_velocity, change Dimension to 1 and DimensionMode to 'Fixed'; for Gazebo_SL_Bus_gazebo_msgs_Image->data bus signal, change Dimension to 307200 (Gazebo RGB sensor 320*240*4) and DimensionMode to 'Variable'.
%
%% 1-6) Check Joint Sensor
% There are a lot of moving parts in the model: task scheduling chart, inverse kinematics, low level controller, and camera sensor. We will break down the task to test components one by one. We will use Manual Switches to change operation modes. For joint sensor, we can switch to User Input mode to interactively change the joint position.
% Flip the Joint Reference Switch to use User Input for joint reference:

set_param('gazeboCosimRobotArm/Joint Reference Switch','sw','1'); 

% You can also just double click the manual switch.
%
% The interactive dashboard allows you to change the joint reference while the simulation is running.
%
% By changing the actuator type with variant subsystem, Simulink can directly set the joint position. This way you can still test movements even if you do not have a joint controller.
% Set the actuator type to be motion actuated

actuator_type = 1; 

%
% Verify that joints are connected correctly by changing the joint reference interactively.

open_system('gazeboCosimRobotArm/User Input: Joint Position');

%
%% 2) Joint Controller
% Now that the joint sensors are working, we can test the low level controller. In this example, we are using  the Computed Torque Controller without the feed-forward component from the reference acceleration. Also, the feed-back term is separately added. For more details, see Build Computed Torque Controller Using Robotics Manipulator Blocks in the Perform Safe Trajectory Tracking Control Using Robotics Manipulator Blocks example.
%
% Set the actuator type to be torque actuated

actuator_type = 2; 

% Verify that the joint controllers are working by changing the joint reference interactively.

open_system('gazeboCosimRobotArm/User Input: Joint Position');

%
%% 3) Inverse Kinematics
% Once joint controllers are working, we can test the inverse kinematics by giving end effector position and orientation as the input. Joint angles will be calculated by the Inverse Kinematics Solver block.
%
% Double click Joint Reference Switch again to use the output from the Inverse Kinematics solver.

set_param('gazeboCosimRobotArm/Joint Reference Switch','sw','0'); 

%
% Verify inverse kinematics by changing the end effector reference interactively.

open_system('gazeboCosimRobotArm/User Input: End Effector'); 

%
%
%% 4) Task Scheduling
% This is where we close the outer loop. Stateflow chart is a simple state machine that will generate trajectory. There are two modes of operations: User Input and Automatic mode. In User Input mode, desired end effector pose can be changed from the User Input: End Effector subsystem block as seen earlier. In Automatic mode, the robot automatically picks the target object and places on the other side. The series of tasks the robot go through are nine states. Trajectories are generated with cubicpolytraj to create a smooth trajectory between two points.
%
% Run simulation and press the switch operationMode to toggle to automatic mode.
%

open_system('gazeboCosimRobotArm/Chart');

%
%
%% 5) Camera Sensor
% We use a similar approach here to simplify the sensor model when you do not want to use the camera. To get the pose of target object, Simulink can simply use the ground truth that is read directly from the simulator, or it can use the information from a sensor to extract the pose of target object using computer vision algorithms. In this example a simple technique was used to extract red intensity from the RGB camera. Then 'Blob Analysis' was used to extract the planar position of the red object.
%
% Toggle the manual switch to use information from the camera sensor.

set_param('gazeboCosimRobotArm/Target Sensor Switch','sw','0'); 

%
% Set different target position to test the sensor algorithm:

set_param('gazeboCosimRobotArm/Gazebo Model/target_x','Value','0.5');
set_param('gazeboCosimRobotArm/Gazebo Model/target_y','Value','0.4');

%
% Verify that the sensor algorithm is calculating the correct pose.

open_system('gazeboCosimRobotArm/Chart');
set_param('gazeboCosimRobotArm','SimulationCommand','Start')

% Copyright 2020 The MathWorks, Inc.
%