amino  1.0-beta2
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 
48 struct aa_rx_sg_sub;
49 
50 /*-- Kinematic Solvers --*/
51 
55 typedef int aa_rx_ik_fun( void *context,
56  size_t n_tf, const double *TF, size_t ld_TF,
57  size_t n_q, double *q );
58 
59 
60 struct aa_rx_ksol_opts;
61 
65 AA_API struct aa_rx_ksol_opts*
67 
71 AA_API void
72 aa_rx_ksol_opts_destroy( struct aa_rx_ksol_opts *opts);
73 
77 AA_API void
78 aa_rx_ksol_opts_set_dt( struct aa_rx_ksol_opts *opts, double dt);
79 
83 AA_API void
84 aa_rx_ksol_opts_set_tol_angle( struct aa_rx_ksol_opts *opts, double tol);
85 
89 AA_API void
90 aa_rx_ksol_opts_set_tol_trans( struct aa_rx_ksol_opts *opts, double tol);
91 
95 AA_API void
96 aa_rx_ksol_opts_set_tol_angle_svd( struct aa_rx_ksol_opts *opts, double tol);
97 
101 AA_API void
102 aa_rx_ksol_opts_set_tol_trans_svd( struct aa_rx_ksol_opts *opts, double tol);
103 
107 AA_API void
108 aa_rx_ksol_opts_set_tol_dq( struct aa_rx_ksol_opts *opts, double tol);
109 
113 AA_API void
114 aa_rx_ksol_opts_set_tol_k_dls( struct aa_rx_ksol_opts *opts, double s2min);
115 
119 AA_API void
120 aa_rx_ksol_opts_set_tol_s2min( struct aa_rx_ksol_opts *opts, double s2min);
121 
125 AA_API void
126 aa_rx_ksol_opts_set_gain_angle( struct aa_rx_ksol_opts *opts, double k );
127 
131 AA_API void
132 aa_rx_ksol_opts_set_gain_trans( struct aa_rx_ksol_opts *opts, double k );
133 
137 AA_API void
138 aa_rx_ksol_opts_set_max_iterations( struct aa_rx_ksol_opts *opts, size_t n );
139 
143 AA_API void
144 aa_rx_ksol_opts_set_frame( struct aa_rx_ksol_opts *opts, aa_rx_frame_id frame );
145 
149 AA_API void
150 aa_rx_ksol_opts_take_config( struct aa_rx_ksol_opts *opts, size_t n_q,
151  double *q, enum aa_mem_refop refop );
152 
156 AA_API void
157 aa_rx_ksol_opts_take_gain_config( struct aa_rx_ksol_opts *opts, size_t n_q,
158  double *q, enum aa_mem_refop refop );
159 
163 AA_API void
164 aa_rx_ksol_opts_take_seed( struct aa_rx_ksol_opts *opts, size_t n_q,
165  double *q_all, enum aa_mem_refop refop );
166 
167 
171 AA_API void
172 aa_rx_ksol_opts_center_seed( struct aa_rx_ksol_opts *opts,
173  const struct aa_rx_sg_sub *ssg );
174 
178 AA_API void
179 aa_rx_ksol_opts_center_configs( struct aa_rx_ksol_opts *opts,
180  const struct aa_rx_sg_sub *ssg,
181  double gain );
182 
183 /*-- Jacobian IK Solver --*/
184 
185 struct aa_rx_ik_jac_cx;
186 
190 AA_API struct aa_rx_ik_jac_cx *
191 aa_rx_ik_jac_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ksol_opts *opts );
192 
196 AA_API void
197 aa_rx_ik_jac_cx_destroy( struct aa_rx_ik_jac_cx *cx );
198 
199 
203 AA_API int aa_rx_ik_jac_solve( const struct aa_rx_ik_jac_cx *context,
204  size_t n_tf, const double *TF, size_t ld_TF,
205  size_t n_q, double *q );
206 
210 AA_API int aa_rx_ik_jac_fun( void *context,
211  size_t n_tf, const double *TF, size_t ld_TF,
212  size_t n_q, double *q );
213 
214 
215 /* AA_API int */
216 /* aa_rx_sg_sub_ksol_dls( const struct aa_rx_sg_sub *ssg, */
217 /* const struct aa_rx_ksol_opts *opts, */
218 /* size_t n_tf, const double *TF, size_t ld_TF, */
219 /* size_t n_q_all, const double *q_start_all, */
220 /* size_t n_q, double *q_subset ); */
221 
222 /* static inline int */
223 /* aa_rx_sg_chain_ksol_dls( const struct aa_rx_sg_sub *ssg, */
224 /* const struct aa_rx_ksol_opts *opts, */
225 /* const double *TF, */
226 /* size_t n_q_all, const double *q_start_all, */
227 /* size_t n_qs, double *q_subset ) */
228 /* { */
229 /* return aa_rx_sg_sub_ksol_dls( ssg, opts, 1, TF, 7, */
230 /* n_q_all, q_start_all, */
231 /* n_qs, q_subset ); */
232 /* } */
233 
234 
235 #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:55
AA_API void aa_rx_ksol_opts_take_seed(struct aa_rx_ksol_opts *opts, size_t n_q, double *q_all, enum aa_mem_refop refop)
Set a configuration seed (initial) value.
AA_API void aa_rx_ksol_opts_set_dt(struct aa_rx_ksol_opts *opts, double dt)
Set integration step.
signed long aa_rx_frame_id
Type for frame indices.
Definition: scenegraph.h:49
AA_API void aa_rx_ksol_opts_destroy(struct aa_rx_ksol_opts *opts)
Destroy options struct for kinematic solver.
AA_API void aa_rx_ksol_opts_set_tol_trans(struct aa_rx_ksol_opts *opts, double tol)
Set translational tolerance.
AA_API void aa_rx_ik_jac_cx_destroy(struct aa_rx_ik_jac_cx *cx)
Destroy a Jacobian IK solver.
AA_API void aa_rx_ksol_opts_set_tol_angle_svd(struct aa_rx_ksol_opts *opts, double tol)
Set angular tolerance to switch to SVD.
AA_API void aa_rx_ksol_opts_set_tol_angle(struct aa_rx_ksol_opts *opts, double tol)
Set angular tolerance.
AA_API void aa_rx_ksol_opts_set_gain_trans(struct aa_rx_ksol_opts *opts, double k)
Set translational gain.
The scenegraph data structure.
AA_API void aa_rx_ksol_opts_set_tol_s2min(struct aa_rx_ksol_opts *opts, double s2min)
Set minimum square singular value for damped least squares (SVD).
AA_API void aa_rx_ksol_opts_set_tol_dq(struct aa_rx_ksol_opts *opts, double tol)
Set tolerance on joint motion.
AA_API void aa_rx_ksol_opts_center_seed(struct aa_rx_ksol_opts *opts, const struct aa_rx_sg_sub *ssg)
Set the configuration seed (initial) to be the center position.
AA_API struct aa_rx_ksol_opts * aa_rx_ksol_opts_create()
Create options struct for kinematic solver.
AA_API int aa_rx_ik_jac_solve(const struct aa_rx_ik_jac_cx *context, size_t n_tf, const double *TF, size_t ld_TF, size_t n_q, double *q)
Solve the Jacobian IK.
AA_API void aa_rx_ksol_opts_take_gain_config(struct aa_rx_ksol_opts *opts, size_t n_q, double *q, enum aa_mem_refop refop)
Set a configuration gain (nullspace).
AA_API void aa_rx_ksol_opts_set_tol_trans_svd(struct aa_rx_ksol_opts *opts, double tol)
Set translational tolerance to switch to SVD.
AA_API void aa_rx_ksol_opts_take_config(struct aa_rx_ksol_opts *opts, size_t n_q, double *q, enum aa_mem_refop refop)
Set a reference configuration.
AA_API void aa_rx_ksol_opts_set_tol_k_dls(struct aa_rx_ksol_opts *opts, double s2min)
Set damping constant for damped least squares (LU decompisition).
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
AA_API struct aa_rx_ik_jac_cx * aa_rx_ik_jac_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ksol_opts *opts)
Create a Jacobian IK solver.
AA_API void aa_rx_ksol_opts_set_gain_angle(struct aa_rx_ksol_opts *opts, double k)
Set angular gain.
AA_API void aa_rx_ksol_opts_set_frame(struct aa_rx_ksol_opts *opts, aa_rx_frame_id frame)
Set frame to solve.
AA_API void aa_rx_ksol_opts_set_max_iterations(struct aa_rx_ksol_opts *opts, size_t n)
Set maximum interations.
AA_API int aa_rx_ik_jac_fun(void *context, size_t n_tf, const double *TF, size_t ld_TF, size_t n_q, double *q)
Convenience function for Jacobian IK solver.