AA_API void
aa_rx_ksol_opts_center_configs( struct aa_rx_ksol_opts *opts,
const struct aa_rx_sg_sub *ssg,
double gain )
{
size_t n_qs = aa_rx_sg_sub_config_count(ssg);
double *q_ref = AA_NEW_AR(double, n_qs);
double *q_gain = AA_NEW_AR(double, n_qs);
aa_rx_sg_sub_center_configs( ssg, n_qs, q_ref );
for( size_t i = 0; i < n_qs; i ++ ) {
double min=0 ,max=0;
aa_rx_config_id config_id = aa_rx_sg_sub_config(ssg, i);
int r = aa_rx_sg_get_limit_pos( ssg->scenegraph, config_id, &min, &max );
if( 0 == r ) {
q_gain[i] = gain * (2*M_PI) / (max - min); // scale gain based on range
} else {
q_gain[i] = 0;
}
}
aa_rx_ksol_opts_take_config( opts, n_qs, q_ref, AA_MEM_STEAL );
aa_rx_ksol_opts_take_gain_config( opts, n_qs, q_gain, AA_MEM_STEAL );
}
AA_API void
aa_rx_ksol_opts_center_seed( struct aa_rx_ksol_opts *opts, const struct aa_rx_sg_sub *ssg )
{
size_t n_qs = aa_rx_sg_sub_config_count(ssg);
size_t n_qa = aa_rx_sg_sub_all_config_count(ssg);
double q_s[n_qs];
double *q_a = AA_NEW0_AR(double, n_qa);
aa_rx_sg_sub_center_configs( ssg, n_qs, q_s );
aa_rx_sg_sub_config_set( ssg, n_qs, q_s,
n_qa, q_a );
aa_rx_ksol_opts_take_seed( opts, n_qa, q_a, AA_MEM_STEAL );
}
AA_API void
aa_rx_sg_sub_center_configs( const struct aa_rx_sg_sub *ssg,
size_t n, double *q )
{
size_t n_qs = aa_rx_sg_sub_config_count(ssg);
size_t n_min = AA_MIN(n,n_qs);
for( size_t i = 0; i < n_min; i ++ ) {
double min=0 ,max=0;
aa_rx_config_id config_id = aa_rx_sg_sub_config(ssg, i);
int r = aa_rx_sg_get_limit_pos( ssg->scenegraph, config_id, &min, &max );
if( 0 == r ) {
q[i] = (max + min) / 2;
} else {
q[i] = 0;
}
}
}
AA_API int
aa_rx_mp_set_goal( struct aa_rx_mp *mp,
size_t n_q,
double *q_subset)
{
amino::sgSpaceInformation::Ptr &si = mp->space_information;
amino::sgStateSpace *ss = si->getTypedStateSpace();
const struct aa_rx_sg_sub *ssg = ss->sub_scene_graph;
size_t n_s = aa_rx_sg_sub_config_count(ssg);
assert( n_q == n_s );
amino::sgSpaceInformation::ScopedStateType state(mp->space_information);
ss->copy_state( q_subset, state.get() );
mp->validity_checker->allow();
if( si->isValid( state.get() ) ) {
mp->problem_definition->setGoalState(state);
return AA_RX_OK;
} else {
return AA_RX_INVALID_STATE;
}
}
