AA_API void
aa_rx_sg_chain_jacobian( const struct aa_rx_sg *sg,
size_t n_tf, const double *TF_abs, size_t ld_TF,
size_t n_frames, aa_rx_frame_id *chain_frames,
size_t n_configs, aa_rx_frame_id *chain_configs,
double *J, size_t ld_J )
{
(void)chain_configs; /* Use these for multivariate frame support */
aa_rx_frame_id frame_ee = chain_frames[n_frames-1];
const double *pe = &TF_abs[(size_t)frame_ee * ld_TF] + AA_TF_QUTR_T;
for( size_t i_frame=0, i_config=0;
i_frame < n_frames && i_config < n_configs;
i_frame++ )
{
double *Jr = J + AA_TF_DX_W; // rotational part
double *Jt = J + AA_TF_DX_V; // translational part
aa_rx_frame_id frame = chain_frames[i_frame];
assert( frame >= 0 );
assert( (size_t)frame < n_tf );
enum aa_rx_frame_type ft = aa_rx_sg_frame_type(sg, frame);
switch(ft) {
case AA_RX_FRAME_FIXED:
break;
case AA_RX_FRAME_REVOLUTE:
case AA_RX_FRAME_PRISMATIC: {
const double *a = aa_rx_sg_frame_axis(sg, frame);
const double *E = TF_abs + (size_t)frame*ld_TF;
const double *q = E+AA_TF_QUTR_Q;
const double *t = E+AA_TF_QUTR_T;
switch(ft) {
case AA_RX_FRAME_REVOLUTE: {
aa_tf_qrot(q,a,Jr);
double tmp[3];
for( size_t j = 0; j < 3; j++ ) tmp[j] = pe[j] - t[j];
aa_tf_cross(Jr, tmp, Jt);
break;
}
case AA_RX_FRAME_PRISMATIC:
AA_MEM_ZERO(Jr, 3);
aa_tf_qrot(q,a,Jt);
break;
default: assert(0);
}
i_config++;
J += ld_J;
break;
} /* end joint */
} /* end switch */
} /* end for */
}
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;
}
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;
}
