Code Repositories

• Primary "Blockbot" Code
• Landmark SLAM ROS Package

The LoCoBots we used are hosted by the GMU Autonomous Robotics Laboratory, and configured by Michael Schader. Some of the information here on getting started is installation dependent and may change overtime.

To start the LoCoBot, you will need to power on both the base (toggle switch on the side) and the onboard Mini PC located under the frame (push button, lights up blue). The rechargeable on board power supply provides plenty of charge for a few hours of use. Once the PC is powered on, the accompanying laptop computer can be used to SSH wirelessly to the onboard PC. Must be connected to the MASON network in the lab (not MASON SECURE). Each LoCoBot is numbered, so the for example, ssh locobot04.

The Locobot PCs are running on Ubuntu 20.04 focal and are already configured to start development. We recommend creating your own directory for your files. There are several ROS nodes that need to be launched to operate the Locobot, but these can be easily stopped and started via aliased daemon scripts. Run one of the following:

To terminate, run sa in the same terminal window. You may need to repeat and should get confirmation the processes have stopped. The LIDAR spin is a useful indicator of the state as well as the base chimes.

Interbotix provides a locobot.py module with the InterbotixLocobotXS class. If everything is running you can start controlling right away by importing this module and calling the API commands. Michael Schader created a simple bash script that runs a Python shell with the class loaded (located at ~/mds-locobot/scripts/python3.sh):

You can now start driving. Here's a simple API command to drive forward .2 m/s for 2 seconds:

Now let's rotate. This will rotate counterclockwise for 1 radian/second for two seconds. In your code we recommend using math.pi in your angular velocity units and pose angles.

To use MoveIt to plan a trajectory and execute a successful one, use locobot.arm.set_ee_cartesian_trajectory() providing the x, y, z values. When in home pose, x, y, z are all 0. Play around with the values to find which plans work and what positions are reachable.

Always make sure the arm is in the sleep pose before terminating the processes. The torque will stop, causing the arm to fall if not at the sleep pose.

The functions locobot.gripper.open(), locobot.gripper.close() can be used to open and close the end-effector of the arm.

The function locobot.base.command_velocity() could be used to move the robot, and check how much the robot thinks it moved using locobot.base.get_odom() that provides the (x, y, yaw) with respect to the origin. The origin is the position where the robot initiates or can be manually set using locobot.base.reset_odom().

To tilt the camera, pass an angle in radians to this function. 0 is parallel to the ground, positive angles are towards the ground. locobot.camera.tilt(.5)

Interacting with the Interbotix Locobot API in a shell is a great way to discover other functions and get a feel for how to control. We recommend taking some time to manually direct the robot via the shell to see what is possible. You can wrap a function around the Python help() function to get more information, or consult the Interbotix documentation.

In this project, AprilTags were used to identify the block and the landmarks. This is a useful project simplification to avoid computer vision tasks such as camera depth perception and object detection (which could be a project all on its own), as well as the error those could introduce.

The /tag_detections topic provides the list of AprilTag detections. The relevant AprilTag information was extracted from this and stored in the class variables (check get_tag_data()).

Understanding AprilTag Values:

The Interbotix provides an AprilTag Module to combine the camera information with the AprilTag pixel data to provide real meter values for the tag detection data.

The raw AprilTag detections provided by /tag_detections topic include the pose and orientation of the AprilTag with respect to the camera (see Fig. 5). To understand the values, let's consider the AprilTag with ID 5 (the tag on the robot arm in Fig. 4). The detections[0].pose.pose.pose has the required information, where position is the estimated position of the AprilTag and the orientation is the tilt of the tag in all dimensions with respect to the camera's field.

By placing AprilTags on objects or on the ground, they can be used to calculate the bearing and range from the robot's center to the center of the AprilTag. These values are always relative to the robot's pose at the time of the detection. We used this in two ways: to estimate the block's coordinates to drive close to it, and to detect relative landmark positions to aide in localization.

Fig. 9. Robot Bearing and Range (bird's-eye view)

We use 2D odometry, however the tag's position.z value is the 3D distance to the tag's center. Therefore we need to use the position.y value to project to a line parallel to the camera. We then account for the camera tilt by rotating opposite the tilt angle. This gives us the line parallel to the ground from the camera to the tag plane. In this project we do not permit the camera to pan.

To keep the measurement inline with the odometry, we further take into account that the left camera performing the tag detections is not at the center of the robot. A combination of measurements and trial-and-error led us to the values below, though you could consider adjusting, particularly the camera_radius_offset. To handle the left camera's offset from the robot's true heading center, we can simply subtract the offset from the tag's x value (since it's in real meters).

