Cognitive Robotics: Lab 10

Part I: Path Planning with RRTs

Copy the file Lab10.h.fsm to your project directory and include it in UserBehaviors.h.

Skim the code to see how it works. This behavior moves the arm between far left and far right positions while avoiding obstacles. It uses an RRT 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.) Once the Lab10 behavior starts, press the green button each time you want the robot to look for obstacles and then move the arm.

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 path planner 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 ExecutePath code in Lab10 to plot the initial position of the gripper in local shape space as a green point. Then go through each of the nodes in the path and plot the gripper position as a blue point. (Note: you will have to use forward kinematics to calculate the gripper position from the joint angles.) 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 10.