15-494/694 Cognitive Robotics: Lab 6

Learning Goal: This lab will introduce you to the Tekkotsu kinematics facility.

Part 1: CameraTrackGripper Demo

  1. Do this first step using Mirage. Go to Root Control > Framework Demos > Kinematics Demos > CameraTrackGripper. Turn on the RawCam viewer and the Arm Controller. Move the arm and note that the center of the gripper (located between the two fingers) remains centered in the camera image. Type "msg" to the Tekkotsu console a few times to display the finger-to-base transformation matrix and observe how it changes as you move the arm.

  2. Review the source code for CameraTrackGripper, which you can find in Tekkotsu/Behaviors/Demos/Kinematics.

  3. Now run CameraTrackGripper on the robot, but don't use the Arm Controller. Instead, press the Play button on the Create and the robot will say "Arm relaxed". (If it doesn't say this, press the button more firmly; make sure the sound is turned up.) Now take hold of the arm and move it around; the camera will follow. How well does the camera track the gripper on the real robot?

Part 2: GripperTrackCamera demo

  1. In this demo you will move the camera using the head controller, and the robot will move its arm to try to keep the left fingertip centered in the camera image and a constant distance away. This requires an arm with more degrees of freedom, so we're going to use the Calliope5KP in Mirage. Type the following: 
    cd /usr/local/Tekkotsu/project
./tekkotsu-CALLIOPE5KP -c mirage.plist

  2. Run the GripperTrackCamera demo. Make sure the Arm Controller is not running or it will fight with the demo. Use the Head Controller to move the head and notice that the arm moves appropriately to keep the left finger 300 mm from the camera.

  3. Review the code for GripperTrackCamera.

  4. In your ~/project directory, type "make tekkotsu-CALLIOPE5KP" to compile for the Calliope5KP robot using /usr/local/Tekkotsu as the root.

  5. Write your own kinematics demo for the Calliope5KP. Suggestion: have the robot look for an easter egg half slightly ahead (so it's within reach) but at a variable offset left or right. When it finds an easter egg, have it move the arm so that the gripper touches the egg. Or perhaps you could have it do something more elaborate, such as push the egg away. Be sure to include proper #ifdef directives, as in GripperTrackCamera, so that your demo program will not cause errors when compiling for other robot models.

Part 3: Holding Hands

Write a behavior that allows you to lead the Calliope2SP around by the hand:
  1. Start by moving the arm to a posture where the gripper is sticking out straight ahead and can easily be grasped. You can use a PostureNode for this, with the setOuputCmd method to set joint angles directly rather than loading a posture file.

  2. In your state machine, leave the posture node active so it holds the arm in this fixed position.

  3. Even with the posture node active, you can displace the arm slightly by grasping it and gently pulling or pushing. This is how we will guide the robot.

  4. Write code to calculate the position of the gripper frame and notice when it is displaced from its intended position. For vertical displacements above a threshold, have the robot walk forward or back; for horizontal displacements have it turn left or right, for as long as the displacement persists. Make the speed proportional to the magnitude of the displacement. You'll want to use a WalkNode and the setTargetVelocity method for this, rather than a PilotNode, so you can continually change the velocity in a tight sensor loop, like the other kinematics demos we've examined in this lab. You need to keep the WalkNode active, though; you can't keep exiting and reentering it because that will not leave the walk active enough to produce any motion. Instead you should define a subclass of WalkNode that does: 
      virtual void doStart() {
    subscribe to sensorEGID events
  }

  virtual void doEvent() {
    read the gripper position and call setTargetVelocity to
    change the walk's velocity while it's running
  }

What to Hand In:

Supply the following in a zip file:
  1. The source code for the kinematics demo you created in part 2, along with the source for the Mirage world.

  2. The source code for your holding hands demo from part 3.

Due Wednesday, March 18, 2015.