Fig. 10. Measured camera offsets (bird's-eye view)

Now we need to consider that not only does the camera_radius_offset impact the true range, but also the bearing. Here is the full trigonometry problem we can solve. Since we already accounted for the camera's x offset, we can assume the camera is centered int he diagram.

Fig. 11. Bearing/Range Triangles

We use the tag's shifted_x value to project the ground_projection to the camera center. Imagine a line parallel to the ground that goes from the camera to the plane of the tag.

Now we can add the camera_radius_offset to get the base of the larger triangle. We can now solve for the bearing and range values.

Return the values, though since the camera is upside down we need to flip the bearing angle.

Overview

In order to accurately travel to goal pose's, including returning back to the "home" position where the bin is located, the robot needs top track where it is. The Kobuki mobile base publishes the current odometry on the topic /locobot/odom. Conveniently Interbotix provides a get_odom() function that produces a simple 2D pose, rather than the quaternion.

Odometry by itself for localizing the LoCoBot isn't too bad, especially just driving in a straight line. However as it starts to make turns and move further, it starts to become inaccurate. We sought to demonstrate an effective use of Landmark SLAM to improve the LoCoBot's estimate of it's current pose.

Imagine you are lost driving in a city. You know where you started and roughly how far you've driven and maybe the direction you went, but not well enough to say exactly where you are (and no GPS signal!). You make a turn and suddenly see a pizza place you've driven past before, and then a gas station you previously stopped at. Now you know exactly where you are located.

We placed four large AprilTags on the ground to serve as stationary landmarks. At every time step the robot can register what landmark tags are visible, and their bearing and range relative to the robot. For simplicity the robot always knows which landmark it is detecting via the AprilTag ID. A future project could ignore the tag ID values and experiment with performing data association of unknown landmarks.

GTSAM

To fuse together the landmark observations, the robot's odometry readings, and the potential error in both of these, we turn to probabilistic and leverage factor graphs and Bayes Networks. A formal discussion of these are outside the scope of this tutorial (see Probabilistic Robotics). The idea is at every time step, the robot doesn't know it's exact pose, but it can use "evidence" of what it perceives to reason about it's current pose and assign a certainty value (covariance) to it. In our case the "evidence" is the current odometry and the landmark detections. A factor graph can be constructed connecting each pose to the evidence it has about its estimated value, which can then be optimized.

To handle building and solving the factor graph, we use the Georgia Tech Smoothing and Mapping (GTSAM) Library. We've implemented a Python class to encapsulate the code for building and optimizing using the library for our project setup.

First the GTSAM factor graph and any state variables ae initialized. The starting pose (always (0,0,0) for us) is added to the graph as a PriorFactorPose2 We also define a noise model. The noise variables correspond to the factors that are added to the graph. Note that ODOMETRY_NOISE is the noise in the change in pose from the previous odometry reading, i.e. how much it has moved since the last time registered observation. We set the time steps 1/10 a second. The LANDMARK_NOISE corresponds to the range and bearing calculations for the landmarks.

Each time step a new observation is added, which includes the current odometry pose, the AprilTag data for any detected landmarks, and the tilt of the camera at the time of the observation. History is updated. We track the time index each call to create a distinct pose. We do not attempt to detect a loop closure.

To add the new odometry to the graph, we use a BetweenFactorPose2. This expects the change relative to the previous pose, therefore we first take the difference. Since the poses are in the global coordinate system, to get the changes relative the previous we need to rotate based on the previous pose's yaw value.

The BetweenFactorPose2 is created and added to the graph. An initial estimate must also be added for the current pose, so we use the current odometry.

For each landmark AprilTag detection, we create a BearingRangeFactor2D connecting the current pose to the landmark. We use the tag ID value as the landmark ID.

Finally if we haven't seen a landmark before, an initial estimate must be set. A global (x, y) coordinate can be calculated by taking the odometry pose and the landmark bearing and range.

And finally optimize() can be called on demand to optimize the entire graph. In practice we do this every time step.

ROS Topic

We felt it would be most effective and ROS-like to encapsulate the Landmark SLAM tasks into its own ROS package. This would allow us to launch it independently, have it listen directly to topics to get all the data it needs, and then continuously publish to the topic /landmark_slam/estimated_pose. Now our main BlockBot class no longer needed to handle all the calls to the BlockBotLocalizer to keep the estimated pose up to date. It simply subscribes to to the /landmark_slam/estimated_pose topic and updates a class property.

Some useful ROS links:

• Creating a ROS Package
• Understanding ROS Topics
• Writing a Simple Publisher and Subscriber

The LocalizerListener creates a ROS node and runs continuously to manage the BlockBotLocalizer, listening to the following topics directly:

• /locobot/mobile_base/commands/reset_odometry: To re-instantiate the factor graph when the LoCoBot has it's odometry reset.
• /locobot/mobile_base/odom: Get the current odometry pose.
• /tag_detections: Data for all AprilTag's currently visible. The specific landmarks IDs are configured in the launch file.
• /locobot/joint_states: To lookup the current camera tilt.

Every 1/10 of a second, data from these topics are read and observations added to the GTSAM factor graph.

The launch file specifies the namespace /landmark_slam, sets the AprilTag IDs for the landmarks, and directs to run the listener script.

And finally roslaunch is used to execute the launch file. We put this along with the required "source" command in a bash script to allow a background daemon process to execute.

Assessment

To demonstrate the effectiveness of our Landmark SLAM implementation, the bin to deposit the block in is placed at the starting position, and no other identification or detection is used (we do not use the AprilTag on the bin). Once a block is picked up it is directed to travel back to the origin and deposit the block. The bin is large enough to account for small errors, but if the localization is too off it will fail to drop the block in the bin. We also marked the starting position on the floor as a visual indication to us how far off the position estimate is.

We found the the localization was effective and there was a noticeable improvement in consistently making it to the block when landmarks were used. It was most noticeable when it came to the pose angle, as without landmarks it's angle towards the box had more error. At first we faced challenges getting Landmark SLAM working due to odometry noise model being too large relative to the landmark noise. Furthermore as we improved and corrected our bearing and range calculations, the performance improved. Logging the odometry values along with estimated pose and comparing was useful to help tune and assess. It was also useful to log GTSAM's estimated landmark positions at each time step to confirm the positions were remaining consistent.

The LoCoBot is built upon the Yujin Robot's Kobuki Base, a differential drive robot.The linear and angular velocities can be commanded directly by issuing a Twist message to the topic /locobot/cmd_vel. The Interbotix API offers a function command_velocity(x=0, yaw=0) that handles the publishing. This is designed to be called repeatedly by a controller. A Proportional Integral Derivative (PID) controller is a proven method for setting control values to reach a target set point accurately with minimal oscillation. We found PID Explained to be a helpful resource.

As part of this project, we implemented our own PID controller. Specifically two Python classes:

PIDController

A generic PID Controller implementation. Designed to be passed in the error each call of stpe(), which then returns the control value. A new PID Controller instance should be created each time a new goal set point is needed. KP, KI, and KD gain values can be specified upon instantiation.

LocobotController

A higher level controller where a specific goal pose is specified, and with each call to step(current_pose) control linear and angular velocities are returned. It encapsulates tuned gained values and breaks the control into two phase: "Move to Goal Point" and "Rotate to Goal Pose Angle". We do not have any obstacles in the way of the robot, and therefor it can always take a straight line Euclidean path to the it's goal point. If only the goal point is of concern such as with a waypoint, then a value of None can be passed as the pose angle to skip the rotation phase. For example, (1, 2, None).

The euclidean distance to the (x, y) goal point from the current pose is determined. If it is within a certain TRAVEL_ACCEPTANCE_RADIUS, then the point is considered reached.

If it hasn't been reached, then calculated the relative angle from the current pose to the goal point.

Since we can set the linear and angular velocities directly, we can use separate PID Controller instances to minimize each of the distance to the goal point the relative again. If tuned right these two controllers should balance eah other to make progress towards the goal, adjusting automatically to changes in error. We add the constraint that the LoCoBot should only move forward, and therefore the error to the linear velocity controller is always positive. Controlling Physical Systems by Nathan Sprague explains the idea of separate controllers for differential drive velocity control, as well as a good overview of PID Controllers in general.

Once it has reached the goal point, it now needs to rotate to the correct pose angle. At this stage we always return zero for the linear velocity. For the angular velocity we have a separately tuned PID Controller for this phase, and pass it the error in the current pose angle and the desired angle (taking into account either direction of rotation).

Once the theta error is within the GOAL_THETA_MARGIN, we have reached the goal pose and the velocities are zeroed out. We realized it was necessary to make sure once the controller had entered the "angle" phase it no longer checked against the TRAVEL_ACCEPTANCE_RADIUS, as it could become unstable going back and forth between stages. We do find in practice that sometimes by rotating to the pose angle the distance to the goal point increases due to error. We found this was small enough to not be a large issue, but more work could be done to address.

Tuning

Tuning the gain values was challenging. We settled on values that gave us fairly consistent results, although maybe moves a little slower than it could. Use them as starting points. How to Tune a PID Controller was helpful in providing some intuition. We found it was better to be conservative with the integral terms, although we believe a small value is necessary to help overcome too slow of acceleration. One important consideration to balance the maximum angular and linear velocity values. The ratio needs to be such that if the robot is facing away from it's goal (both theta and distance errors are high), setting both values to the max will cause the robot to turn around. We do recommend setting max velocities to keep the robot under control and safe.