Exemple #1
0
static void theta2quat() {
    double theta = (aa_frand() - 0.5) * 2 * M_PI;
    double qx[4], qy[4], qz[4];
    double Rx[9], Ry[9], Rz[9];
    double qRx[4], qRy[4], qRz[4];
    aa_tf_xangle2rotmat( theta, Rx );
    aa_tf_yangle2rotmat( theta, Ry );
    aa_tf_zangle2rotmat( theta, Rz );

    aa_tf_xangle2quat( theta, qx );
    aa_tf_yangle2quat( theta, qy );
    aa_tf_zangle2quat( theta, qz );

    aa_tf_rotmat2quat( Rx, qRx );
    aa_tf_rotmat2quat( Ry, qRy );
    aa_tf_rotmat2quat( Rz, qRz );

    aa_tf_qminimize( qx );
    aa_tf_qminimize( qRx );
    aa_tf_qminimize( qy );
    aa_tf_qminimize( qRy );
    aa_tf_qminimize( qz );
    aa_tf_qminimize( qRz );

    aveq("xangle2quat", 4, qx, qRx, 1e-6 );
    aveq("yangle2quat", 4, qy, qRy, 1e-6 );
    aveq("xangle2quat", 4, qz, qRz, 1e-6 );
}
Exemple #2
0
void regress_1() {


    aa_mem_region_t reg;
    aa_mem_region_init( &reg, 1024*32 );

    // Create list for way points
    struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( &reg );

    double r0[4] = {0,1,0,0};
    double rr[6][4];
    double rrel[4][4];
    double x0[3] = {0};

    aa_tf_zangle2quat(.5*M_PI, rrel[0]);
    aa_tf_eulerzyx2quat(0, 0, .10*M_PI, rrel[1]);
    aa_tf_eulerzyx2quat(0, 0, -.2*M_PI, rrel[2]);
    aa_tf_eulerzyx2quat(-.5*M_PI, 0, 0, rrel[3]);

    AA_MEM_CPY(rr[0], r0, 4);
    for( size_t i = 0; i < 4; i ++ ) {
        aa_tf_qmul( rr[i], rrel[i], rr[i+1] );
    }
    AA_MEM_CPY(rr[5], r0, 4);

    size_t diff = 2;
    for( size_t i = 0; i < sizeof(rr)/sizeof(rr[0])-diff; i ++ ) {
        rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, rr[i+diff], x0 );
    }

    // generate trajectory
    struct rfx_trajx_seg_list *seglist =
        //rfx_trajx_splend_generate( plist, &reg );
        rfx_trajx_parablend_generate( plist, &reg );


    // plot trajectory
    rfx_trajx_seg_list_plot( seglist, .001, NULL );
}