Introduction | Design | Implementation: Hardware | Implementation: Software | Results & Video | Reflection | Team | Additional Materials |
Detection and TF broadcast of the current ball position in the camera frame occur via the Kinect2 Sensor and OpenCV. The Kalman Filter Node listens for the current ball position then calculates the ball trajectory and solves for the intersection point between the trajectory and a sphere with a particular radius centered at the shoulder joint. The filter publishes a PoseStamped Vector for the estimated trajectory intersection point for which the arm should immediately move to. The 7-DOF end effector PD controller takes this target pose, compares it to the current pose, and controls the arm to the correct position using current/torque control.
We also wrote two launch files. One to run all simulation files and Koko robot controller, and one to initialize the Kinect2, ball detection node, static world transform, and Kalman Filter. Below is the final system transform tree [rqt_tf_tree].
Taking the rgb_image from the Kinect2, we transform the image to the HSV color space, then apply a bitmask for a tennis neon color. Using OpenCV, we find the maximum contour and the minimum enclosing circle around the contour to obtain a (u, v) center pixel of the ball in our 2D image. After finding the (u, v) center pixel of the ball, we use the PinmodelCamera ROS package to find the 3D ray (X, Y, Z) directly from the raw_rgb, raw_depth image and camera_info. This Final Camera frame (X, Y, Z) position is then broadcasted as a TF as “ball,” which the Kalman filter Node can then listen for.
The Kalman Filter ball tracking node is a custom publisher subscriber node, which listens for published ball positions from the vision system and publishes a PoseStamped end effector pose for any predicted intersections with the boundary of the robot’s workspace. The included software can be divided into three main parts: pruning for the filter, the kalman filter, and the intersection solver. The kalman filter was written to operate within the camera frame. Because the gravity vector was known beforehand and noise was assumed to be decoupled, the problem of estimating the position and velocities of x, y, and z could be decoupled resulting in faster computation. The entire node was run once every new ball PointStamped was published, following: Pruning:
We had to write custom controller code to actuate the robot. Exposed to us were encoder values read from the robot, and input torques to each of the robots joints. This is quite challenging due to the robots extreme nonlinearities as well as having to consider the speed at which the robot must actuate to catch the ball. This means the robot must act virtually instantly once the ball trajectory is known, and therefore cannot spend time calculating optimal trajectories. A more direct control method in the end effector frame space is what we ended up controlling the arm with, as it allowed for a more natural translation between the robot control and our end objective. Seen below is the block diagram of the robot controller.
At each control loop the following occurs
Testing our code in simulation was vital for making sure the individual components of our object was working before everything could be integrated together and before the real robot hardware was working. We were able to test the integration of the ball tracking, ball trajectory estimation, and catching all in simulation. The simulator we used was Gazebo, letting us simulate the effects of gravity on the arm. We were able to integrate sensory information from the real world and inferencing that with our simulated robot. This was also useful as a sanity check to insure the robot control was working in a perfect environment before debugging the code in a real world setting.