The goal of our project is to implement Vision and Model Predictive Control (MPC) into a Sawyer arm to enable it to detect and intercept a ball in real time. Specifically, the sawyer should be able to:
The sawyer robot needs to know accurately where the ball is in relation to its own position. In order words, we need the X, Y, Z coordinate of the ball in the robot's frame.
We use the RealSence depth camera to obtain both the RGB and depth images, utilizing its built-in stereo vision. The camera is positioned such that the ball is in view, and the depth information of the ball can then be obtained from the depth image. The camera is at a fixed position from the sawyer so that the position of the ball in the camera frame can be easily transformed into the robot frame.
Tradeoff: RealSense D435 Camera is readily available in the lab and with ROS packages available, however they have limitations including 30 FPS limit on RGB images and accurate depth range of up to 3 meters. Additionally, we are limited to one camera due to budget. This leads to vision with shorter range, narrower field of view, and increased latency.
We use a simple HSV thresholding method to detect the ball within the RGB image from the camera. We filter the RGB image with a set of constraints on the values of H, S, and V such that only the ball pixels are left. We can then obtain the 2D coordinate of the ball with a simple averaging operation, and then combine it with depth to obtain the final 3D coordinate of the ball.
Tradeoff: HSV thresholding is very simple and overall effective, but relies on having little background noise. We used a combination of multiple techniques including Kalman filtering, DBSCAN clustering, and depth thresholding to reduce noise but the end result is still not noise-proofed. Better and more robust ball detection may require more advanced feature-based ball detection methods like using algorithms from OpenCV or training neural networks.
To transform ball coordinate in camera frame to the robot frame, we utilize AR tag tracking to simplify the setup process. We put an AR tag on the back of the camera, which can then be viewed by the Sawyer head camera to compute the position of the camera in the robot frame. Afterwards, for any 3D coordinate of the ball in camera frame, we can simply perform a frame transform to get the coordinate in the robot frame.
To detect a ball throw towards the robot and obtain the initial velocity of the ball, we define two depth planes with one closer and one farther from the sawyer. The two points used to compute the initial velocity vector are selected as the first positions that crosses each plane. This setup appears to be more robust towards different ways of throwing the ball and doesn't require computing the velocity constantly which is less susceptible to noise.
Tradeoff: This method is strongly impacted by the FPS of the camera, and requires higher FPS for faster ball velocities. Additionally, the ball must remain in view of the camera until after the second plane which limits the type of trajectories.
Using the initial velocity, we predict the future positions of the ball by computing projectile motion with basic kinematic equations. We define a strike zone around the Sawyer and select the first point in the predicted trajectory that enters the zone as the strike point, which is the target point that the end-effector of the Sawyer needs to reach.
Tradeoff: Forces other than gravity like air drag are ignored, which limits the conditions of operation.
The motion planning and actuation problem was formulated as an optimization problem, minimizing the error from desired joint states to the current ones. Additional terms in the cost function allow for tuning of the control response, and adding constraints to the problem allowed the team to easily integrate the system dynamics and account for the Sawyer's actuator limits. Solving this problem gives us an efficient sequence of open-loop control inputs to drive the arm to the desired state. Model Predictive Control solves the optimization problem at each time step in real time with the current state, and we implement the first control action determined from the solution to the optimization problem. This forms a closed-loop controller to account for model errors, ensuring robustness in the face of any system disturbances.
Tradeoff: Tradeoffs: We utilize Intera SDK's set_joint_velocities function, which provides a convenient layer of abstraction for our control scheme. However, this will slow down the controller by requiring the controller to first determine torques to achieve the desired velocities. In addition, our model assumes that joint velocities can be instantaneously assigned, an approximation that introduces error into our system dynamics predictions.