Cognitive Robotics: Lab 10

Part I: Path Planning with RRTs

Copy the file Lab10.cc.fsm to your project directory.

Read the code to see how it works. This behavior moves the arm between left and right positions while avoiding obstacles. It uses the Grasper to plan the path the arm will take.

Compile and run the behavior. Make sure the robot is standing, and walk forward a few steps so that the legs are in a good, stable position, but don't let the right front leg come too far forward or it will interfere with the travel of the arm. (The robot needs to be standing so that the transformation from camera to local space will work correctly.) Use the Arm Remote Control to position the gripper somewhere on the far left side of the robot's body, with the elbow straight. Once the Lab10 behavior starts, press the green button each time you want the robot to look for obstacles and then move the arm to the other side.

Start out with no obstacles in front of the robot. The path generated is very simple. Verify that the arm sweeps back and forth in response to button presses while remaining fully extended.

Place an easter egg or a small piece of pink tape about five inches ahead of the shoulder servo. Press the green button again and see what path the robot takes.

Move the easter egg closer to the robot, so that the arm can't avoid hitting it as it tries to go from one side to the other. What does the Grasper do now?

Part II: Displaying the Path

Start this part in lab and finish it for homework.

The path produced by the path planner is a vector of joint configurations. For the arm we're using today, a joint configuration consists of three values: the shoulder, elbow, and wrist joint angles. As the arm follows the path, the tip of the gripper traces out a trajectory in the workspace. We can plot this trajectory to visualize the path the arm will take.

Modify the PlanPath code in Lab10 to plot the initial position of the gripper in local shape space as a green point. Also modify PlanPath to use GrasperRequest::computeReach, which causes the Grasper to send a GrasperEvent containing the path it computed.

Modify CheckResult to go through each of the nodes in the path that was returned and plot the gripper position as a blue point. You will have to extract the path from the GrasperEvent. Then you will have to use forward kinematics to calculate the gripper position from each set of joint angles. (Create a PostureEngine instance, set the arm joint positions using the methods it provides, and then use getPosition to obtain the gripper location.) Plot the final position of the gripper as a red point.

Hand in your code and a screenshot of local shape space by Friday, April 9.