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 */
}
void cross() {
double a[3], b[3], c1[3], c2[3];
aa_vrand( 3, a );
aa_vrand( 3, b );
// cross
aa_tf_cross(a,b,c1);
aa_vecm_cross(a,b,c2);
aveq( "cross-equal", 3, c1, c2, .0001 );
aa_tick("cross non-vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_tf_cross(a,b,c1);
}
aa_tock();
aa_tick("cross vec: ");
for( size_t i = 0; i < N; i ++ ) {
aa_vecm_cross(a,b,c1);
}
aa_tock();
}
