amino  1.0-beta2
Lightweight Robot Utility Library
scenegraph.h
Go to the documentation of this file.
1 /* -*- mode: C; c-basic-offset: 4; -*- */
2 /* ex: set shiftwidth=4 tabstop=4 expandtab: */
3 /*
4  * Copyright (c) 2015, Rice University
5  * All rights reserved.
6  *
7  * Author(s): Neil T. Dantam <ntd@rice.edu>
8  *
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of copyright holder the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
23  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
24  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
25  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
27  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
28  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
29  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
30  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
31  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef AMINO_SCENEGRAPH_H
39 #define AMINO_SCENEGRAPH_H
40 
41 #include "amino/mat.h"
42 #include "rxtype.h"
43 
52 typedef signed long aa_rx_frame_id;
53 
57 typedef signed long aa_rx_config_id;
58 
62 #define AA_RX_FRAME_ROOT ((aa_rx_frame_id)-1)
63 
67 #define AA_RX_FRAME_NONE ((aa_rx_frame_id)-2)
68 
72 #define AA_RX_CONFIG_NONE ((aa_rx_config_id)-1)
73 
77 #define AA_RX_CONFIG_MULTI ((aa_rx_config_id)-2)
78 
86 };
87 
91 AA_API struct aa_rx_sg *
93 
97 AA_API void aa_rx_sg_destroy(struct aa_rx_sg *scene_graph);
98 
105 AA_API int aa_rx_sg_init ( struct aa_rx_sg *scene_graph );
106 
115  const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );
116 
123 AA_API const char *
125  const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );
126 
133 AA_API const char *
135  const struct aa_rx_sg *scene_graph, aa_rx_config_id config_id );
136 
145  const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id );
146 
147 
151 AA_API size_t
153  const struct aa_rx_sg *scene_graph );
154 
158 AA_API size_t
160  const struct aa_rx_sg *scene_graph );
161 
170  const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame);
171 
176 AA_API size_t
178  const struct aa_rx_sg *scene_graph, size_t n_names,
179  const char **names );
180 
188  const struct aa_rx_sg *scene_graph, const char *config_name);
189 
196 AA_API void
198  const struct aa_rx_sg *scene_graph, size_t n,
199  const char **config_name, aa_rx_config_id *ids );
200 
201 
205 AA_API void
207  const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset,
208  const aa_rx_config_id *ids,
209  const double *config_all,
210  double *config_subset );
211 
215 AA_API void
217  const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset,
218  const aa_rx_config_id *ids, const double *config_subset,
219  double *config_all
220  );
221 
222 
230  const struct aa_rx_sg *scene_graph, const char *frame_name);
231 
232 
233 /* /\** */
234 /* * Return the index of a configuration variable for the given frame. */
235 /* *\/ */
236 /* AA_API aa_rx_config_id aa_rx_sg_frame_config_id( */
237 /* struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id); */
238 
255 ( struct aa_rx_sg *scene_graph,
256  const char *parent, const char *name,
257  const double q[4], const double v[3] );
258 
276 ( struct aa_rx_sg *scene_graph,
277  const char *parent, const char *name,
278  const double q[4], const double v[3],
279  const char *config_name,
280  const double axis[3], double offset );
281 
299 ( struct aa_rx_sg *scene_graph,
300  const char *parent, const char *name,
301  const double q[4], const double v[3],
302  const char *config_name,
303  const double axis[3], double offset );
304 
309 ( struct aa_rx_sg *scene_graph,
310  const char *name );
311 
315 AA_API void
316 aa_rx_sg_set_limit_pos( struct aa_rx_sg *scenegraph,
317  const char *config_name,
318  double min, double max );
319 
323 AA_API void
324 aa_rx_sg_set_limit_vel( struct aa_rx_sg *scenegraph,
325  const char *config_name,
326  double min, double max );
327 
331 AA_API void
332 aa_rx_sg_set_limit_acc( struct aa_rx_sg *scenegraph,
333  const char *config_name,
334  double min, double max );
335 
339 AA_API void
340 aa_rx_sg_set_limit_eff( struct aa_rx_sg *scenegraph,
341  const char *config_name,
342  double min, double max );
351 AA_API int
352 aa_rx_sg_get_limit_pos( const struct aa_rx_sg *scenegraph,
353  aa_rx_config_id config_id,
354  double *min, double *max );
355 
364 AA_API int
365 aa_rx_sg_get_limit_vel( const struct aa_rx_sg *scenegraph,
366  aa_rx_config_id config_id,
367  double *min, double *max );
368 
377 AA_API int
378 aa_rx_sg_get_limit_acc( const struct aa_rx_sg *scenegraph,
379  aa_rx_config_id config_id,
380  double *min, double *max );
381 
388 AA_API int
389 aa_rx_sg_get_limit_eff( const struct aa_rx_sg *scenegraph,
390  aa_rx_config_id config_id,
391  double *min, double *max );
392 
393 
397 AA_API double
398 aa_rx_sg_config_center( const struct aa_rx_sg *sg, aa_rx_config_id config_id );
399 
403 AA_API void
404 aa_rx_sg_center_configs( const struct aa_rx_sg *sg,
405  size_t n, double *q );
406 
412 AA_API const double *aa_rx_sg_frame_axis
413 ( const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame );
414 
415 
416 /* AA_API void */
417 /* aa_rx_sg_tf_alloc( const struct aa_rx_sg *scene_graph, */
418 /* struct aa_mem_region *reg, */
419 /* double **TF_rel, size_t *ld_rel, */
420 /* double **TF_abs, size_t *ld_abs); */
421 
422 /* AA_API void */
423 /* aa_rx_sg_tf_pop( const struct aa_rx_sg *scene_graph, */
424 /* struct aa_mem_region *reg, */
425 /* double *TF_rel, double *TF_abs ); */
426 
427 
428 /* #define AA_RX_SG_TF_ALLOC( SCENE_GRAPH, REG, TF_REL, LD_REL, TF_ABS, LD_ABS ) \ */
429 /* double *TF_REL, *TF_ABS; \ */
430 /* size_t LD_REL, LD_ABS; \ */
431 /* aa_rx_sg_tf_alloc( SCENE_GRAPH, REG, &TF_REL, &LD_REL, &TF_ABS, &LD_ABS ); */
432 
433 /* #define AA_RX_SG_TF_COUNT_GET( SCENE_GRAPH, REG, N_Q, Q, N_TF, TF_REL, LD_REL, TF_ABS, LD_ABS ) \ */
434 /* double *TF_REL, *TF_ABS; \ */
435 /* size_t LD_REL, LD_ABS; \ */
436 /* size_t N_TF = aa_rx_sg_frame_count(SCENE_GRAPH); \ */
437 /* aa_rx_sg_tf_alloc( SCENE_GRAPH, REG, &TF_REL, &LD_REL, &TF_ABS, &LD_ABS ); \ */
438 /* aa_rx_sg_tf(SCENE_GRAPH, N_Q, Q, N_TF, \ */
439 /* TF_REL, LD_REL, \ */
440 /* TF_ABS, LD_ABS ); */ \
441 
442 
443 
463 AA_API void aa_rx_sg_tf
464 ( const struct aa_rx_sg *scene_graph,
465  size_t n_q, const double *q,
466  size_t n_tf,
467  double *TF_rel, size_t ld_rel,
468  double *TF_abs, size_t ld_abs );
469 
470 
484 AA_API struct aa_dmat *
485 aa_rx_sg_get_tf_abs( const struct aa_rx_sg *scene_graph,
486  struct aa_mem_region *reg,
487  const struct aa_dvec *q );
488 
489 AA_API void
490 aa_rx_sg_fill_tf_abs( const struct aa_rx_sg *scene_graph,
491  const struct aa_dvec *q,
492  struct aa_dmat *tf_abs );
493 
513 ( const struct aa_rx_sg *scene_graph,
514  size_t n_q, const double *q,
515  size_t n_tf,
516  double *TF_rel, size_t ld_rel,
517  double *TF_abs, size_t ld_abs );
518 
519 
539 ( const struct aa_rx_sg *scene_graph,
540  size_t n_q, const double *q,
541  size_t n_tf,
542  double *TF_rel, size_t ld_rel,
543  double *TF_abs, size_t ld_abs );
544 
574 ( const struct aa_rx_sg *scene_graph,
575  size_t n_q,
576  const double *q0,
577  const double *q,
578  size_t n_tf,
579  const double *TF_rel0, size_t ld_rel0,
580  const double *TF_abs0, size_t ld_abs0,
581  double *TF_rel, size_t ld_rel,
582  double *TF_abs, size_t ld_abs );
583 
584 
585 
597  const struct aa_rx_sg *scene_graph,
598  void (*function)(void *context, aa_rx_frame_id frame_id, struct aa_rx_geom *geom),
599  void *context );
600 
611 AA_API void aa_rx_sg_rel_tf (
612  const struct aa_rx_sg *scene_graph,
613  const aa_rx_frame_id frame_from,
614  const aa_rx_frame_id frame_to,
615  const double * tf_abs,
616  size_t ld_abs,
617  double * from_tf_to);
618 
622 AA_API void aa_rx_sg_reparent_id ( const struct aa_rx_sg *scene_graph,
623  aa_rx_frame_id new_parent,
624  aa_rx_frame_id frame,
625  const double *E1);
626 
630 AA_API void aa_rx_sg_reparent_name ( const struct aa_rx_sg *scene_graph,
631  const char *new_parent,
632  const char *frame,
633  const double *E1);
634 
640 AA_API struct aa_rx_sg * aa_rx_sg_copy( const struct aa_rx_sg * orig);
641 
645 AA_API void aa_rx_sg_allow_collision( struct aa_rx_sg *scene_graph,
646  aa_rx_frame_id id0, aa_rx_frame_id id1, int allowed );
647 
651 AA_API void aa_rx_sg_allow_collision_name( struct aa_rx_sg *scene_graph,
652  const char* frame0, const char* frame1, int allowed );
653 
654 
655 
659 AA_API double *
660 aa_rx_sg_alloc_tf ( const struct aa_rx_sg *sg, struct aa_mem_region *region );
661 
665 AA_API double *
666 aa_rx_sg_alloc_config ( const struct aa_rx_sg *sg, struct aa_mem_region *region );
667 
668 #endif /*AMINO_SCENEGRAPH_H*/
AA_API int aa_rx_sg_get_limit_vel(const struct aa_rx_sg *scenegraph, aa_rx_config_id config_id, double *min, double *max)
Get velocity limit values.
AA_API void aa_rx_sg_duqu(const struct aa_rx_sg *scene_graph, size_t n_q, const double *q, size_t n_tf, double *TF_rel, size_t ld_rel, double *TF_abs, size_t ld_abs)
Compute transforms for the scene graph as dual quaternions.
AA_API void aa_rx_sg_reparent_id(const struct aa_rx_sg *scene_graph, aa_rx_frame_id new_parent, aa_rx_frame_id frame, const double *E1)
Change the parent of frame in the scenegraph.
AA_API void aa_rx_sg_config_indices(const struct aa_rx_sg *scene_graph, size_t n, const char **config_name, aa_rx_config_id *ids)
Return the indices of a configuration variable in the scene graph.
AA_API const double * aa_rx_sg_frame_axis(const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame)
Return pointer to frame axis.
A rotating transform.
Definition: scenegraph.h:84
AA_API const char * aa_rx_sg_config_name(const struct aa_rx_sg *scene_graph, aa_rx_config_id config_id)
Return the config of the given frame.
AA_API void aa_rx_sg_set_limit_eff(struct aa_rx_sg *scenegraph, const char *config_name, double min, double max)
Set effort limit values.
AA_API aa_rx_config_id aa_rx_sg_config_id(const struct aa_rx_sg *scene_graph, const char *config_name)
Return the index of a configuration variable in the scene graph.
AA_API int aa_rx_sg_init(struct aa_rx_sg *scene_graph)
Setup the scenegraph internal indices.
AA_API const char * aa_rx_sg_frame_name(const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id)
Return the name of the given frame.
Container for scene geometry.
Definition: rxtype.h:63
signed long aa_rx_frame_id
Type for frame indices.
Definition: scenegraph.h:52
A fixed transform.
Definition: scenegraph.h:83
AA_API void aa_rx_sg_rm_frame(struct aa_rx_sg *scene_graph, const char *name)
Remove a frame.
AA_API aa_rx_frame_id aa_rx_sg_frame_id(const struct aa_rx_sg *scene_graph, const char *frame_name)
Return the index of a frame in the scene graph.
AA_API double * aa_rx_sg_alloc_config(const struct aa_rx_sg *sg, struct aa_mem_region *region)
Allocate storage for config array from region.
AA_API void aa_rx_sg_destroy(struct aa_rx_sg *scene_graph)
Destroy a scene graph.
AA_API enum aa_rx_frame_type aa_rx_sg_frame_type(const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id)
Return the type of the given frame.
AA_API void aa_rx_sg_add_frame_fixed(struct aa_rx_sg *scene_graph, const char *parent, const char *name, const double q[4], const double v[3])
Add a fixed-transform frame to the scene graph.
A prismatic (sliding) transform.
Definition: scenegraph.h:85
AA_API void aa_rx_sg_tf(const struct aa_rx_sg *scene_graph, size_t n_q, const double *q, size_t n_tf, double *TF_rel, size_t ld_rel, double *TF_abs, size_t ld_abs)
Compute transforms for the scene graph as quaternion-translations.
AA_API aa_rx_frame_id aa_rx_sg_frame_parent(const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame_id)
Return the parent id of the frame.
AA_API struct aa_dmat * aa_rx_sg_get_tf_abs(const struct aa_rx_sg *scene_graph, struct aa_mem_region *reg, const struct aa_dvec *q)
Allocate and compute absolute transforms for the scene graph.
Data Structure for Region-Based memory allocation.
Definition: mem.h:198
AA_API void aa_rx_sg_reparent_name(const struct aa_rx_sg *scene_graph, const char *new_parent, const char *frame, const double *E1)
Change the parent of frame in the scenegraph.
AA_API void aa_rx_sg_center_configs(const struct aa_rx_sg *sg, size_t n, double *q)
Fill q with the centered positions of each configuration.
AA_API void aa_rx_sg_rel_tf(const struct aa_rx_sg *scene_graph, const aa_rx_frame_id frame_from, const aa_rx_frame_id frame_to, const double *tf_abs, size_t ld_abs, double *from_tf_to)
Get transform between two given frames.
AA_API void aa_rx_sg_tf_update(const struct aa_rx_sg *scene_graph, size_t n_q, const double *q0, const double *q, size_t n_tf, const double *TF_rel0, size_t ld_rel0, const double *TF_abs0, size_t ld_abs0, double *TF_rel, size_t ld_rel, double *TF_abs, size_t ld_abs)
Updated transforms efficiently when only some configurations change.
Opaque type for a scene_graph.
AA_API size_t aa_rx_sg_frame_count(const struct aa_rx_sg *scene_graph)
Return the number of frames in scene_graph.
AA_API void aa_rx_sg_add_frame_revolute(struct aa_rx_sg *scene_graph, const char *parent, const char *name, const double q[4], const double v[3], const char *config_name, const double axis[3], double offset)
Add a revolute-joint frame to the scene graph.
Block matrix descriptors and linear algebra operations.
Descriptor for a vector.
Definition: mat.h:60
AA_API void aa_rx_sg_config_get(const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset, const aa_rx_config_id *ids, const double *config_all, double *config_subset)
Retrieve a subset of the configuration vector.
AA_API int aa_rx_sg_get_limit_pos(const struct aa_rx_sg *scenegraph, aa_rx_config_id config_id, double *min, double *max)
Get position limit values.
AA_API aa_rx_config_id aa_rx_sg_frame_config(const struct aa_rx_sg *scene_graph, aa_rx_frame_id frame)
Return the config id of frame.
AA_API void aa_rx_sg_allow_collision_name(struct aa_rx_sg *scene_graph, const char *frame0, const char *frame1, int allowed)
Set allowed collisions between frame0 and frame1.
AA_API int aa_rx_sg_get_limit_eff(const struct aa_rx_sg *scenegraph, aa_rx_config_id config_id, double *min, double *max)
Get effort limit values.
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
Scenegraph-related type declarations.
AA_API size_t aa_rx_sg_config_names(const struct aa_rx_sg *scene_graph, size_t n_names, const char **names)
Fill names with pointers to config names.
AA_API void aa_rx_sg_map_geom(const struct aa_rx_sg *scene_graph, void(*function)(void *context, aa_rx_frame_id frame_id, struct aa_rx_geom *geom), void *context)
Call function for every geometry object in the scene graph.
AA_API size_t aa_rx_sg_config_count(const struct aa_rx_sg *scene_graph)
Return the number of configuration variables in scene_graph.
AA_API void aa_rx_sg_set_limit_pos(struct aa_rx_sg *scenegraph, const char *config_name, double min, double max)
Set position limit values.
AA_API void aa_rx_sg_config_set(const struct aa_rx_sg *scene_graph, size_t n_all, size_t n_subset, const aa_rx_config_id *ids, const double *config_subset, double *config_all)
Fill a subset of the configuration vector.
Descriptor for a block matrix.
Definition: mat.h:69
AA_API struct aa_rx_sg * aa_rx_sg_copy(const struct aa_rx_sg *orig)
Deeply copy a scenegraph.
AA_API double * aa_rx_sg_alloc_tf(const struct aa_rx_sg *sg, struct aa_mem_region *region)
Allocate storage for TF array from region.
AA_API void aa_rx_sg_add_frame_prismatic(struct aa_rx_sg *scene_graph, const char *parent, const char *name, const double q[4], const double v[3], const char *config_name, const double axis[3], double offset)
Add a prismatic-joint frame to the scene graph.
AA_API void aa_rx_sg_set_limit_acc(struct aa_rx_sg *scenegraph, const char *config_name, double min, double max)
Set acceleration limit values.
AA_API double aa_rx_sg_config_center(const struct aa_rx_sg *sg, aa_rx_config_id config_id)
Get the centered position of a configuration variable.
AA_API int aa_rx_sg_get_limit_acc(const struct aa_rx_sg *scenegraph, aa_rx_config_id config_id, double *min, double *max)
Get acceleration limit values.
signed long aa_rx_config_id
Type for configuration indices.
Definition: scenegraph.h:57
AA_API void aa_rx_sg_tfmat(const struct aa_rx_sg *scene_graph, size_t n_q, const double *q, size_t n_tf, double *TF_rel, size_t ld_rel, double *TF_abs, size_t ld_abs)
Compute transforms for the scene graph as matrices.
aa_rx_frame_type
Enum of frame types.
Definition: scenegraph.h:82
AA_API void aa_rx_sg_set_limit_vel(struct aa_rx_sg *scenegraph, const char *config_name, double min, double max)
Set velocity limit values.
AA_API struct aa_rx_sg * aa_rx_sg_create()
Construct a new, empty scene graph.
AA_API void aa_rx_sg_allow_collision(struct aa_rx_sg *scene_graph, aa_rx_frame_id id0, aa_rx_frame_id id1, int allowed)
Set allowed collisions between frames id0 and id1.