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 
54 struct aa_rx_sg_sub;
55 
61 AA_API void
62 aa_rx_sg_sub_destroy( struct aa_rx_sg_sub *sg );
63 
67 AA_API size_t
68 aa_rx_sg_sub_config_count( const struct aa_rx_sg_sub *sg_sub );
69 
70 
74 AA_API size_t
75 aa_rx_sg_sub_all_config_count( const struct aa_rx_sg_sub *sg_sub );
76 
80 AA_API size_t
81 aa_rx_sg_sub_frame_count( const struct aa_rx_sg_sub *sg_sub );
82 
87 aa_rx_sg_sub_config( const struct aa_rx_sg_sub *sg_sub, size_t i );
88 
93 aa_rx_sg_sub_frame( const struct aa_rx_sg_sub *sg_sub, size_t i );
94 
99 aa_rx_sg_sub_configs( const struct aa_rx_sg_sub *sg_sub );
100 
105 aa_rx_sg_sub_frames( const struct aa_rx_sg_sub *sg_sub );
106 
107 
108 AA_API void
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 );
113 
114 AA_API void
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
119  );
120 
121 
125 AA_API struct aa_rx_sg_sub *
126 aa_rx_sg_chain_create( const struct aa_rx_sg *sg,
127  aa_rx_frame_id root, aa_rx_frame_id tip );
128 
129 
133 AA_API void
134 aa_rx_sg_sub_center_configs( const struct aa_rx_sg_sub *ssg,
135  size_t n, double *q );
136 
137 /*-- Jacobians --*/
138 
142 AA_API void
143 aa_rx_sg_sub_jacobian_size( const struct aa_rx_sg_sub *ssg,
144  size_t *rows, size_t *cols );
145 
149 AA_API void
150 aa_rx_sg_sub_jacobian( const struct aa_rx_sg_sub *ssg,
151  size_t n_tf, const double *TF_abs, size_t ld_TF,
152  double *J, size_t ld_J );
153 
154 /*-- Kinematic Solvers --*/
155 
159 typedef int aa_rx_ik_fun( void *context,
160  size_t n_tf, const double *TF, size_t ld_TF,
161  size_t n_q, double *q );
162 
163 
164 struct aa_rx_ksol_opts;
165 
166 AA_API struct aa_rx_ksol_opts*
167 aa_rx_ksol_opts_create();
168 
169 AA_API void
170 aa_rx_ksol_opts_destroy( struct aa_rx_ksol_opts *opts);
171 
172 AA_API void
173 aa_rx_ksol_opts_set_dt( struct aa_rx_ksol_opts *opts, double dt);
174 
175 AA_API void
176 aa_rx_ksol_opts_set_tol_angle( struct aa_rx_ksol_opts *opts, double tol);
177 
178 AA_API void
179 aa_rx_ksol_opts_set_tol_trans( struct aa_rx_ksol_opts *opts, double tol);
180 
181 AA_API void
182 aa_rx_ksol_opts_set_tol_angle_svd( struct aa_rx_ksol_opts *opts, double tol);
183 
184 AA_API void
185 aa_rx_ksol_opts_set_tol_trans_svd( struct aa_rx_ksol_opts *opts, double tol);
186 
187 AA_API void
188 aa_rx_ksol_opts_set_tol_dq( struct aa_rx_ksol_opts *opts, double tol);
189 
190 AA_API void
191 aa_rx_ksol_opts_set_tol_k_dls( struct aa_rx_ksol_opts *opts, double s2min);
192 
193 AA_API void
194 aa_rx_ksol_opts_set_tol_s2min( struct aa_rx_ksol_opts *opts, double s2min);
195 
196 AA_API void
197 aa_rx_ksol_opts_set_gain_angle( struct aa_rx_ksol_opts *opts, double k );
198 
199 AA_API void
200 aa_rx_ksol_opts_set_gain_trans( struct aa_rx_ksol_opts *opts, double k );
201 
202 AA_API void
203 aa_rx_ksol_opts_set_max_iterations( struct aa_rx_ksol_opts *opts, size_t n );
204 
205 AA_API void
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 );
208 
209 AA_API void
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 );
212 
213 AA_API void
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 );
216 
217 
218 AA_API void
219 aa_rx_ksol_opts_center_seed( struct aa_rx_ksol_opts *opts,
220  const struct aa_rx_sg_sub *ssg );
221 
225 AA_API void
226 aa_rx_ksol_opts_center_configs( struct aa_rx_ksol_opts *opts,
227  const struct aa_rx_sg_sub *ssg,
228  double gain );
229 
230 /*-- Jacobian IK Solver --*/
231 
232 struct aa_rx_ik_jac_cx;
233 
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 );
236 
237 AA_API void
238 aa_rx_ik_jac_cx_destroy( struct aa_rx_ik_jac_cx *cx );
239 
240 
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 );
244 
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 );
248 
249 
250 /* AA_API int */
251 /* aa_rx_sg_sub_ksol_dls( const struct aa_rx_sg_sub *ssg, */
252 /* const struct aa_rx_ksol_opts *opts, */
253 /* size_t n_tf, const double *TF, size_t ld_TF, */
254 /* size_t n_q_all, const double *q_start_all, */
255 /* size_t n_q, double *q_subset ); */
256 
257 /* static inline int */
258 /* aa_rx_sg_chain_ksol_dls( const struct aa_rx_sg_sub *ssg, */
259 /* const struct aa_rx_ksol_opts *opts, */
260 /* const double *TF, */
261 /* size_t n_q_all, const double *q_start_all, */
262 /* size_t n_qs, double *q_subset ) */
263 /* { */
264 /* return aa_rx_sg_sub_ksol_dls( ssg, opts, 1, TF, 7, */
265 /* n_q_all, q_start_all, */
266 /* n_qs, q_subset ); */
267 /* } */
268 
269 
270 #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:159
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:49
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
Definition: amino.h:96
signed long aa_rx_config_id
Type for configuration indices.
Definition: scenegraph.h:54
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.