amino
Lightweight Robot Utility Library
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
scene_kin.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_RX_SCENE_KIN_H
39 #define AMINO_RX_SCENE_KIN_H
40 
41 #include "scenegraph.h"
42 
53 struct aa_rx_sg_sub;
54 
60 AA_API void
61 aa_rx_sg_sub_destroy( struct aa_rx_sg_sub *sg );
62 
66 AA_API size_t
67 aa_rx_sg_sub_config_count( const struct aa_rx_sg_sub *sg_sub );
68 
69 
73 AA_API size_t
74 aa_rx_sg_sub_all_config_count( const struct aa_rx_sg_sub *sg_sub );
75 
79 AA_API size_t
80 aa_rx_sg_sub_frame_count( const struct aa_rx_sg_sub *sg_sub );
81 
86 aa_rx_sg_sub_config( const struct aa_rx_sg_sub *sg_sub, size_t i );
87 
92 aa_rx_sg_sub_frame( const struct aa_rx_sg_sub *sg_sub, size_t i );
93 
98 aa_rx_sg_sub_configs( const struct aa_rx_sg_sub *sg_sub );
99 
104 aa_rx_sg_sub_frames( const struct aa_rx_sg_sub *sg_sub );
105 
106 
107 AA_API void
108 aa_rx_sg_sub_config_get(
109  const struct aa_rx_sg_sub *ssg,
110  size_t n_all, const double *config_all,
111  size_t n_subset, double *config_subset );
112 
113 AA_API void
114 aa_rx_sg_sub_config_set(
115  const struct aa_rx_sg_sub *ssg,
116  size_t n_sub, const double *config_subset,
117  size_t n_all, double *config_all
118  );
119 
120 
124 AA_API struct aa_rx_sg_sub *
125 aa_rx_sg_chain_create( const struct aa_rx_sg *sg,
126  aa_rx_frame_id root, aa_rx_frame_id tip );
127 
128 
132 AA_API void
133 aa_rx_sg_sub_center_configs( const struct aa_rx_sg_sub *ssg,
134  size_t n, double *q );
135 
136 /*-- Jacobians --*/
137 
141 AA_API void
142 aa_rx_sg_sub_jacobian_size( const struct aa_rx_sg_sub *ssg,
143  size_t *rows, size_t *cols );
144 
148 AA_API void
149 aa_rx_sg_sub_jacobian( const struct aa_rx_sg_sub *ssg,
150  size_t n_tf, const double *TF_abs, size_t ld_TF,
151  double *J, size_t ld_J );
152 
153 /*-- Kinematic Solvers --*/
154 
158 typedef int aa_rx_ik_fun( void *context,
159  size_t n_tf, const double *TF, size_t ld_TF,
160  size_t n_q, double *q );
161 
162 
163 struct aa_rx_ksol_opts;
164 
165 AA_API struct aa_rx_ksol_opts*
166 aa_rx_ksol_opts_create();
167 
168 AA_API void
169 aa_rx_ksol_opts_destroy( struct aa_rx_ksol_opts *opts);
170 
171 AA_API void
172 aa_rx_ksol_opts_set_dt( struct aa_rx_ksol_opts *opts, double dt);
173 
174 AA_API void
175 aa_rx_ksol_opts_set_tol_angle( struct aa_rx_ksol_opts *opts, double tol);
176 
177 AA_API void
178 aa_rx_ksol_opts_set_tol_trans( struct aa_rx_ksol_opts *opts, double tol);
179 
180 AA_API void
181 aa_rx_ksol_opts_set_tol_angle_svd( struct aa_rx_ksol_opts *opts, double tol);
182 
183 AA_API void
184 aa_rx_ksol_opts_set_tol_trans_svd( struct aa_rx_ksol_opts *opts, double tol);
185 
186 AA_API void
187 aa_rx_ksol_opts_set_tol_dq( struct aa_rx_ksol_opts *opts, double tol);
188 
189 AA_API void
190 aa_rx_ksol_opts_set_tol_k_dls( struct aa_rx_ksol_opts *opts, double s2min);
191 
192 AA_API void
193 aa_rx_ksol_opts_set_tol_s2min( struct aa_rx_ksol_opts *opts, double s2min);
194 
195 AA_API void
196 aa_rx_ksol_opts_set_gain_angle( struct aa_rx_ksol_opts *opts, double k );
197 
198 AA_API void
199 aa_rx_ksol_opts_set_gain_trans( struct aa_rx_ksol_opts *opts, double k );
200 
201 AA_API void
202 aa_rx_ksol_opts_set_max_iterations( struct aa_rx_ksol_opts *opts, size_t n );
203 
204 AA_API void
205 aa_rx_ksol_opts_take_config( struct aa_rx_ksol_opts *opts, size_t n_q,
206  double *q, enum aa_mem_refop refop );
207 
208 AA_API void
209 aa_rx_ksol_opts_take_gain_config( struct aa_rx_ksol_opts *opts, size_t n_q,
210  double *q, enum aa_mem_refop refop );
211 
212 AA_API void
213 aa_rx_ksol_opts_take_seed( struct aa_rx_ksol_opts *opts, size_t n_q,
214  double *q_all, enum aa_mem_refop refop );
215 
216 
217 AA_API void
218 aa_rx_ksol_opts_center_seed( struct aa_rx_ksol_opts *opts,
219  const struct aa_rx_sg_sub *ssg );
220 
224 AA_API void
225 aa_rx_ksol_opts_center_configs( struct aa_rx_ksol_opts *opts,
226  const struct aa_rx_sg_sub *ssg,
227  double gain );
228 
229 /*-- Jacobian IK Solver --*/
230 
231 struct aa_rx_ik_jac_cx;
232 
233 AA_API struct aa_rx_ik_jac_cx *
234 aa_rx_ik_jac_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ksol_opts *opts );
235 
236 AA_API void
237 aa_rx_ik_jac_cx_destroy( struct aa_rx_ik_jac_cx *cx );
238 
239 
240 AA_API int aa_rx_ik_jac_solve( const struct aa_rx_ik_jac_cx *context,
241  size_t n_tf, const double *TF, size_t ld_TF,
242  size_t n_q, double *q );
243 
244 AA_API int aa_rx_ik_jac_fun( void *context,
245  size_t n_tf, const double *TF, size_t ld_TF,
246  size_t n_q, double *q );
247 
248 
249 /* AA_API int */
250 /* aa_rx_sg_sub_ksol_dls( const struct aa_rx_sg_sub *ssg, */
251 /* const struct aa_rx_ksol_opts *opts, */
252 /* size_t n_tf, const double *TF, size_t ld_TF, */
253 /* size_t n_q_all, const double *q_start_all, */
254 /* size_t n_q, double *q_subset ); */
255 
256 /* static inline int */
257 /* aa_rx_sg_chain_ksol_dls( const struct aa_rx_sg_sub *ssg, */
258 /* const struct aa_rx_ksol_opts *opts, */
259 /* const double *TF, */
260 /* size_t n_q_all, const double *q_start_all, */
261 /* size_t n_qs, double *q_subset ) */
262 /* { */
263 /* return aa_rx_sg_sub_ksol_dls( ssg, opts, 1, TF, 7, */
264 /* n_q_all, q_start_all, */
265 /* n_qs, q_subset ); */
266 /* } */
267 
268 
269 #endif /*AMINO_RX_SCENE_KIN_H*/
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.
Definition: scene_kin.h:158
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.
Definition: scenegraph.h:48
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.
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
Definition: amino.h:86
signed long aa_rx_config_id
Type for configuration indices.
Definition: scenegraph.h:53
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.