Domino Stacking Robot

Authors: Gregory Aiosa, Daniel Augustin, Michael Jenz, Chenyu Zhu

Summary

This project uses the Franka Emika Robot (FER) to manipulate dominoes into several preset patterns. Using a domino computer vision algorithm, the robot records the positions of the dominoes and then manipulates the dominoes to the goal positions before toppling them.

To avoid collisions, our algorithm reorients the domino before placing it in the final position. Due to the variable height of the workspace surface, we implemented force-controlled placement of the dominoes on the table. Importantly, this force control is not usable in simulation or demo. See more in the discussion section on how to run this project in simulation.

The project relies on an accurate extrinsic calibration of the camera. We used easy_handeye2 to calibrate the camera in hand. You can install the easy_handeye2 version we used with 'vcs import < easy_handeye.repos'. Our calibration file is /place_dominoes/calibration/my_eih_calib.calib which should be moved to '~/.ros2/easy_handeye2/calibrations' for it to be published when we launch 'domino.launch.xml' by the handeye_publisher.

Demo

Squiggle.Splitscreen.mp4

Quickstart

Real Demo

Set up dominoes lying down within view of the camera and within the workspace of the robot. After setting up the robot, camera, and dominoes, run the following commands:

1. Use ros2 launch place_dominoes domino.launch.xml open_cv:=true demo:=false pattern:=straight_line.yaml.
2. Use ros2 service call /place_dominoes std_srvs/srv/Empty to start moving the dominoes.
3. Use ros2 service call /topple_dominoes std_srvs/srv/Empty to topple the dominoes after they are in their goal poses.

Tip: use squiggle.yaml or circle.yaml as other options for goal patterns.

Simulation Demo

1. Use ros2 launch place_dominoes domino.launch.xml pattern:=straight_line.yaml scene:=jig_laying.yaml demo:=True
2. Use ros2 service call /place_dominoes std_srvs/srv/Empty to start moving the dominoes.

Tip: use scene:=jig_standing.yaml as another option for start pattern.

Launchfiles

domino.launch.xml - This launchfile is the primary launchfile. Run this every time when using this project. It launches the fer_moveit launchfile (demo or real), the RealSense node, the place_dominoes node, the find_dominoes node, the apriltag_node, and the handeye_publisher node.

easy_handeye_calibrate.launch.xml - This launchfile is launched in addition to the above when doing extrinsic calibrations with easy_handeye2. It launches the easy_handeye2 calibrate.launch.py launchfile.

Nodes

place_dominoes - This node commands the movement of the robot. Using the motion_planning_interface, the node commands the robot to move the dominoes iteratively in an algorithm that first reorients and then places the dominoes in their final position.

find_dominoes - This node controls the computer vision. It is constantly monitoring the images from the RealSense topics for dominoes. When its /collect_domino_tfs service is called by the place_dominoes node it publishes the transforms of the dominoes to the tf tree.

System Architecture

Domino Movement Algorithm

The domino movement algorithm is the core algorithm repeated to move the dominoes from their start state to the goal state. It involves three parts, initial pickup, staging, and final placement. The first and last are self-explanatory, but the staging step is critical given the small size of the dominoes. The staging step is required since the form factor of the robot arm and the small size of the dominoes does not allow for the robot to correctly grasp the dominoes for final placement from the lying down position. Instead, the robot must orient the domino into the standing position before placing it in its goal position. This style of gripping the domino ensures that the grippers do not collide with neighboring dominoes in the pattern.

Domino Vision Algorithm

The domino vision algorithm identifies the initial poses of the dominoes on the table for the arm to reach for during the domino movement algorithm. The vision algorithm has two components:

1. Position ID: Finds the center of the domino using color filtering, uses depth camera and intrinsic calibration to calculate the 3D position of the domino in reference to the camera.
2. Orientation ID: Uses OpenCV bounding boxes to identify the orientation of the domino about the vertical z-axis on the table and turns this into a quaternion.

This vision algorithm makes two major assumptions, that the camera is perpendicular to the table at the time of reading, and that the table is flat. These assumptions proved to be poor assumptions since the actual height of the table relative to the robot varied drastically, thus, the dominoes were not perpendicular to the camera. These assumptions introduced a small amount of inaccuracy in the domino vision algorithm. And until we introduced the force sensing placement algorithm, the small inaccuracies in the computer vision algorithm accumulated into large inaccuracies in the final placement.

Force Sensing Placement Algorithm

The force sensing placement algorithm uses force feedback to ensure that the gripper or domino has reached the table height. This algorithm was implemented to compensate for the table not being flat and ensuring that the gripper reaches down far enough either to pick up or put down the domino. The algorithm moves the gripper down close to the table before switching to small millimeter movements down as it monitors the effort of the fer joint 2. If the effort exceeds a threshold, the robot knows it has contacted the table and stops moving down. Implementing this algorithm helped us get rid of hard coded values and increase the robustness of its performance tenfold.

One result of using this algorithm was that we needed to toggle when the collision objects were used. For example, we removed the table collision object when forcing the domino or gripper onto the table to prevent collision based planning errors. This, however, was not enough since the domino once detached from the gripper would appear stuck in the table. So, we then detached and removed the domino collision object while forcing it into the table and respawned the domino at the correct position in Rviz after. These two cases show how inaccuracies between Rviz and the real world can sometimes be turned from obstacles into tools to be exploited.