%% Apply Forces and Torques in Gazebo
% This example illustrates a collection of ways to apply forces and torques to models in the Gazebo® simulator. First, application of torques is examined in three distinct ways using doors for illustration. Second, two TurtleBot® Create models demonstrate the forcing of compound models. Finally, object properties (bounce, in this case) are examined using basic balls.
% Prerequisites: Getting Started with Gazebo and a Simulated Turtlebot, Add, Build, and Remove Objects in Gazebo
%% Connect to Gazebo
% On your Linux® machine, start Gazebo. If you are using the virtual machine from Getting Started with Gazebo and a Simulated Turtlebot, click Gazebo Empty world on the desktop.
% Initialize ROS by replacing ipaddress with the IP address of the virtual machine. Create an instance of the ExampleHelperGazeboCommunicator class.

rosinit('http://192.168.233.133:11311')
gazebo = ExampleHelperGazeboCommunicator;

%
%% Add Moving Doors
% This section demonstrates three distinct methods for applying joint torques. In this case, doors are used.
% Create a door model and spawn three instances in the simulator. Specify the spawn position and orientation (units are meters and radians).

 doormodel = ExampleHelperGazeboModel('hinged_door','gazeboDB');
 door1 = spawnModel(gazebo,doormodel,[-1.5 2.0 0]);
 door2 = spawnModel(gazebo,doormodel,[-1.5 0.5 0],[0 0 pi]);
 door3 = spawnModel(gazebo,doormodel,[-1.5 -2.5 0]);

% All units in Gazebo are specified using SI convention. With the doors added, the world looks like this image:
%
% Note: When the Gazebo simulation is left idle, movable items often drift. If you see the doors moving slowly without a command, this behavior is normal. This happens because there is often more friction in the real world than there is in the ideal setting of the Gazebo simulator.
%
% Retrieve handles for the links and joints of the first door and display them.

 [links, joints] = getComponents(door1)

%
% For the first door, apply a torque directly to the hinge joint.
% Apply the torque to the first door using jointTorque. Doing so makes it open and stay open during the simulation. The first two lines define the stop time and effort parameters for the torque application. The second entry in the joints cell array is hinged_door::hinge. Use this in the jointTorque call.

 stopTime = 5; % Seconds 
 effort = 3.0; % Newton-meters
 jointTorque(door1, joints{2}, stopTime, effort);

%



% The second method is to apply a torque to the door link instead of the hinge joint. This method is not as clean because the torque is applied to the center of mass of the link (which is the door in this case) and is not applied around the axis of rotation. This method still produces a torque that moves the door.
% Use the applyForce function. The second entry in links is 'hinged_door::door'. Use it in the applyForce call.

 forceVector = [0 0 0];     % Newtons
 torqueVector = [0 0 3];    % Newton-meters
 applyForce(door2, links{2}, stopTime, forceVector, torqueVector);

%
%
% You can apply a force (instead of a torque) directly to the center of mass of the door for it to move. The commands are:

 forceVector = [0 -2 0];     % Newtons
 applyForce(door2, links{2}, stopTime, forceVector);

% Note: The forces are always applied from the world coordinate frame and not the object frame. When you apply this force, it continually operates in the negative y direction. It does not result in a constant torque on the door.
%
% For the third door, manually define the hinge angle without applying a force or torque.
% Use a while loop to create a swinging behavior for the door. Use the setConfig function of the ExampleHelperGazeboSpawnedModel class.

angdelta = 0.1;    % Radians
 dt = 0;            % Seconds
 angle = 0;         % Radians
 tic
 while (toc < stopTime)

     if angle > 1.5 || angle < 0     % In radians
         angdelta = -angdelta;        
     end
    
     angle = angle+angdelta;        
     setConfig(door3,joints{2},angle); 
     pause(dt);
 end

%
%
%% Create TurtleBot Objects for Manipulation
% This section demonstrates creation and external manipulation of a TurtleBot Create. It illustrates simple control of a more complex object.
% Create another TurtleBot in the world by adding the GazeboModel from the database (GazeboDB). The robot spawned is a TurtleBot Create, not a Kobuki. Apply an external torque to its right wheel.
% Note: Spawning the Create requires an internet connection.

 botmodel = ExampleHelperGazeboModel('turtlebot','gazeboDB');
 bot = spawnModel(gazebo,botmodel,[1,0,0]);

% The TurtleBot originally spawns facing along the x-axis with an angle of 0 degrees. Change the orientation to pi/2 radians (90 degrees) using this command:

 setState(bot,'orientation',[0 0 pi/2]);

%
%
% Using applyForce, make the right wheel of the TurtleBot Create move by applying an external torque to it from the  ExampleHelperGazeboSpawnedModel object.

[botlinks, botjoints] = getComponents(bot)

%
% The second entry of botjoints is 'turtlebot::create::right_wheel' Use botjoints{2} in the jointTorque call.

turnStopTime = 1;       % Seconds
turnEffort = 0.2;       % Newton-meters
jointTorque(bot, botjoints{2}, turnStopTime, turnEffort)

%
% You can experiment with application of forces to a TurtleBot base instead of to the wheels.
% Make a second TurtleBot Create with spawnModel:

 bot2 = spawnModel(gazebo,botmodel,[2,0,0]);
 [botlinks2, botjoints2] = getComponents(bot2)

%
% Apply a force to the base in the y direction. See that the base barely moves. The force is acting perpendicular to the wheel orientation.
% The first entry of botlinks2 is 'turtlebot::create::base'. Use botlinks2{1} in the applyForce call.

 applyForce(bot2,botlinks2{1},2,[0 1 0]);

% Apply a force in the x direction. The robot moves more substantially.

 applyForce(bot2,botlinks2{1},2,[1 0 0]);

% Apply a torque to the TurtleBot base to make it spin.

 applyForce(bot2,botlinks2{1},2,[0 0 0],[0 0 1]);

%
%
%% Add Bouncing Balls
% This section demonstrates the creation of two balls and exposes the 'bounce' property.
% Use the ExampleHelperGazeboModel class to create two balls in the simulation. Specify the parameters of bouncing by using addLink.

 bounce = 1;    % Unitless coefficient
 maxCorrectionVelocity = 10;    % Meters per second
 ballmodel = ExampleHelperGazeboModel('ball');
 addLink(ballmodel,'sphere',0.2,'color',[0.3 0.7 0.7 0.5],'bounce',[bounce maxCorrectionVelocity]);

%
% Spawn two balls, one on top of the other, to illustrate bouncing.

 spawnModel(gazebo,ballmodel,[0 1 2]);
 spawnModel(gazebo,ballmodel,[0 1 3]);
 
 pause(5);

% After adding the balls, the world looks like this:
%
%
%% Remove Models and Shut Down
% Clean up the models.

 exampleHelperGazeboCleanupApplyForces;

% Clear the workspace of publishers, subscribers, and other ROS-related objects when you finish with them.

clear

% Use rosshutdown once you are done working with the ROS network. Shut down the global node and disconnect from Gazebo.

rosshutdown

% When finished, close the Gazebo window on your virtual machine
%
%% Next Steps
% Refer to the next example: Test Robot Autonomy in Simulation
%
% Copyright 2014-2018 The MathWorks, Inc.
%