Bring it all in

Tutorial written by Yosser D'Avanzo, 2016.

At this point, we should be familiar with every section of the robot: base, chest, arms and grippers.These parts can now come together to form a more complicated task, fetch(), where we attempt to grab a bottle from a table. The robot will begin at some arbitrary position from the table denoted by an x (meters), y (meters), and theta (degrees). We will be using moveForward() and turnTo() from the “Hello World” Dance, and also the gripper() function from the Firmly Grasp It sections:

def fetch( MMM, x, y, theta ):
johnny.reset()

Note: As described before, the functions moveForward() and turnTo() work on the assumption that the wheels will not slip on the floor. In theory, we could add special wheels to mitigate this, but this still does not account for steep inclines, obstacles in the way, tipping over, or people bumping into it - there are so many ways that the robot could fail in that it would take a miracle to solve all of these. With this in mind, we reduce the problem space to only worrying about moving the robot, and we assume that nothing else will affect its locomotion. By doing this, we clarify the problem, provide insight to possible methods of implementation, and we make the issue much more approachable. For our case, we will assume that the robot will not slip on the floor, there are no obstacles other than the table, and that the robot can only begin with a positive y-coordinate value.

The moveForward() function at the beginning served its purpose in simple forwards locomotion. We can now go back and build upon it, adding modifications to allow for negative inputs that will make the robot move in reverse. This can be done by saving the sign of dist in a variable, dir, making the time delay the absolute value of dist/1.8, and multiplying both setWheelVelocity() inputs by dir to have them move in the proper direction:

Coordinating Coordinates

The coordinate system is arbitrary and can be placed anywhere, which gives us the ability to choose the best origin location. In order to narrow down the best origin location, we should consider that the easiest way to interface with a table is to approach it perpendicular to the edge we'll be approaching it from. (Approaching from any other angle means the robot will have to turn while at the table, which can cause a snag or a slip.) This example will be working with an origin where the edge of the table closest to the bottle, this same edge as the x-axis, and the perpendicular line containing the bottle being the y-axis. Viewing form the top, the x-axis is positive towards the right, the y-axis is positive away from the table, and theta starts on the x-axis and is positive counterclockwise. Setting up the coordinates in this fashion makes it easy to approach the table by zeroing out the x-coordinate, and then traveling along the y-axis. We can go ahead and store the location of the table's side the robot will interact with:

loc = ( 0 , 0 )

With the location of our destination and our robot's location relative to it, we can begin positioning. The first step is to move the robot to the y-axis. This requires moving to 0 degrees if he is to the left of loc or 180 degrees if to the right of loc. We should update our theta to move around properly, but not our x or y, so that we may find our way back:

Once in position, we want to move x meters in that direction, then turn towards the table (which should be towards 270 degree theta), and finally record the change:

moveForward( johnny, x )
turnTo( johnny, theta, 270 )
theta = 270

Note that if our robot begins on the y-axis, then the code will skip over any orientation in the x-direction and point directly towards the table. By this point, our robot should be able to move forward y meters to be flush with the table:

moveForward( johnny, y )

Our robot will now scan the table with an ultrasonic rangefinder while sweeping his shoulder. Using the extension arm and gripper, our robot will then reach out and grab the bottle and finally retract the arm. We will assume that the bottle is at a height approximately equal to the height of the robot's shoulder joint so that it could be found by sweeping, that the bottle is reachable by extending the arm, and that the gripper is able to hold the bottle in the air.

Sweeping the arm can be done by starting the arm at one extreme, then having a for loop increment the angle until reaching the other end. Small steps will take longer, but big steps risk missing the bottle entirely. Note that the code is running fast enough to use small steps without taking more than a few seconds total. We will start by setting the left arm to the upper limit of 180 deg, and taking 3 degree steps:

After moving the shoulders to the current step in the for loop, we should check that the ultrasonic range finder returns values within the distance we can reach with the extension arm. If so, we extend the arm to that distance and clamp the gripper, and return:

Finally, we need to know when to let go of the bottle in the left hand. The only input we can get from the user that they are ready to receive the bottle is through the ultrasonic range finder. We will do this with a call to hiFive(). At the end of hiFive(), we need to return True to tell us that he was indeed hi-fived, in which case the left arm will be raised to promptly release the bottle: