-
Notifications
You must be signed in to change notification settings - Fork 6
Tutorial1: Controlling rccar from MATLAB
Description: This tutorial covers basics of controlling a car model in Gazebo from MATLAB using the Gazebo-MATLAB Bridge in here
keywords: Gazebo, MATLAB
Create a world file called tutorial.world with the following content:
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<!-- Load rccar mode -->
<include>
<uri>model://rccar</uri>
<pose> 0 0 0.05 0 0 0</pose>
</include>
<!-- Load the world Plugin -->
<plugin name="gazebo_rosmatlab_bridge" filename="libgazebo_rosmatlab_bridge.so">
<joints>rear_left_wheel_joint;base_to_steeringblock1;rear_right_wheel_joint;base_to_steeringblock2</joints>
<links>carbody</links>
</plugin>
</world>
</sdf>
First we load the ground plane as
<include>
<uri>model://ground_plane</uri>
</include>
Next we load a rccar model:
<include>
<uri>model://rccar</uri>
<pose> 0 0 0.05 0 0 0</pose>
</include>
The pose argument above can be used to place the model anywhere in the world file. The arguments for the pose are x,y,z position and r,p,y orientation.
Finally we load the plugin needed to communicate between the Gazebo and MATLAB as
<plugin name="gazebo_rosmatlab_bridge" filename="libgazebo_rosmatlab_bridge.so">
<joints>rear_left_wheel_joint;base_to_steeringblock1;rear_right_wheel_joint;base_to_steeringblock2</joints>
<links>carbody</links>
</plugin>
The joints and links listed in the plugin are the ones we are interested in controlling from MATLAB. For example in rccar model, we are interested in controlling rear wheel joints and front steering joints. We are also interested in feedback of the car's position which is given by the carbody link.
The joints and links are provided in a semi colon separated format and full scoped names such as rccar::left_wheel_joint can also be given to disambiguate between different joints with same names.
Finally run gazebo with the above world file as
gazebo tutorial.world
This will provide some information on the terminal such as
Dbg Loading Links...
Dbg carbody
Dbg Loading Joints...
Dbg rear_left_wheel_joint;base_to_steeringblock1;rear_right_wheel_joint;base_to_steeringblock2
Dbg Wrote Names
Dbg Done Loading Plugin!
Tip: The names of these joints and links, can be found in the left side panel in gazebo under various model names like rccar etc. Try adding another joint or link to the plugin to see if you understand how to load the plugin.
To use the Matlab interface, you have to run MATLAB from the matlab_scripts folder. To start with, create a convenience class for interacting with Gazebo from Matlab.
h = GazeboMatlabSimulator; %Creates a Matlab Bridge using a helper class
This class provides various methods to control models in gazebo. It also shows up the list of links and joints loaded before as
>> h.AvailableNames{1}
ans =
'carbody'
>> h.AvailableNames{2}
ans =
'rear_left_wheel_joint''base_to_steeringblock1' 'rear_right_wheel_joint''base_to_steeringblock2'
The initial position of the car model can be set anywhere in Gazebo World from MATLAB. To do this first we create a model state and assign some values to it
modelstate = MatlabRigidBodyState;%Helper class to store rigidbody state
modelstate.position = [ 1,1,0.05];%Initial position
modelstate.orientation = rpy2quat([pi/2, 0, 0]);%Initial orientation
modelstate.linearvelocity(1) = 1;%Initial velocity
The members of the model state are self explanatory. One thing to note is that, the velocities are written in body frame.
Finally to set the model at this new state we run this command
h.SetModelState('Unicycle',modelstate);%Set the model state. Joints are not modified.
The first argument is the name of the model found in the left panel of Gazebo. To see the car move in Gazebo, you can simulate the world for short time as:
h.Step(1);%Simulate Gazebo world for 1 second.
There are two ways to control a joint from MATLAB. In the first way, torque can be applied to a joint and in the second a servo motor can be attached to the joint which provides either position or velocity control. We will show how to use both the methods below.
To apply a torque to a joint from MATLAB, first we pick the joints to be actuated. Then we set the torque to be applied to the joints. Finally we run the Gazebo simulation in the third step to move the car in the gazebo window
h.ActuatedJoints = [1;3];% The rear wheel joints
u = [1;1]; %Torques to be applied
h.Step(1.0,u);%Simulate the trajectory for 1 second with u as the torque
SelfCheck: Similar result can be achieved by controlling the steering wheels(2,4) as well.
To attach a servo to steering wheels we run the following line
h.AttachServo([2,4],[100.0;100.0;50.0],[100;-100;500;-500],0);
The first argument selects the joints we are attaching the servos to. In this case the 2nd and 4th joints are the steering joints of the car. The second argument provides the gains (kp,kd,ki) in above equation. These control the behaviour of servo motor. Look here for more explanation on servo motors.
The third arguments provide bounds on the integration error and control bounds. If you do not know what these should be you can set zeros([0,0,0,0]) for all of them which suggest there are no bounds on applied torque.
The last argument specifies the type of servo. If it is a position control servo(arg = 0), it controls the joint angle. If it is a velocity control servo, it controls the joint velocity(arg = 1). Finally to test the servo, you can give commands to servo in a similar fashion to applying torques:
h.ActuatedJoints = [2;4];% Joints to which servos are attached
u = [1;1]; %Commands to servos (Angles in radians)
h.Step(1.0,u);%Simulate the car for 1 second to see the steering angles rotate to 1 radian
Self Check: Try attaching a velocity servo to joints 1 and 3 (driving wheels) and move the car in a circle at constant velocity