Robotic Vision Final Project
My goal for this project was to control a Baxter robot to catch a ball. My research is in robotics and I felt like this would be a fun challenging project. For the robotic vision side of things, I would need to be able to track a ball and identify the 3D position of the ball relative to the robot. For the robot control, I would need to be able to control Baxter's end effector accurately to cartesian positions. My final implementation was as shown in the block diagram below.
Each of the blocks is discussed in the sections below. The code to get the 3D ball position, update the ball trajectory, and predict the catching location is here. The code to determine the desired joint angles is here.
To get the 3D ball position relative to Baxter's base frame, I made a stereo camera pair using a laptop camera and a webcam mounted to a board (so that once I found a calibration between the two cameras, it wouldn't change). The setup is shown in the image below where 'B' is the baxter base frame, 'RC' is the right camera frame, and 'LC' is the left camera frame (note that the frames in the image are not necessarily oriented as shown, but are in the correct cartesian positions).
I used the following steps to calibrate the cameras and get 3D data:
- Calibrate each individual camera using the instructions and repository provided here. From this you should get a calibration matrix K and distortion parameters d for each camera.
- Calibrate the stereo pair using ROS by following the instructions here. Note that in this case, I wasn't using synchronized stereo cameras, so I had to pass the following arguments as instructed here: --approximate=0.01 (this makes it so it uses the closest time stamped images from each camera for the calibration) and --no-service-check (this disables the check for set_camera_info services at startup). This calibration gives you a rotation R and translation T between the two cameras. In this case it gives you the transform to move you from the right camera frame into the left camera frame; I used the inverse of this as I wanted the ball position relative to the right camera (the laptop) since it was easier to define a transformation matrix from the right camera to Baxter's base. In order to calibrate the cameras, I needed to print a large calibration board as a small one is not visible in full scale by both cameras at the same time.
- Define a transformation from the right camera to Baxter's base. I did this manually by measuring.
- Blur then threshold the image from each camera using a HSV thereshold for a blue ball (or whatever color ball you are using). Then perform multiple erosions and dilations to eliminate unwanted noise. I then passed this thresholded image into a camshift tracker with a Kalman filter in order to track the ball and get a pixel location for the center of the ball in each image.
- Convert the pixel locations to normalized image coordinates using the K and d parameters for each camera (cv2.undistortPoints).
- Pass the normalized pixel coordinates for each image along with the transformation matrix (3x4) for each camera (which you obtained from the stereo calibration using the identity for the right camera) into cv2.triangulatePoints in order to get a 3D location of the ball (note that you need to divide by the 4th element of the result to get correct results). This gives you a 3D location of the ball relative to the right camera frame. Multiply this by the transform from the right camera to Baxter's base, and you have the 3D location of the ball relative to Baxter's base.
Once, I was able to accurately track the 3D location of the ball, I defined a vertical plane in front of baxter where I wanted him to catch the ball. In order to determine where on the plane Baxter would catch the ball, I assumed the trajectory of the ball was quadratic and wrote the equations for where the parabola would intersect the plane. In order to determine the quadratic equation the ball was following, I made my own implementation of a recursive least squares algorithm so that I could update the equation the ball was following with each new point without having to take an inverse. I then simply found the intersection of this parabola with my defined plane in order to determine the 3D position I wanted Baxter to move his hand to.
I needed to determine which joint angles to command to achieve the desired position of Baxter's end effector. Baxter has a service call for inverse kinematics, but it is fairly slow and fails fairly often. I needed a robust fast way to do inverse kinematics, so I decided to use trac_ik. This is basically just a fast robust inverse kinematics ROS package. I was actually fortunate enough to find somebody who had already entered Baxter's parameters and set up a ROS package to use trac_ik with Baxter, so I used this to determine the joint angles to command Baxter to.
Finally, given the desired joint angles, I could use a joint angle controller to move the arm to the desired position. Baxter has a native joint angle controller, but it is slow and underdamped. A student in the RaD lab had recently written a Model Predictive Control joint angle controller that had a much better rise time and was significantly less underdamped. I used this controller to control Baxter's joint angles. Unfortunately, I can't share this code as it is on a private lab repository. But if you are interested in it. Please contact me and I will see if there is a way I can share it with you.
Unfortunately, at the end of this project, Baxter was not able to catch a ball thrown to him. He could however, catch a ball that was moved to him as I was holding it. The issue was speed. A thrown ball was to Baxter in less than a second which was only enough time to get maybe 10-15 ball positions for the recursive least squares algorithm updating the equation for the trajectory of the ball. It was getting a good trajectory, but by the time it was an accurate enough equation, the joint angle controller wasn't fast enough to move the arm to the desired position before it crossed the plane. If I moved the ball slowly while holding it, Baxter was able to move his arm to the correct position.
Even though I didn't reach all my goals for this project, I'm glad that I chose something cool to do and almost got there. I learned a lot during this project and this class.