15-494/694 Cognitive Robotics Lab 13:
Cube Manipulation

I. Software Update, SDK Update, and Initial Setup

Note: This lab can be done individually or in teams of 2.

At the beginning of every lab you should update your copy of the cozmo-tools package. If you have uncommitted changes you will need to do:

$ cd ~/cozmo-tools
$ git commit -a -m Update
Then do this:
$ git pull

II. Rolling A Cube

Put cube 1 in front of the robot, make sure he can see it, and do robot.roll_cube(cube1). This is a built-in SDK action. The robot should roll the cube.

Write a RolleCube node to provide this functionality within the cozmo-tools framework (e.g., it will generate completion or failure events based on the result of the SDK action.) See the DockWithCube node in cozmo_fsm/nodes.py for a template to follow. Test your node.

III. Path Planning Next To A Wall

Put a cube up against a wall. Position Cozmo so he can see both the cube and the wall; verify this in the world map viewer. Do "show path_viewer" to see the path planner's workspace. Use PickUpCube to pick up the cube.

Now put the robot back at its starting position, and turn the cube so that the LEDs are facing the robot instead of facing upward. If you have modified PickUpCube's pick_side method successfully, it will realize that the only location from which the cube can be picked up is on the far side of the cube: the bottom face. But that goal location would put the robot in collision with the wall. Therefore, the Pilot (which is used by PickUpCube) should generate a PilotEvent with status GoalCollides.

Verify that this is the case by calling PilotToPose to try to drive the robot to a position just on the other side of the wall.

You can use a transition of form =PILOT(GoalCollides)=> to check for this situation. Write a program to do that.

IV. When To Roll A Cube

If you've detected that a cube you want to pick up is not right-side up, and the attempt to pick it up fails because you can't approach the back face without colliding with something, then you have to consider other options. One option is to roll the cube. This will both change its orientation and move it a bit away from the obstacle, such as a wall.

When rolling a cube, the robot chooses the most direct approach to the cube. So if the LEDs are facing the robot, rolling the cube will leave the cube upside down, and it can then be picked up from any side face. But if the LEDs are facing to the side instead of toward the robot, rolling the cube will leave the LEDs still facing to the side, which doesn't help, but at least the cube will be further from the wall.

For homework, write code that can pick up a cube even if it's next to a wall and not right side up. Your code should roll the cube as necessary, then call PickUpCube to complete the operation.

Note: over the weekend there will be changes to the Pilot and PickUpCube machinery to make it easier to respond to path planning failures.

Hand In

Hand in your code for picking up a cube, rolling it as necessary.

Dave Touretzky
Last modified: Fri Mar 31 05:15:38 EDT 2017