Lab 31: Robot Programming by Demonstration

Next, you will be developing a miniature version of a programming by demonstration (PbD) system. This system captures the essence of how most industrial robots are programmed today. In PbD, you program the robot to do a task by moving its arm to a sequence of poses. The system saves the sequence of poses as an action and can execute the action again at a later time. An extra feature of the system you will be developing is that instead of saving poses in the base frame, your system will provide the option to specify poses as offsets relative to an AR marker.

Your PbD system will allow you to: (a) save end-effector poses relative to a marker and (b) move the robot’s arm to the same relative poses after the marker has been moved. Using this system you will be able to define different actions relative to the shelf (which will have markers attached to it) in order to roughly manipulate objects that are on the shelf. You will do this simply by saving a sequence of end-effector poses and gripper states (open/closed) relative to the marker on the shelf.

The system should allow you to demonstrate end-effector poses by physically moving the robot's arms to the desired poses. This is called "kinesthetic teaching". You will need some kind of interface (command-line or web-based) to save poses after moving the arm to each pose. For each pose, you will need to use your interface to specify which fiducial the pose is relative to (or if the pose is relative to the robot's base or torso). Once you have finished defining an action, you should use your interface to save the action to a file. You also need to build a program executor that can load an action from a file and execute it.

You should be able to use your PbD system to develop your final projects, so you will want to invest some time and do a good job with this assignment.

Example workflow

Creating an action

1. The robot starts at the table. The table has an object with a tag on it, and a tag near the edge.
2. The user presses the "Create program" button on an interface.
3. The robot's arm relaxes.
4. The user moves the arm to a pre-grasp pose and presses a "Save pose" button.
5. The interface asks if the pose should be relative to the base frame or to a tag.
6. The user says it should be relative to tag 1.
7. The user presses the "Open gripper" button.
8. The user saves another pose with the gripper in the grasp pose and saves it relative to the tag 1.
9. The user presses the "Close gripper" button.
10. The user saves another pose at the dropoff location and saves it relative to tag 2.
11. The user presses "Open gripper."
12. The user moves the arm to a final resting position and saves a pose relative to the base frame.
13. The user presses the "Save program" button, and the program is saved to a file. You will need to create a data structure to represent the program.

Executing a program

1. A separate application loads the program from disk and executes it.
2. The user rearranges the scene slightly and runs the program again.

Relaxing / freezing the arm

Whenever you move the robot's arm using MoveIt, you will notice that the arm freezes and cannot be moved by hand. To enable users to program the robot by moving the arm, you will need to relax the robot's arm. To do this, you will need to stop the robot's arm controller. However, to send MoveIt commands to the arm, you will need to start the arm controller. In short, before you create a program, stop the arm controller, and before you execute a program, start the arm controller.

To start or stop the arm controller, we will use an undocumented feature of the Fetch robot. The Fetch robot API exposes an action named /query_controller_states of type robot_controllers_msgs/QueryControllerStates. You will need to create an action client to use this action.

To stop the arm controller (relax):

goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()

To start the arm controller (before sending any MoveIt commands):

goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
self._controller_client.send_goal(goal)
self._controller_client.wait_for_result()

Transform arithmetic

You may want to revisit Lab 24: Transform arithmetic. Let's say you have ^BT_{tag1, t1} and ^BT_{wrist, t1} at t=1. At runtime (t=2), the tag tag1 is in a slightly different pose, given as ^BT_{tag1, t2}. Where should the wrist go at time=2?

Hints:

• The programmed offset between the wrist and tag is ^{tag1, t1}T_{wrist, t1}
• ^AT_B is the matrix inverse of ^BT_A
• You want to compute ^BT_{wrist, t2}

So, you will need to chain the transformations together and invert some of the matrices.

Developing in simulation

You will want to test your system in simulation first, because otherwise the robot will be too busy.

Perception

You should save a few point clouds of the table with AR tags on it and hallucinate them in simulation as explained in the previous labs.

Moving the arm

In your system, users should physically move the robot's arm. However, you can't do this in simulation. Instead, you can just teleop the arm using the teleop applications we developed in previous labs. You can also use the Tutorial: MoveIt RViz plugin.

Note that while on the real robot, you want to relax the arm, you always need to switch the arm controller on when sending MoveIt commands (e.g., when using teleop). As a result, when you are in simulation, you should not relax the arm when programming.

To know if you are in simulation, you can check if the value of use_sim_time is set on the ROS parameter server.

Summary table for the arm controller

Phase	Real world or simulation	Arm controller enabled or disabled
Program creation	Simulation	Enabled
Program creation	Real world	Disabled
Program execution	Simulation	Enabled
Program execution	Real world	Enabled

Testing PbD with On-Shelf Manipulation

To demonstrate the capabilities of your PbD system you will program (by demonstration) three robot actions relative to a shelf bin whose location relative to an AR marker is fixed. For these actions we will assume that the bin has only one relatively compliant item in it.

1. Push-in: The first action should move the robot's gripper in front of the bin and poke it into the bin slightly to move a centrally located item further into the shelf (e.g. to correct for any overhang).
2. Pull-out: The second action should move the robot's gripper into the bin close to the top, move it downwards to make contact with a centrally located object, and then pull it outwards to drop it into a tote on the floor.
3. Pick-and-place: The third action should approach the bin at the center with an open gripper, move inwards to make contact with the object, then close the gripper and extract the object by moving backwards. It should then move the object close above the tote and drop it in.

Obviously, these actions will only work with certain objects in certain configurations, as they are relative to the shelf, not the object itself. That's fine for now. When you make a video of the actions you programmed make sure to demonstrate actions that succeed. So if you find that the action fails when you test it, you need to go back and re-define your actions differently (e.g. try having a different number of poses, using different pose configurations, or adjusting the motion speed) or change the object configuration.