void aveq( const char * name,
size_t n, const double *a, const double *b, double tol ) {
if( !aa_veq(n, a, b, tol) ) {
fprintf( stderr, "FAILED: %s\n",name);
fprintf( stderr, "a: ");
aa_dump_vec( stderr, a, n );
fprintf( stderr, "b: ");
aa_dump_vec( stderr, b, n );
abort();
}
}
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 helper0( const char *name, aa_opt_gmcreate_fun fun) {
double A[] = {120, 110, 1, 210, 30, 1};
double b_u[] = {15000, 4000, 75};
double b_l[] = {-DBL_MAX, -DBL_MAX, -DBL_MAX};
double c[] = {1, 1};
double x_l[] = {0,0};
double x_u[] = {1000,1000};
double x[2];
struct aa_opt_cx *cx = fun( 3,2,
A, 3,
b_l, b_u,
c,
x_l, x_u );
aa_opt_set_direction(cx, AA_OPT_MAXIMIZE );
int r = aa_opt_solve(cx,2,x);
printf("r: %d\n", r );
aa_dump_vec( stdout, x, 2 );
double xref[] = {21.875, 53.125};
assert(sizeof(xref) == sizeof(x));
aafeq( name, xref[0] + xref[1], x[0]+x[1], 1e-3 );
}
void helper1( const char *name, aa_opt_gmcreate_fun fun) {
double A[] = {.5, 1,
2, 2,
1, 4 };
double b_u[] = {24, 60};
double b_l[] = {-DBL_MAX, -DBL_MAX };
double c[] = {6, 14, 13};
double x_l[] = {0,0, 0};
double x_u[] = {DBL_MAX,DBL_MAX,DBL_MAX};
size_t n_x = sizeof(x_l) / sizeof(x_l[0]);
size_t n_c = sizeof(b_u) / sizeof(b_u[0]);
assert( n_x == sizeof(x_l)/sizeof(double) );
assert( n_x == sizeof(x_u)/sizeof(double) );
assert( n_x == sizeof(c)/sizeof(double) );
assert( sizeof(b_l) == sizeof(b_u) );
assert( n_x * sizeof(b_u)
==
sizeof(A) );
double x[n_x];
struct aa_opt_cx *cx = fun( n_c, n_x,
A, n_c,
b_l, b_u,
c,
x_l, x_u );
aa_opt_set_direction(cx, AA_OPT_MAXIMIZE );
int r = aa_opt_solve(cx,n_x,x);
printf("r: %d\n", r );
aa_dump_vec( stdout, x, n_x );
double xref[] = {36, 0, 6 };
assert(sizeof(xref) == sizeof(x));
aafeq( name,
cblas_ddot( (int)n_x, xref, 1, c, 1 ),
cblas_ddot( (int)n_x, x, 1, c, 1 ),
1e-3 );
}
void dump(struct pir_state *state) {
double ql[4], xl[3];
aa_tf_duqu2qv( state->S_wp[PIR_LEFT], ql, xl );
printf("xl: "); aa_dump_vec( stdout, xl, 3 );
}
