www.gusucode.com > robotexamples 工具箱 matlab源码程序 > robotexamples/ros/GazeboApplyForcesExample.m
%% Apply Forces and Torques in Gazebo
%% Introduction
% This example illustrates a collection of ways to apply forces and torques
% to models in the Gazebo(R) simulator. First, application of torques is
% examined in three distinct ways using doors for illustration. Second, two
% TurtleBot(R) Create models demonstrate the forcing of
% compound models. Finally, object properties (bounce, in this case) are examined
% using basic balls.
%
% Prerequisites: <docid:robotics_examples.example-GettingStartedWithGazeboExample>,
% <docid:robotics_examples.example-GazeboModelAndSimulationPropertiesExample>,
% <docid:robotics_examples.example-GazeboAddBuildAndRemoveObjectsExample>
% Copyright 2014-2015 The MathWorks, Inc.
%% Connect to Gazebo
% * On your Linux(R) machine, start Gazebo. If you are using the virtual
% machine from <docid:robotics_examples.example-GettingStartedWithGazeboExample>, use the Gazebo
% Empty world.
% * Initialize ROS by replacing the sample IP address (192.168.1.1) with the IP address of the
% virtual machine. Create an instance of the |ExampleHelperGazeboCommunicator|
% class.
%
% ipaddress = '192.168.1.1'
%
%%
rosinit(ipaddress)
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 graphic:
%
% <<three_doors_world.png>>
%
% *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 joints of the first door and display them
%%
[links, joints] = getComponents(door1)
%%
% The output should look like this:
%
% <<doors_links.png>>
%
% 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);
%%
%
% <<one_open_door.png>>
%
% 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);
%%
%
% <<two_open_doors.png>>
%
% * 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
%%
%
% <<three_open_doors.png>>
%% 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]);
%%
%
% <<one_tbot_create.png>>
%
% * 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)
%%
%
% <<tbot_output.png>>
%
% * 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)
%%
%
% <<one_tbot_moving.png>>
%
% 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)
%%
%
% <<tbot2_list.png>>
%
% <<two_tbot_create.png>>
%
% * 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]);
%%
%
% <<two_tbot_moving.png>>
%% 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 graphic:
%
% <<bouncing_balls.png>>
%% Remove Models and Shut Down
% * Clean up the models.
%%
exampleHelperGazeboCleanupApplyForces;
%%
% * It is good practice to clear the workspace of publishers,
% subscribers, and other ROS-related objects when you finish with them.
%%
clear
%%
% * It is recommended to use |<docid:robotics_ref.bupf5_j_8 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: <docid:robotics_examples.example-GazeboRobotAutonomyExample>
%%
displayEndOfDemoMessage(mfilename)