Simple_circle_blocking.m clear all close all % initialize rosshutdown % to \'clo

Question

Simple_circle_blocking.m

clear all

close all

% initialize

rosshutdown % to 'close' any previous sessions

rosinit('192.168.139.130'); % initialize Matlab ROS node

tbot = turtlebot % the data structure that allows access to the turtlebot and its sensors

% this command sets the current robot pose estimate to zero (it does not

% move the robot). This is equivalent to setting the ``world coordinate

% frame'' to coincide with the current robot frame.

resetOdometry(tbot)

% the variable robot_poses stores the history of robot pose received from odometry. It

% is declared as global for computational efficiency

global robot_poses

% we initialize robot_poses as an empty matrix with 10000 columns.

% This can store up to 1000 seconds of odometry, received at 10Hz.

robot_poses = zeros(4,10000);

% create a Matlab "timer" for receiving the odometry. This will effectively create a new thread

% that executes 'curr_pose = get_pose_from_tbot_odometry(tbot);' every 100 msec.

% this will return the current robot pose in the variable curr_pose, and

% will also store the current pose, together with its timestamp, in robot_poses

odometry_timer = timer('TimerFcn','curr_pose = get_pose_from_tbot_odometry(tbot);','Period',0.1,'ExecutionMode','fixedRate');

% start the timer. This will keep running until we stop it.

start(odometry_timer)

   

% create a Matlab timer for plotting the trajectory

plotting_timer = timer('TimerFcn','plot_trajectory(robot_poses)','Period',5,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(plotting_timer)

% make the robot move in a circle for 100 seconds

rot_vel = 0.5

lin_vel = 0.5;

% this is a "blocking function call". This means that once we call the

% function we cannot execute any other Matlab commands for 100 seconds.

setVelocity(tbot,lin_vel,rot_vel,'Time', 100)

simple_circle_non_blocking.m

clear all

close all

% initialize

rosshutdown % to 'close' any previous sessions

rosinit('192.168.139.130'); % initialize Matlab ROS node

tbot = turtlebot % the data structure that allows access to the turtlebot and its sensors

% this command sets the current robot pose estimate to zero (it does not

% move the robot). This is equivalent to setting the ``world coordinate

% frame'' to coincide with the current robot frame.

resetOdometry(tbot)

% the variable robot_poses stores the history of robot pose received from odometry. It

% is declared as global for computational efficiency

global robot_poses

% we initialize robot_poses as an empty matrix with 10000 columns.

% This can store up to 1000 seconds of odometry, received at 10Hz.

robot_poses = zeros(4,10000);

% create a Matlab "timer" for receiving the odometry. This will effectively create a new thread

% that executes 'curr_pose = get_pose_from_tbot_odometry(tbot);' every 100 msec.

% this will return the current robot pose in the variable curr_pose, and

% will also store the current pose, together with its timestamp, in robot_poses

odometry_timer = timer('TimerFcn','curr_pose = get_pose_from_tbot_odometry(tbot);','Period',0.1,'ExecutionMode','fixedRate');

% start the timer. This will keep running until we stop it.

start(odometry_timer)

  

% these are the variables that are used to define the robot velocity

lin_vel = 0; % meters per second

rot_vel = 0; % rad/second

% create a Matlab "timer" for continuoulsy sending velocity commands to the robot.

% This will create a new thread that runs the command setVelocity(tbot,lin_vel,rot_vel) every 100msec

velocity_command_timer = timer('TimerFcn','setVelocity(tbot,lin_vel,rot_vel)','Period',0.1,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(velocity_command_timer)

% create a Matlab timer for plotting the trajectory

plotting_timer = timer('TimerFcn','plot_trajectory(robot_poses)','Period',5,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(plotting_timer)

  

% make the robot move in a circle continuously

% since the commands to the robot are sent by velocity_command_timer,

% this way of commanding the robot's velocity is non-blocking.

% We are free to execute other Matlab commands here, and

% therefore, in general (but perhaps not always), this approach to

% commanding the robot's velocity is preferrable to using blocking calls

lin_vel = 0.5;

rot_vel = 0.5;

plot_trajectory.m

function plot_trajectory(robot_poses)

max_ind = max(find(robot_poses(4,:)));

figure(121)

plot(robot_poses(1,1:max_ind),robot_poses(2,1:max_ind),'.')

axis equal

xlabel('x (m)')

ylabel('y (m)')

title('Robot trajectory')

figure(122)

subplot(3,1,1)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(1,1:max_ind))

ylabel('x (m)')

title('Robot pose over time ')

subplot(3,1,2)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(2,1:max_ind))

ylabel('y (m)')

subplot(3,1,3)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1),180/pi*robot_poses(3,1:max_ind))

