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( ® );
}
