TSR visualization psuedocode

gistfile1.py

# Compute the base to gripper offset for the robot.
T0_e = robot.GetLink('grasp_frame').GetTransform()
T0_b = robot.GetTransform()
Te_b = numpy.dot(numpy.linalg.inv(T0_e), T0_b)

# Turn off everything except things named 'gripper'!
# Autoscale the window by initially centering the gripper.
for link in robot.GetLinks():
    if 'gripper' not in link.GetName():
        link.SetVisible(False)
robot.SetTransform(Te_b)
env.SetViewer('qtcoin')

# Define a TSR viewing function.
def view_tsr_list(tsrlist, idx=None):
    global model
    is_running = True

    # Create a function that just loops displaying TSR samples.
    def sample_tsr_list():
        while is_running:

            # Retrieve a TSR from the database model.
            # Use the specified index if possible.
            if idx is not None:
                tsr = tsrlist[idx]
            else:
                tsr = tsrlist[numpy.random.randint(len(tsrlist))]

            # Display useful debugging frames for the TSR.
            with env:
                # Origin
                origin_axes = openravepy.misc.DrawAxes(env, numpy.eye(4),
                                                       linewidth=3.0,
                                                       dist=0.2)
                # Initial task frame
                task_axes = openravepy.misc.DrawAxes(env, tsr.T0_w,
                                                     linewidth=2.0,
                                                     dist=0.2)
                T0_g = tsr.sample()
                T0_ws = numpy.dot(T0_g, numpy.linalg.inv(tsr.Tw_e))

                # Sampled task frame
                sample_axes = openravepy.misc.DrawAxes(env, T0_ws,
                                                       linewidth=2.0,
                                                       dist=0.2)

                # Sampled grasp frame
                grasp_axes = openravepy.misc.DrawAxes(env, T0_g,
                                                      linewidth=1.0,
                                                      dist=0.2)

                # Place gripper at the grasp frame.
                robot.SetTransform(numpy.dot(T0_g, Te_b))

                # Eliminate unused variable lint warning.
                [origin_axes, task_axes, sample_axes, grasp_axes]

            # Wait to generate next pose.
            time.sleep(1.0/args.framerate)

    # Start a visualization thread and wait for user input.
    th = threading.Thread(target=sample_tsr_list)
    th.start()
    try:
        raw_input('[ENTER] to exit.')
    finally:
        is_running = False
        th.join()

# Display the specified grasp and potentially index.
print "## Displaying TSR [{}] ##".format(
    args.index if args.index is not None else "ALL")
view_tsr_list(tsrlist, args.index)

# If debugging, start a console now.
if args.debug:
    InteractiveShellEmbed(banner1='## TSR Viewing Console ##',
                          banner2='## Use Ctrl+D to exit. ##')()