xlabel('Time (sec)')

ylabel('Angle (deg)')

get_pose_from_tbot_odometry.m

function curr_pose = get_pose_from_tbot_odometry(tbot)

global robot_poses

persistent call_count

%this counts how many times we've received odometry

if isempty(call_count)

    call_count=1;

end

% get the odometry data

[odom,odomdata] = getOdometry(tbot);

pose = odomdata.Pose.Pose;

% x position of the robot in the world frame

x = odomdata.Pose.Pose.Position.X;

% y position of the robot in the world frame

y = odomdata.Pose.Pose.Position.Y;

% quaternion of the robot's orientation in the world frame

q= odomdata.Pose.Pose.Orientation;

% get the angle from the quaternion

phi = 2*atan2(q.Z,q.W);

% get the timestamp of the odoemtry reading

time = odomdata.Header.Stamp;

% get the time in seconds

t = time.Sec + time.Nsec/1e9;

robot_poses(:,call_count) = [x;y;phi;t];

call_count = call_count+1 ;

% return current pose also

curr_pose = [x;y;phi];

Provided sample code You are given two simple Matlab scripts, which demonstrate two different methods for controlling the robot motion, and also contain code for reading the robot's odometry and plotting the trajectory Odometry refers to the basic process by which the robot keeps track of its position and orientation as n the environment. Specifically, the robot has encoders attached to each of its powered wheels. t moves These encoders allow it to keep track of the wheels' rotation (and in turn, of the wheels' displacement) Additionally, the robot has a which measures its rotational velocity about the Z axis. By combining this information, the robot can track its pose (position and orientation) as it moves in the plane. In general, odometry is fairly accurate over short time periods, but it becomes quite inaccurate over time, as you will observe. You are given the scripts simple-circle-blocking.m and simple-circle-nonblocking.m, that you can use as the starting point for your implementations. You should carefully read the provided code, and its functionality. To run these scripts, you should run the virtual machine, and change the IP address used in the scripts to the "Virtual Machine IP" shown on your VM's desktop. When the VM is running, run the "Gazebo Empty" simulator. The provided scripts make use of Matlab timer objects. These allow us to run a ven function at regular intervals, effectively creating a multi-threaded environment. We are using the following timers: odometry-timer: this timer runs every 100 msec, and reads the current pose estimate from the robot odometry. The current estimate is returned in the variable curr-pose, which is a 3x1 vector, containing the robot's r and y position, and its orientation, in the world frame (see previous section for the definition of these). The units for rand y are meters, and is in rad. Additionally, the pose is stored in the variable robot-pos which contains the entire history of robot poses. Each column of this variable contains the r, y, of the robot pose, as well as the time, t, at which this pose estimate was plotting-timer: this timer runs every 5 sec, and plots the entire history of robot poses obtained from odometry. Two plots are generated: one showing the trajectory on the z y plat ne, and one showing the variables r, y, over time. velocity-command timer: this timer runs every 100 msec, and sends velocity commands to the robot (not used in the blocking version) Assignment 2: Moving in a square closed-loop (65 points] For this assignment we will implement the same square-motion as above, but using a very simple "closed loop" control approach. Specifically, note that in the implementation of the previous assignment we are not using the robot's ometry to control the robot motion. Therefore, even if the robot's odometry shows th t is not moving in the correct trajectory, we ignore that information comple In this assignment we will use the robot's odometry to try to correct (some of) the errors in the trajectory We begin by observing that to have the robot move n a square of size 5 x 5 m. we can implement the following approach: 1. Initialization: set the robot's desired orientation as 0 2. Define Po to be the current position of the robot. 3. Keep moving forward, while maintaining the robot's orientation as close to d* as possible, until we reach a location that is at a distance of 5 meters to Po 4. Set 5. Rotate in place, until the robot's orientation equals (within some small tolerance threshold), then repeat from Step 2 In order to keep the robot's orientation close to the desired one in Step 3, we can use a simple proportional controller (P-controller). Specifically if we define the error between the robot's current orientation, o, and the robot's desired orientation o", as Ad then we can choose the robot's rotational velocity as By choosing the robot's rotational velocity in this way, we seek to rotate the robot in a way that reduces the error between the desired and the current (estimated) orientation of the robot. Implement the approach described above, and comment on its performance compared to the open-loop approach of Assignment 1. Experiment with different valu of kp, and report what effect this choice has on the orientation, and on the error between the desired orientation and the estimat ude snapshots of the trajectory plots in your report. Hint: When computing the orientation errors, we should always make sure that the result is expressed within the interval to avoid unjustified large corrections (for example, if the desired orientation is 0, and the orientation estimate is 2T, the computed value of Ad is 2nr. This will result in a large rotational velocity computed by equation (1) even though the orientation is in fact perfect To make sure the error is within the desired bounds, we can use the Matlab function wrapToPi

Explanation / Answer

% initialize

rosshutdown % to 'close' any previous sessions

rosinit('192.168.139.130'); % initialize Matlab ROS node

tbot = turtlebot % the data structure that allows access to the turtlebot and its sensors

% this command sets the current robot pose estimate to zero (it does not

% move the robot). This is equivalent to setting the ``world coordinate

% frame'' to coincide with the current robot frame.

resetOdometry(tbot)

% the variable robot_poses stores the history of robot pose received from odometry. It

% is declared as global for computational efficiency

global robot_poses

% we initialize robot_poses as an empty matrix with 10000 columns.

% This can store up to 1000 seconds of odometry, received at 10Hz.

robot_poses = zeros(4,10000);

% create a Matlab "timer" for receiving the odometry. This will effectively create a new thread

% that executes 'curr_pose = get_pose_from_tbot_odometry(tbot);' every 100 msec.

% this will return the current robot pose in the variable curr_pose, and

% will also store the current pose, together with its timestamp, in robot_poses

odometry_timer = timer('TimerFcn','curr_pose = get_pose_from_tbot_odometry(tbot);','Period',0.1,'ExecutionMode','fixedRate');

% start the timer. This will keep running until we stop it.

start(odometry_timer)

   

% create a Matlab timer for plotting the trajectory

plotting_timer = timer('TimerFcn','plot_trajectory(robot_poses)','Period',5,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(plotting_timer)

% make the robot move in a circle for 100 seconds

rot_vel = 0.5

lin_vel = 0.5;

% this is a "blocking function call". This means that once we call the

% function we cannot execute any other Matlab commands for 100 seconds.

setVelocity(tbot,lin_vel,rot_vel,'Time', 100)

simple_circle_non_blocking.m

clear all

close all

% initialize

rosshutdown % to 'close' any previous sessions

rosinit('192.168.139.130'); % initialize Matlab ROS node

tbot = turtlebot % the data structure that allows access to the turtlebot and its sensors

% this command sets the current robot pose estimate to zero (it does not

% move the robot). This is equivalent to setting the ``world coordinate

% frame'' to coincide with the current robot frame.

resetOdometry(tbot)

% the variable robot_poses stores the history of robot pose received from odometry. It

% is declared as global for computational efficiency

global robot_poses

% we initialize robot_poses as an empty matrix with 10000 columns.

% This can store up to 1000 seconds of odometry, received at 10Hz.

robot_poses = zeros(4,10000);

% create a Matlab "timer" for receiving the odometry. This will effectively create a new thread

% that executes 'curr_pose = get_pose_from_tbot_odometry(tbot);' every 100 msec.

% this will return the current robot pose in the variable curr_pose, and

% will also store the current pose, together with its timestamp, in robot_poses

odometry_timer = timer('TimerFcn','curr_pose = get_pose_from_tbot_odometry(tbot);','Period',0.1,'ExecutionMode','fixedRate');

% start the timer. This will keep running until we stop it.

start(odometry_timer)

  

% these are the variables that are used to define the robot velocity

lin_vel = 0; % meters per second

rot_vel = 0; % rad/second

% create a Matlab "timer" for continuoulsy sending velocity commands to the robot.

% This will create a new thread that runs the command setVelocity(tbot,lin_vel,rot_vel) every 100msec

velocity_command_timer = timer('TimerFcn','setVelocity(tbot,lin_vel,rot_vel)','Period',0.1,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(velocity_command_timer)

% create a Matlab timer for plotting the trajectory

plotting_timer = timer('TimerFcn','plot_trajectory(robot_poses)','Period',5,'ExecutionMode','fixedSpacing');

% start the timer. This will keep running until we stop it.

start(plotting_timer)

  

% make the robot move in a circle continuously

% since the commands to the robot are sent by velocity_command_timer,

% this way of commanding the robot's velocity is non-blocking.

% We are free to execute other Matlab commands here, and

% therefore, in general (but perhaps not always), this approach to

% commanding the robot's velocity is preferrable to using blocking calls

lin_vel = 0.5;

rot_vel = 0.5;

plot_trajectory.m

function plot_trajectory(robot_poses)

max_ind = max(find(robot_poses(4,:)));

figure(121)

plot(robot_poses(1,1:max_ind),robot_poses(2,1:max_ind),'.')

axis equal

xlabel('x (m)')

ylabel('y (m)')

title('Robot trajectory')

figure(122)

subplot(3,1,1)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(1,1:max_ind))

ylabel('x (m)')

title('Robot pose over time ')

subplot(3,1,2)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1), robot_poses(2,1:max_ind))

