39 #ifndef AMINO_RX_SCENE_SUB_H
40 #define AMINO_RX_SCENE_SUB_H
143 aa_rx_sg_sub_config_get(
144 const struct aa_rx_sg_sub *ssg,
145 size_t n_all,
const double *config_all,
146 size_t n_subset,
double *config_subset );
149 aa_rx_sg_sub_config_set(
150 const struct aa_rx_sg_sub *ssg,
151 size_t n_sub,
const double *config_subset,
152 size_t n_all,
double *config_all
161 const struct aa_dvec *config_subset,
169 const struct aa_dvec *config_all,
170 struct aa_dvec *config_subset );
175 AA_API struct aa_rx_sg_sub *
185 size_t n,
double *q );
188 aa_rx_sg_sub_center_configv(
const struct aa_rx_sg_sub *ssg,
193 aa_rx_sg_sub_rand_config(
const struct aa_rx_sg_sub *ssg,
210 size_t *rows,
size_t *cols );
217 size_t n_tf,
const double *TF_abs,
size_t ld_TF,
218 double *J,
size_t ld_J );
226 const struct aa_dmat *TF_abs );
249 const struct aa_rx_fk *fk );
262 const struct aa_rx_fk *fk,
276 const struct aa_rx_fk *fk,
292 const struct aa_rx_fk *fk );
305 const struct aa_rx_fk *fk,
309 aa_rx_sg_sub_jac_vel_fill(
const struct aa_rx_sg_sub *ssg,
310 const struct aa_rx_fk *fk,
336 const double *q_start,
337 const double *path_sub,
345 const struct aa_rx_sg_sub *ssg,
#define AA_API
calling and name mangling convention for functions
Scenegraph-related type declarations.
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 size_t aa_rx_sg_sub_all_frame_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of frames in the full scenegraph.
AA_API void aa_rx_sg_sub_config_gather(const struct aa_rx_sg_sub *ssg, const struct aa_dvec *config_all, struct aa_dvec *config_subset)
Fill a subset configuration vector with values from the full vector.
AA_API aa_rx_frame_id * aa_rx_sg_sub_frame_ees(const struct aa_rx_sg_sub *sg_sub)
Return an array of end effector frames, if any.
AA_API void aa_rx_sg_sub_jac_twist_fill2(const struct aa_rx_sg_sub *ssg, const struct aa_rx_fk *fk, struct aa_dmat *Jp, struct aa_dmat *Jr)
Fill in the blocks of the twist jacobian.
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.
AA_API void aa_rx_fk_sub(struct aa_rx_fk *fk, const struct aa_rx_sg_sub *ssg, const struct aa_dvec *q_sub)
Compute the forward kinematics for the sub-scenegraph.
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 void aa_rx_sg_sub_jac_vel_fill2(const struct aa_rx_sg_sub *ssg, const struct aa_rx_fk *fk, struct aa_dmat *Jp, struct aa_dmat *Jr)
Fill in the blocks of the velocity jacobian.
AA_API void aa_rx_sg_sub_expand_path(const struct aa_rx_sg_sub *ssg, size_t n_pts, const double *q_start, const double *path_sub, double *path_all)
Expand sub path to full config path.
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 double * aa_rx_sg_sub_alloc_config(const struct aa_rx_sg_sub *ssg, struct aa_mem_region *region)
Allocate sub scene graph config array.
AA_API double * aa_rx_sg_sub_alloc_jacobian(const struct aa_rx_sg_sub *ssg, struct aa_mem_region *region)
Allocate jacobian matrix for sub scene graph.
AA_API const struct aa_rx_sg * aa_rx_sg_sub_sg(const struct aa_rx_sg_sub *sg_sub)
Return the original scene graph for the sub-scenegraph.
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.
AA_API void aa_rx_sg_sub_destroy(struct aa_rx_sg_sub *sg)
Destroy the scengraph subset.
AA_API size_t aa_rx_sg_sub_frame_ee_count(const struct aa_rx_sg_sub *sg_sub)
Return the number of end effector frames, if any.
AA_API struct aa_dmat * aa_rx_sg_sub_get_jacobian(const struct aa_rx_sg_sub *ssg, struct aa_mem_region *reg, const struct aa_dmat *TF_abs)
Allocate and compute the Jacobian matrix for the sub-scenegraph.
AA_API aa_rx_frame_id aa_rx_sg_sub_frame_ee(const struct aa_rx_sg_sub *sg_sub)
Return the end-effector frame id, if any.
AA_API struct aa_dmat * aa_rx_sg_sub_jac_vel_get(const struct aa_rx_sg_sub *ssg, struct aa_mem_region *region, const struct aa_rx_fk *fk)
Allocate and fill the velocity Jacobian.
AA_API void aa_rx_sg_sub_jac_twist_fill(const struct aa_rx_sg_sub *ssg, const struct aa_rx_fk *fk, struct aa_dmat *J)
Fill in the blocks of the twist jacobian.
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 struct aa_dmat * aa_rx_sg_sub_jac_twist_get(const struct aa_rx_sg_sub *ssg, struct aa_mem_region *region, const struct aa_rx_fk *fk)
Allocate and fill the twist Jacobian.
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.
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.
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 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.
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_config_scatter(const struct aa_rx_sg_sub *ssg, const struct aa_dvec *config_subset, struct aa_dvec *config_all)
Fill a full configuration vector with values from the subset vector.
The scenegraph data structure.
signed long aa_rx_config_id
Type for configuration indices.
signed long aa_rx_frame_id
Type for frame indices.
Descriptor for a block matrix.
Data Structure for Region-Based memory allocation.
Opaque type for a scene_graph.