38 #ifndef AMINO_RX_SCENE_KIN_H
39 #define AMINO_RX_SCENE_KIN_H
109 aa_rx_sg_sub_config_get(
110 const struct aa_rx_sg_sub *ssg,
111 size_t n_all,
const double *config_all,
112 size_t n_subset,
double *config_subset );
115 aa_rx_sg_sub_config_set(
116 const struct aa_rx_sg_sub *ssg,
117 size_t n_sub,
const double *config_subset,
118 size_t n_all,
double *config_all
125 AA_API struct aa_rx_sg_sub *
135 size_t n,
double *q );
144 size_t *rows,
size_t *cols );
151 size_t n_tf,
const double *TF_abs,
size_t ld_TF,
152 double *J,
size_t ld_J );
160 size_t n_tf,
const double *TF,
size_t ld_TF,
161 size_t n_q,
double *q );
164 struct aa_rx_ksol_opts;
166 AA_API struct aa_rx_ksol_opts*
167 aa_rx_ksol_opts_create();
170 aa_rx_ksol_opts_destroy(
struct aa_rx_ksol_opts *opts);
173 aa_rx_ksol_opts_set_dt(
struct aa_rx_ksol_opts *opts,
double dt);
176 aa_rx_ksol_opts_set_tol_angle(
struct aa_rx_ksol_opts *opts,
double tol);
179 aa_rx_ksol_opts_set_tol_trans(
struct aa_rx_ksol_opts *opts,
double tol);
182 aa_rx_ksol_opts_set_tol_angle_svd(
struct aa_rx_ksol_opts *opts,
double tol);
185 aa_rx_ksol_opts_set_tol_trans_svd(
struct aa_rx_ksol_opts *opts,
double tol);
188 aa_rx_ksol_opts_set_tol_dq(
struct aa_rx_ksol_opts *opts,
double tol);
191 aa_rx_ksol_opts_set_tol_k_dls(
struct aa_rx_ksol_opts *opts,
double s2min);
194 aa_rx_ksol_opts_set_tol_s2min(
struct aa_rx_ksol_opts *opts,
double s2min);
197 aa_rx_ksol_opts_set_gain_angle(
struct aa_rx_ksol_opts *opts,
double k );
200 aa_rx_ksol_opts_set_gain_trans(
struct aa_rx_ksol_opts *opts,
double k );
203 aa_rx_ksol_opts_set_max_iterations(
struct aa_rx_ksol_opts *opts,
size_t n );
206 aa_rx_ksol_opts_take_config(
struct aa_rx_ksol_opts *opts,
size_t n_q,
207 double *q,
enum aa_mem_refop refop );
210 aa_rx_ksol_opts_take_gain_config(
struct aa_rx_ksol_opts *opts,
size_t n_q,
211 double *q,
enum aa_mem_refop refop );
214 aa_rx_ksol_opts_take_seed(
struct aa_rx_ksol_opts *opts,
size_t n_q,
215 double *q_all,
enum aa_mem_refop refop );
219 aa_rx_ksol_opts_center_seed(
struct aa_rx_ksol_opts *opts,
220 const struct aa_rx_sg_sub *ssg );
227 const struct aa_rx_sg_sub *ssg,
232 struct aa_rx_ik_jac_cx;
234 AA_API struct aa_rx_ik_jac_cx *
235 aa_rx_ik_jac_cx_create(
const struct aa_rx_sg_sub *ssg,
const struct aa_rx_ksol_opts *opts );
238 aa_rx_ik_jac_cx_destroy(
struct aa_rx_ik_jac_cx *cx );
241 AA_API int aa_rx_ik_jac_solve(
const struct aa_rx_ik_jac_cx *context,
242 size_t n_tf,
const double *TF,
size_t ld_TF,
243 size_t n_q,
double *q );
245 AA_API int aa_rx_ik_jac_fun(
void *context,
246 size_t n_tf,
const double *TF,
size_t ld_TF,
247 size_t n_q,
double *q );
AA_API void aa_rx_ksol_opts_center_configs(struct aa_rx_ksol_opts *opts, const struct aa_rx_sg_sub *ssg, double gain)
Convenience function to set IK options to center joints.
int aa_rx_ik_fun(void *context, size_t n_tf, const double *TF, size_t ld_TF, size_t n_q, double *q)
General type for an IK solver function.
AA_API size_t aa_rx_sg_sub_frame_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of frames in the scenegraph subset.
signed long aa_rx_frame_id
Type for frame indices.
AA_API aa_rx_config_id * aa_rx_sg_sub_configs(const struct aa_rx_sg_sub *sg_sub)
Return the array of full scenegraph config ids contained in the sub-scenegraph.
AA_API void aa_rx_sg_sub_jacobian(const struct aa_rx_sg_sub *ssg, size_t n_tf, const double *TF_abs, size_t ld_TF, double *J, size_t ld_J)
Compute the Jacobian matrix for the sub-scenegraph.
AA_API void aa_rx_sg_sub_destroy(struct aa_rx_sg_sub *sg)
Destroy the scengraph subset.
AA_API struct aa_rx_sg_sub * aa_rx_sg_chain_create(const struct aa_rx_sg *sg, aa_rx_frame_id root, aa_rx_frame_id tip)
Create a sub-scenegraph for the kinematic chain starting at root and ending a tip.
The scenegraph data structure.
AA_API void aa_rx_sg_sub_center_configs(const struct aa_rx_sg_sub *ssg, size_t n, double *q)
Fill q with the centered positions of each configuration.
AA_API size_t aa_rx_sg_sub_config_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of configuration variables in the scenegraph subset.
AA_API size_t aa_rx_sg_sub_all_config_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of configuration variables in the full scenegraph.
AA_API aa_rx_frame_id aa_rx_sg_sub_frame(const struct aa_rx_sg_sub *sg_sub, size_t i)
Return the full scenegraph frame id for the i'th frame of the sub-scenegraph.
Opaque type for a scene_graph.
AA_API aa_rx_frame_id * aa_rx_sg_sub_frames(const struct aa_rx_sg_sub *sg_sub)
Return the array of full scenegraph frame ids contained in the sub-scenegraph.
AA_API void aa_rx_sg_sub_jacobian_size(const struct aa_rx_sg_sub *ssg, size_t *rows, size_t *cols)
Determine the size of the Jacobian matrix for the sub-scenegraph.
#define AA_API
calling and name mangling convention for functions
signed long aa_rx_config_id
Type for configuration indices.
AA_API aa_rx_config_id aa_rx_sg_sub_config(const struct aa_rx_sg_sub *sg_sub, size_t i)
Return the full scenegraph config id for the i'th configuration of the sub-scenegraph.