ylabel('y (m)')

subplot(3,1,3)

plot(robot_poses(4,1:max_ind)-robot_poses(4,1),180/pi*robot_poses(3,1:max_ind))

xlabel('Time (sec)')

ylabel('Angle (deg)')

get_pose_from_tbot_odometry.m

function curr_pose = get_pose_from_tbot_odometry(tbot)

global robot_poses

persistent call_count

%this counts how many times we've received odometry

if isempty(call_count)

    call_count=1;

end

% get the odometry data

[odom,odomdata] = getOdometry(tbot);

pose = odomdata.Pose.Pose;

% x position of the robot in the world frame

x = odomdata.Pose.Pose.Position.X;

% y position of the robot in the world frame

y = odomdata.Pose.Pose.Position.Y;

% quaternion of the robot's orientation in the world frame

q= odomdata.Pose.Pose.Orientation;

% get the angle from the quaternion

phi = 2*atan2(q.Z,q.W);

% get the timestamp of the odoemtry reading

time = odomdata.Header.Stamp;

% get the time in seconds

t = time.Sec + time.Nsec/1e9;

robot_poses(:,call_count) = [x;y;phi;t];

call_count = call_count+1 ;

% return current pose also

curr_pose = [x;y;phi];

Related Questions

Navigate

• Browse (All)
• Browse S
• Subjects

---

---

Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.