diff --git a/include/reflex/tf.h b/include/reflex/tf.h index e39759f..eb491d7 100644 --- a/include/reflex/tf.h +++ b/include/reflex/tf.h @@ -65,20 +65,15 @@ struct rfx_tf_desc { size_t n_frame; /** - * Compute transforms. + * Compute absolute transforms. * * @param[in] q the configuration variables * @param[in] incQ increment of q array - * @param[out] E_rel the relative frames in quaternion-translation form - * @param[in] ldRel leading dimension of E_rel * @param[out] E_abs the absolute frames in quaternion-translation form * @param[in] ldAbs leading dimension of E_abs - * @param[in] options reserved for future use */ void (*frames)( const double *AA_RESTRICT q, size_t incQ, - double * AA_RESTRICT E_rel, size_t ldRel, - double * AA_RESTRICT E_abs, size_t ldAbs, - int options); + double * AA_RESTRICT E_abs, size_t ldAbs ); /** * Array of axes @@ -90,6 +85,11 @@ struct rfx_tf_desc { */ const char **frame_name; + /** + * Array of frame names + */ + const char **config_name; + /** * Array of frame parent indices */ diff --git a/lisp/frame.lisp b/lisp/frame.lisp index 1e5072b..3e7307c 100644 --- a/lisp/frame.lisp +++ b/lisp/frame.lisp @@ -290,6 +290,15 @@ collect (if p p -1)))) +(defun emit-config-names-array (name frames &key (stream t)) + (format stream "~&const char * ~A[] = {~&~{~& \"~A\"~^,~}~&};" + name + (loop for f in frames + for config = (frame-configuration f) + when config + collect config))) + + (defun rel-fun-decl (name &key (stream t) block-arrays) (format stream "~&void ~A( ~A )" name @@ -322,18 +331,17 @@ name (concatenate 'string "const double *AA_RESTRICT q, size_t incQ, " - "double *AA_RESTRICT E_rel, size_t ldRel, " - "double * AA_RESTRICT E_abs, size_t ldAbs," - "int options"))) + "double * AA_RESTRICT E_abs, size_t ldAbs" ))) (defun emit-frame-fun (name rel-fun abs-fun frames &key (stream t)) (frame-fun-decl name :stream stream) ;; TODO: interleave relative and absolute frame computation for more linear memory access ;; TODO: add options to elide recopying fixed frames (format stream "~&{~&") - (format stream "~& (void) options;") - (format stream "~& if(q) ~A(q, incQ, E_rel, ldRel);" rel-fun) - (format stream "~& if(E_abs) ~A(E_rel, ldRel, E_abs, ldAbs);" abs-fun) + (format stream "~& double *E_rel = (double*)aa_mem_region_local_alloc(7*sizeof(double)*~A);" (length frames)) + (format stream "~& ~A(q, incQ, E_rel, 7);" rel-fun) + (format stream "~& ~A(E_rel, 7, E_abs, ldAbs);" abs-fun) + (format stream "~& aa_mem_region_local_pop(E_rel);") (format stream "~&}")) ;; (defun emit-jacobian-fun (name frames &key (stream t)) @@ -418,7 +426,8 @@ (absolute-function "abs_tf") (frames-function "frames") (parents-array "parents") - (names-array "names") + (frame-names-array "frame_names") + (config-names-array "config_names") (axes-array "axes") descriptor (block-arrays t) @@ -431,7 +440,8 @@ (format f "~{~&#include <~A>~}" system-headers) (format f "~{~&#include <~A>~}" system-headers) (format f "~{~&#include \"~A\"~}~%" headers) - (format f "~&extern const char *~A[];" names-array) + (format f "~&extern const char *~A[];" frame-names-array) + (format f "~&extern const char *~A[];" config-names-array) (format f "~&extern const ssize_t ~A[];" parents-array) (format f "~&extern const double ~A[][3];" axes-array) (when descriptor @@ -456,7 +466,8 @@ (format f "~{~&#include \"~A\"~}" headers) (emit-rel-fun relative-function frames :stream f :block-arrays block-arrays) (emit-parents-array parents-array frames :stream f) - (emit-names-array names-array frames :stream f) + (emit-names-array frame-names-array frames :stream f) + (emit-config-names-array config-names-array frames :stream f) (emit-axes-array axes-array frames :stream f) (emit-abs-fun absolute-function frames :stream f :normalize normalize :block-arrays block-arrays) (emit-frame-fun frames-function relative-function absolute-function frames :stream f) @@ -466,7 +477,8 @@ (format f "~& .n_frame = ~A," frame-max) (format f "~& .frames = ~A," frames-function) (format f "~& .config_axes = ~A[0]," axes-array) - (format f "~& .frame_name = ~A," names-array) + (format f "~& .frame_name = ~A," frame-names-array) + (format f "~& .config_name = ~A," config-names-array) (format f "~& .frame_parent = ~A," parents-array) (format f "~&};")) ) diff --git a/scripts/rfx-urdfc b/scripts/rfx-urdfc index 28631a9..d843b01 100755 --- a/scripts/rfx-urdfc +++ b/scripts/rfx-urdfc @@ -34,7 +34,8 @@ REL_FUN="frame_rel" ABS_FUN="frame_abs" FRAMES_FUN="frames" ID_PREFIX="URDF_" -NAME_ARRAY=urdf_name +FRAME_NAME_ARRAY=urdf_frame_name +CONFIG_NAME_ARRAY=urdf_config_name PARENT_ARRAY=urdf_parent AXIS_ARRAY=urdf_axis FRAME_MAX="URDF_FRAME_MAX" @@ -58,7 +59,8 @@ Options: --id-prefix NAME, prefix for C identifiers --axis-array NAME, array for frame axes --parent-array NAME, array for frame parents - --name-array NAME, array for frame names + --frame-name-array NAME, array for frame names + --config-name-array NAME, array for config names --header-file NAME, output file for header --source-file NAME, output file for source --dot-file NAME, output file for dot @@ -89,9 +91,13 @@ EOF shift ID_PREFIX="$1" ;; - --name-array) + --frame-name-array) shift - NAME_ARRAY="$1" + FRAME_NAME_ARRAY="$1" + ;; + --config-name-array) + shift + CONFIG_NAME_ARRAY="$1" ;; --parent-array) shift @@ -203,7 +209,8 @@ sbcl --script <