static void eulerzyx(const double *q) { double qm[4], e_q[3], e_R[3]; aa_tf_quat2eulerzyx(q,e_q); aa_tf_qminimize2( q, qm ); { double q_e[4]; aa_tf_eulerzyx2quat( e_q[0], e_q[1], e_q[2], q_e ); aa_tf_qminimize( q_e ); aveq("quat->euler->quat", 4, qm, q_e, .001 ); } { double R[9], q_e[4]; aa_tf_quat2rotmat(q, R); aa_tf_rotmat2eulerzyx(R,e_R); aa_tf_eulerzyx2quat( e_R[0], e_R[1], e_R[2], q_e ); aa_tf_qminimize( q_e ); aveq("quat->euler->quat/rotmat->euler->quat", 4, qm, q_e, .001 ); aveq("quat->euler/rotmat->quat", 3, e_q, e_R, .001 ); } aveq("quat->euler/rotmat->quat", 3, e_q, e_R, .001 ); }
void regress_1() { aa_mem_region_t reg; aa_mem_region_init( ®, 1024*32 ); // Create list for way points struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® ); 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, ® ); rfx_trajx_parablend_generate( plist, ® ); // plot trajectory rfx_trajx_seg_list_plot( seglist, .001, NULL ); }
int better_example(void) { // create a regeion memory allocator aa_mem_region_t reg; aa_mem_region_init( ®, 1024*32 ); // Create list for way points struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® ); // waypoint translations double X[5][5] = { {0,0,0}, {1,0,0}, {1,1,0}, {1,1,1}, {0,0,0} }; // waypoint euler angles double E[5][5] = { {0,0,0}, {M_PI_2,0,0}, {M_PI_2,M_PI_2,0}, {M_PI_2,M_PI_2,M_PI_2}, {0,0,0} }; // storage for waypoint quaternions double R[5][4]; // Add waypoints to point list for( size_t i = 0; i < sizeof(X)/sizeof(X[0]); i ++ ) { aa_tf_eulerzyx2quat( E[i][0], E[i][1], E[i][2], R[i] ); rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, R[i], X[i] ); } // generate trajectory struct rfx_trajx_seg_list *seglist = rfx_trajx_splend_generate( plist, ® ); // plot trajectory //rfx_trajx_seg_list_plot( seglist, .001, NULL ); // print points for( double t = 0; t < 10; t += .05 ) { double T[12]; // column major [R | t] double dx[6]; // xyz translational velocity, xyz rotational velocity rfx_trajx_seg_list_get_dx_tfmat( seglist, t, T, dx ); printf("--\n"); aa_dump_vec( stdout, T, 12 ); aa_dump_vec( stdout, dx, 6 ); } aa_mem_region_destroy( ® ); return 0; }
void plot_viax() { aa_mem_region_t reg; aa_mem_region_init( ®, 1024*32 ); struct rfx_trajx_point_list *plist = rfx_trajx_point_list_alloc( ® ); double theta = M_PI*.9; //double X[5][5] = { {0,0,0}, {1,0,0}, {1,1,0}, {1,1,1}, {0,0,0} }; //double E[5][5] = { {0,0,0}, {M_PI_2,0,0}, {M_PI_2,M_PI_2,0}, {M_PI_2,M_PI_2,M_PI_2}, {0,0,0} }; double X[2][3] = { {0,0,0}, {0,0,0} }; double E[2][3] = { {theta,0,0}, {theta,theta,0} }; double R[5][4]; double RV[5][3]; size_t n = 2; for( size_t i = 0; i < n; i ++ ) { aa_tf_eulerzyx2quat( E[i][0], E[i][1], E[i][2], R[i] ); aa_tf_quat2rotvec( R[i], RV[i] ); rfx_trajx_point_list_addb_qv( plist, 5*(double)i, 1, R[i], X[i] ); } //rfx_trajx_generate( pT ); //rfx_trajx_plot( pT, .001, NULL ); struct rfx_trajx_seg_list *seglist = //rfx_trajx_splend_generate( plist, ® ); rfx_trajx_parablend_generate( plist, ® ); rfx_trajx_seg_list_plot( seglist, .001, NULL ); return; struct rfx_trajx_seg_list *testlist = rfx_trajx_seg_list_alloc( ® ); { double x_i[6], x_f[6]; AA_MEM_CPY(x_i, X[0], 3 ); AA_MEM_CPY(x_i+3, RV[0], 3 ); AA_MEM_CPY(x_f, X[1], 3 ); AA_MEM_CPY(x_f+3, RV[1], 3 ); struct rfx_trajx_seg *test = rfx_trajx_seg_lerp_rv_alloc( ®, 0, 1, 0, x_i, 1, x_f ) ; rfx_trajx_seg_list_add( testlist, test ); } struct rfx_trajx_seg_list *testlist2 = rfx_trajx_seg_list_alloc( ® ); { struct rfx_trajx_seg *test = rfx_trajx_seg_lerp_slerp_alloc( ®, 0, 1, 0, X[0], R[0], 1, X[1], R[1] ) ; rfx_trajx_seg_list_add( testlist2, test ); } rfx_trajx_seg_list_plot( testlist, .001, NULL ); aa_mem_region_destroy( ® ); }