Esempio n. 1
0
int main(int argc, char *argv[])
{
    (void)argc; (void)argv;

    // Enable collision checking
    aa_rx_cl_init();

    // Initialize scene graph
    struct aa_rx_sg *scenegraph = aa_rx_dl_sg__scenegraph(NULL);
    aa_rx_sg_init(scenegraph);
    aa_rx_sg_cl_init(scenegraph);


    // setup window
    struct aa_rx_win * win = baxter_demo_setup_window(scenegraph);
    struct aa_gl_globals *globals = aa_rx_win_gl_globals(win);
    aa_gl_globals_set_show_visual(globals, 0);
    aa_gl_globals_set_show_collision(globals, 1);

    struct display_cx cx = {0};
    cx.win = win;
    cx.scenegraph = scenegraph;
    cx.i_q = aa_rx_sg_config_id(scenegraph, "left_s0");
    cx.cl = aa_rx_cl_create( scenegraph );

    {
        aa_rx_frame_id n = aa_rx_sg_frame_count(scenegraph);
        aa_rx_frame_id m = aa_rx_sg_config_count(scenegraph);
        double q[m];
        AA_MEM_ZERO(q,m);
        double TF_rel[7*n];
        double TF_abs[7*n];
        aa_rx_sg_tf(scenegraph, m, q,
                    n,
                    TF_rel, 7,
                    TF_abs, 7 );

        struct aa_rx_cl_set *allowed = aa_rx_cl_set_create( scenegraph );
        int col = aa_rx_cl_check( cx.cl, n, TF_abs, 7, allowed );
        aa_rx_cl_allow_set( cx.cl, allowed );
        aa_rx_cl_set_destroy( allowed );
    }

    aa_rx_win_set_display( win, display, &cx );
    aa_rx_win_display_loop(win);


    // Cleanup
    aa_rx_cl_destroy( cx.cl );
    aa_rx_sg_destroy(scenegraph);
    aa_rx_win_destroy(win);
    SDL_Quit();

    return 0;
}
Esempio n. 2
0
int display( struct aa_rx_win *win, void *cx_, struct aa_sdl_display_params *params )
{
    (void)win;
    struct display_cx *cx = (struct display_cx *)cx_;
    const struct timespec *now = aa_sdl_display_params_get_time_now(params);
    const struct timespec *last = aa_sdl_display_params_get_time_last(params);

    const struct aa_rx_sg *scenegraph = cx->scenegraph;

    size_t n = aa_rx_sg_frame_count(scenegraph);
    size_t m = aa_rx_sg_config_count(scenegraph);
    double q[m];
    AA_MEM_ZERO(q,m);

    int first = aa_sdl_display_params_is_first(params);
    if( ! first ) {
        double dt = aa_tm_timespec2sec( aa_tm_sub(*now, *last) );
        cx->q += dt * 45 * (M_PI/180);
    }
    q[ cx->i_q ] = cx->q;

    double TF_rel[7*n];
    double TF_abs[7*n];
    aa_rx_sg_tf(scenegraph, m, q,
                n,
                TF_rel, 7,
                TF_abs, 7 );

    aa_rx_win_display_sg_tf( cx->win, params, scenegraph,
                             n, TF_abs, 7 );

    int col = aa_rx_cl_check( cx->cl, n, TF_abs, 7, NULL );
    printf("in collision: %s\n",
           col ? "yes" : "no" );


    return 0;
}
Esempio n. 3
0
int display( void *globals_, struct aa_sdl_display_params *param )
{
    int updated = aa_sdl_display_params_get_update(param);
    if( !updated ) return 0;
    const struct aa_gl_globals *globals = (const struct aa_gl_globals *) globals_;

    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    check_error("glClearColor");

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    check_error("glClear");

    size_t n = aa_rx_sg_frame_count(scenegraph);
    double TF_rel[7*n];
    double TF_abs[7*n];
    aa_rx_sg_tf(scenegraph, 0, NULL,
                2,
                TF_rel, 7,
                TF_abs, 7 );
    aa_rx_sg_render( scenegraph, globals,
                     (size_t)n, TF_abs, 7 );
    return updated;
}
Esempio n. 4
0
int main(int argc, char *argv[])
{
    const char *name="scenegraph";
    const char *plugin=NULL;
    const char *end_effector=NULL;
    int visual = 1;
    int collision = 0;

    int print_config = 0;
    int print_frame = 0;

    /* Parse Options */
    {
        int c;
        opterr = 0;

        while( (c = getopt( argc, argv, "n:e:?cQF")) != -1 ) {
            switch(c) {
            case 'n':
                name = optarg;
                break;
            case 'c':
                visual = 0;
                collision = 1;
                break;
            case 'e':
                end_effector = optarg;
                break;
            case 'Q':
                print_config = 1;
                break;
            case 'F':
                print_frame = 1;
                break;
            case '?':
                puts("Usage: aarx-view [OPTIONS] PLUGIN_NAME\n"
                     "Viewer for Amino scene graphs"
                     "\n"
                     "Options:\n"
                     "  -n NAME         scene graph name (default: scenegraph)\n"
                     "  -e NAME         name of an end-effector to control\n"
                     "  -Q              Print configuration names and exit\n"
                     "  -F              Print frame names and exit\n"
                     "  -c              view collision geometry\n"
                     "\n"
                     "\n"
                     "Report bugs to " PACKAGE_BUGREPORT "\n" );
                exit(EXIT_SUCCESS);
                break;
            default:
                plugin=optarg;
            }

        }

        while( optind < argc ) {
            plugin = argv[optind++];
        }
    }

    if( NULL == plugin ) {
        fprintf(stderr,
                "ERROR: scene graph plugin not specified.  "
                "See `aarx-view -?` for options\n");
        exit(EXIT_FAILURE);
    }

    /* Initialize scene graph */
    struct aa_rx_sg *scenegraph = aa_rx_dl_sg(plugin, name, NULL);
    assert(scenegraph);
    aa_rx_sg_init(scenegraph); /* initialize scene graph internal structures */

    /* print things */
    if( print_config || print_frame ) {

        if( print_config ) {
            for( aa_rx_config_id i = 0; i < aa_rx_sg_config_count(scenegraph); i ++ ) {
                printf("config[%d]: %s\n", i, aa_rx_sg_config_name(scenegraph, i));
            }
        }
        if( print_frame ) {
            for( aa_rx_frame_id i = 0; i < aa_rx_sg_frame_count(scenegraph); i ++ ) {
                printf("frame[%d]: %s\n", i, aa_rx_sg_frame_name(scenegraph, i));
            }
        }
        exit(EXIT_SUCCESS);
    }

    /* Center configurations */
    size_t m = aa_rx_sg_config_count(scenegraph);
    double q[m];
    for(size_t i = 0; i < m; i ++ ) {
        double min=0,max=0;
        aa_rx_sg_get_limit_pos(scenegraph,(aa_rx_config_id)i,&min,&max);
        q[i] = (max + min)/2;
    }

    /* setup window */
    struct aa_rx_win * win =
        aa_rx_win_default_create ( "Amino: AARX-View", SCREEN_WIDTH, SCREEN_HEIGHT );

    /* Set the scenegraph for the window */
    struct aa_rx_sg_sub *sub = NULL;
    if( end_effector ) {

        aa_rx_frame_id id = aa_rx_sg_frame_id(scenegraph, end_effector);
        if( AA_RX_FRAME_NONE == id ) {
            fprintf(stderr, "Could not find frame `%s'\n", end_effector);
            exit(EXIT_FAILURE);
        }
        sub = aa_rx_sg_chain_create( scenegraph, AA_RX_FRAME_ROOT, id);
        aa_rx_win_set_sg_sub(win, sub);
    } else {
        aa_rx_win_set_sg(win, scenegraph);
    }
    aa_rx_win_set_config(win, m, q);

    struct aa_gl_globals *globals = aa_rx_win_gl_globals(win);
    aa_gl_globals_set_show_visual(globals, visual);
    aa_gl_globals_set_show_collision(globals, collision);

    /* start display */
    aa_rx_win_run();

    /* Cleanup */
    aa_rx_sg_destroy(scenegraph);
    aa_rx_win_destroy(win);
    SDL_Quit();

    return 0;
}