Our final project is to build a behaviour that mimics sheep herding. We specify an area that Chiara will patrol by pink tapes, and use blue / green easter eggs as wolves / sheep. The goal of this behaviour is to identify the region that is enclosed by pink tapes, determine if the easter eggs are in where they are supposed to be, and change their locations if necessary. For our setup, green eggs are the "sheep", which should always be inside the boundary, and blue eggs are the "wolves", which should be kept away from the sheep! If somehow a sheep or wolf is in the wrong place(like an evil human put it deliberately wrong to make Chiara's life harder), our good shepherd will fix this with its arm.

1. Create a complex but convex boundary using pink tape.
2. Determining which is the "inside" of the boundary.
3. Keep all the green easter eggs inside the boundary and all the blue eggs outside. Eggs on the wrong side of the boundary must be pushed to the other side.
4. Patrols the boundary and surrounding area looking for stray sheep that must be herded back to where they belong.
5. Human can move eggs around; robot must notice and respond.

You might be wondering how the robot determines which part of the plane is inside and which part is outside of the convex hull. The most natural way to tackle this problem is using some sort of global map, so that the robot knows the exactly location. But as we gradually discover that a global map relies on a lot of conditions, for instance it needs good localization, and requires the motion to be exact. This way is also more complicated.

So we decided to do everything with just local maps. Since the boundary we are working with is convex hull shaped, we can determine the inside / outside problem fairly easily even without seeing the entire region. Eggs that are inside the partial boundary will be inside the complete for sure, and vice versa.

Thus our approach can be outlined as these steps:
1. Build a local map of the environment which will contain at least part of the convex hull shaped boundary.
2. Generate the next way point using the boundary from step 1.
3. Use that boundary to check if any blue egg is inside, if there isn't any, jump to step 8.
4. Calculate the position of the blue egg, generate a target position and a direction of hitting for it.
5. Use the egg position and target position from step 4 to generate a desired pose for the robot, and walk there.
6. Build a local map for the egg we are manipulating, use RRT to correct the arm's relative position if necessary, then hit the egg towards the target.
7. Restore the robot's pose.
8. Turn towards the next way point.
9. Build a partial boundary, and check if any green eggs are outside. If there is any, use steps 4 to 7 to hit it inside.
10. Move to the way point, and turn towards the centroid of the convex hull generated in step 1.
11. Repeat step 1.

This part of the project constructs a representation of the convex shaped boundary in local shape space, and uses this information to determine whether eggs are in the correct region or not. We first use the map build request to generate a local map of all the lines / ellipses, then use a convex algorithm to build a polygon shape of the boundary. Since we are only dealing with the shapes not sketches, tekkotsu's built-in convex hull function is not very convenient. Thus we wrote a separate convex hull function that takes in vector of points, instead of a sketch.

We also generate the next way point for patrolling during this part. If we need to manipulate any egg, a pair of its position / target position will be calculated too.

Note that since the boundary is convex, the robot doesn't need to see all of it at once. The reason is if some egg are in the wrong part of the region given the partial convex hull, they are guaranteed to be in the wrong part in the complete convex hull. Since the robot is patrolling the boundary, it just need to see the parts that are close to it.
A screen shot of boundary building

We use the arm and the gripper on the Chiara robot to hit objects towards the desired destination. The manipulation behaviour takes in the position of the object, and the position of the target in local frame.

Then it calculates 2 things, a pose and a direction of hitting. When the robot is at that pose, its shoulder's x axis will be aligned with the object and perpendicular to the vector from the object position to the target position. Also the ball shall be within its arm's length. The direction is either in robot's positive y axis or negative y axis. A screen shot of the calculated pose

Afterwards, the robot will move to that pose, and use the camera to build a local map and move the gripper to the proper side of the object using RRT. Finally it will swing its arm to hit the object towards the target position. Note for now, we don't control how hard the robot hits the egg, but hopefully, in the future, we can also take the distance to the target into consideration and adjust the force accordingly.

In short, our approach is simple and fairly robust to errors introduced by walking around. The robot can identify boundary and eggs with ease, correctly determine whether they are in the correct region or not, and do some cute manipulation. Last but not least, it patrols the boundary. We meet with all the listed goals and developed some general purpose methods in the process. We think our final project is a success.

Add some visual feed back during the moving part, for now, once we calculate the destination, we just tell XWalkNode to get the robot there, but it usually ends up with some error. We bypassed this problem by setting a max range for displacement. But adding some feedback and adjustment is essential for larger scenario.