amino  1.0-beta2
Lightweight Robot Utility Library
scene_ik.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  * Copyright (c) 2019, Colorado School of Mines
6  * All rights reserved.
7  *
8  * Author(s): Neil T. Dantam <ntd@rice.edu>
9  *
10  * Redistribution and use in source and binary forms, with or
11  * without modification, are permitted provided that the following
12  * conditions are met:
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of copyright holder the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
24  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
25  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
26  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
28  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
29  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
30  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
31  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
32  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef AMINO_RX_SCENE_IK_H
40 #define AMINO_RX_SCENE_IK_H
41 
42 #include "scenegraph.h"
43 
49 struct aa_rx_sg_sub;
50 
51 /*-- Kinematic Solvers --*/
52 
56 typedef int aa_rx_ik_fun( void *context,
57  size_t n_tf, const double *TF, size_t ld_TF,
58  size_t n_q, double *q );
59 
60 
64 struct aa_rx_ik_parm;
65 
77 
82 
87 
88 };
89 
95 AA_API struct aa_rx_ik_parm*
97 
101 AA_API void
102 aa_rx_ik_parm_destroy( struct aa_rx_ik_parm *parm);
103 
107 AA_API void
108 aa_rx_ik_parm_set_dt( struct aa_rx_ik_parm *parm, double dt);
109 
115 AA_API void
116 aa_rx_ik_parm_set_algo( struct aa_rx_ik_parm *parm,
117  enum aa_rx_ik_algo algo );
118 
119 
120 
121 
122 
123 
127 AA_API void
128 aa_rx_ik_parm_set_tol_angle( struct aa_rx_ik_parm *parm, double tol);
129 
133 AA_API void
134 aa_rx_ik_parm_set_tol_trans( struct aa_rx_ik_parm *parm, double tol);
135 
139 AA_API void
140 aa_rx_ik_parm_set_tol_angle_svd( struct aa_rx_ik_parm *parm, double tol);
141 
145 AA_API void
146 aa_rx_ik_parm_set_tol_trans_svd( struct aa_rx_ik_parm *parm, double tol);
147 
151 AA_API void
152 aa_rx_ik_parm_set_tol_dq( struct aa_rx_ik_parm *parm, double tol);
153 
157 AA_API void
158 aa_rx_ik_parm_set_tol_obj_abs( struct aa_rx_ik_parm *parm, double tol );
159 
163 AA_API void
164 aa_rx_ik_parm_set_tol_obj_rel( struct aa_rx_ik_parm *parm, double tol );
165 
169 AA_API void
170 aa_rx_ik_parm_set_k_dls( struct aa_rx_ik_parm *parm, double s2min);
171 
175 AA_API void
176 aa_rx_ik_parm_set_s2min( struct aa_rx_ik_parm *parm, double s2min);
177 
181 AA_API void
182 aa_rx_ik_parm_set_gain_angle( struct aa_rx_ik_parm *parm, double k );
183 
187 AA_API void
188 aa_rx_ik_parm_set_gain_trans( struct aa_rx_ik_parm *parm, double k );
189 
193 AA_API void
194 aa_rx_ik_parm_set_max_iterations( struct aa_rx_ik_parm *parm, size_t n );
195 
199 AA_API void
200 aa_rx_ik_parm_set_frame( struct aa_rx_ik_parm *parm, aa_rx_frame_id frame );
201 
205 AA_API void
206 aa_rx_ik_parm_take_config( struct aa_rx_ik_parm *parm, size_t n_q,
207  double *q, enum aa_mem_refop refop );
208 
212 AA_API void
213 aa_rx_ik_parm_take_gain_config( struct aa_rx_ik_parm *parm, size_t n_q,
214  double *q, enum aa_mem_refop refop );
215 
219 AA_API void
220 aa_rx_ik_parm_take_seed( struct aa_rx_ik_parm *parm, size_t n_q,
221  double *q_all, enum aa_mem_refop refop );
222 
223 
227 AA_API void
228 aa_rx_ik_parm_center_seed( struct aa_rx_ik_parm *parm,
229  const struct aa_rx_sg_sub *ssg );
230 
234 AA_API void
235 aa_rx_ik_parm_center_configs( struct aa_rx_ik_parm *parm,
236  const struct aa_rx_sg_sub *ssg,
237  double gain );
238 
242 AA_API void
243 aa_rx_ik_parm_set_debug( struct aa_rx_ik_parm *parm, int debug );
244 
245 
246 
247 /*-- Jacobian IK Solver --*/
248 
249 AA_API int
250 aa_rx_ik_jac_x2dq ( const struct aa_rx_ik_parm *parm, size_t n_q,
251  const double *AA_RESTRICT q_act, const double *AA_RESTRICT E_act,
252  const double E_ref[7], const double dx_ref[6],
253  const double *J, double *AA_RESTRICT dq );
254 
255 
256 
257 
258 
262 struct aa_rx_ik_cx;
263 
267 AA_API struct aa_rx_ik_cx *
268 aa_rx_ik_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ik_parm *parm );
269 
273 AA_API void
274 aa_rx_ik_cx_destroy(struct aa_rx_ik_cx *cx);
275 
279 AA_API int
280 aa_rx_ik_solve( const struct aa_rx_ik_cx *context,
281  const struct aa_dmat *TF,
282  struct aa_dvec *q );
283 
289 AA_API struct aa_dvec *
290 aa_rx_ik_get_start( const struct aa_rx_ik_cx *context );
291 
297 AA_API struct aa_dvec *
298 aa_rx_ik_get_seed( const struct aa_rx_ik_cx *context );
299 
305 AA_API void
306 aa_rx_ik_set_start( struct aa_rx_ik_cx *context, const struct aa_dvec *q_start );
307 
313 AA_API void
314 aa_rx_ik_set_seed( struct aa_rx_ik_cx *context, const struct aa_dvec *q_seed );
315 
319 AA_API void
320 aa_rx_ik_set_seed_center( struct aa_rx_ik_cx *context );
321 
325 AA_API void
326 aa_rx_ik_set_seed_rand( struct aa_rx_ik_cx *context );
327 
331 AA_API void
332 aa_rx_ik_set_restart_time( struct aa_rx_ik_cx *context, double t );
333 
337 AA_API double
338 aa_rx_ik_get_restart_time( struct aa_rx_ik_cx *context );
339 
343 AA_API void
344 aa_rx_ik_set_frame_name( struct aa_rx_ik_cx *context, const char *name );
345 
349 AA_API void
350 aa_rx_ik_set_frame_id( struct aa_rx_ik_cx *context, aa_rx_frame_id id );
351 
362 typedef double aa_rx_ik_opt_fun(void *cx, const double *q, double *dq);
363 
382 AA_API void
383 aa_rx_ik_parm_set_obj( struct aa_rx_ik_parm *parm,
384  aa_rx_ik_opt_fun *fun );
385 
394 AA_API void
395 aa_rx_ik_parm_set_eqct( struct aa_rx_ik_parm *parm,
396  aa_rx_ik_opt_fun *fun,
397  double tol);
398 
399 
400 
412 AA_API double
413 aa_rx_ik_opt_err_dqln( void *cx, double *q, double *dq );
414 
432 AA_API double
433 aa_rx_ik_opt_err_dqln_fd( void *cx, const double *q, double *dq );
434 
445 AA_API double
446 aa_rx_ik_opt_err_trans_fd( void *cx, const double *q, double *dq );
447 
461 AA_API double
462 aa_rx_ik_opt_err_qlnpv( void *cx, const double *q, double *dq );
463 
470 AA_API double
471 aa_rx_ik_opt_err_trans( void *cx, const double *q, double *dq );
472 
494 AA_API double
495 aa_rx_ik_opt_err_qlnpv_fd( void *cx, const double *q, double *dq );
496 
497 
510 AA_API double
511 aa_rx_ik_opt_err_jcenter( void *cx, const double *q, double *dq );
512 
513 
519 AA_API int
520 aa_rx_ik_check( const struct aa_rx_ik_cx *context,
521  const struct aa_dmat *TF,
522  struct aa_dvec *q_sub );
523 
524 
525 struct aa_rx_ik_jac_cx;
526 
530 AA_API struct aa_rx_ik_jac_cx *
531 aa_rx_ik_jac_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ik_parm *parm );
532 
536 AA_API void
537 aa_rx_ik_jac_cx_destroy( struct aa_rx_ik_jac_cx *cx );
538 
539 
543 AA_API int aa_rx_ik_jac_solve( const struct aa_rx_ik_jac_cx *context,
544  size_t n_tf, const double *TF, size_t ld_TF,
545  size_t n_q, double *q );
546 
550 AA_API int aa_rx_ik_jac_fun( void *context,
551  size_t n_tf, const double *TF, size_t ld_TF,
552  size_t n_q, double *q );
553 
554 
555 
556 /* AA_API int */
557 /* aa_rx_sg_sub_ksol_dls( const struct aa_rx_sg_sub *ssg, */
558 /* const struct aa_rx_ik_parm *parm, */
559 /* size_t n_tf, const double *TF, size_t ld_TF, */
560 /* size_t n_q_all, const double *q_start_all, */
561 /* size_t n_q, double *q_subset ); */
562 
563 /* static inline int */
564 /* aa_rx_sg_chain_ksol_dls( const struct aa_rx_sg_sub *ssg, */
565 /* const struct aa_rx_ik_parm *parm, */
566 /* const double *TF, */
567 /* size_t n_q_all, const double *q_start_all, */
568 /* size_t n_qs, double *q_subset ) */
569 /* { */
570 /* return aa_rx_sg_sub_ksol_dls( ssg, parm, 1, TF, 7, */
571 /* n_q_all, q_start_all, */
572 /* n_qs, q_subset ); */
573 /* } */
574 
575 
576 #endif /*AMINO_RX_SCENE_IK_H*/
AA_API int aa_rx_ik_check(const struct aa_rx_ik_cx *context, const struct aa_dmat *TF, struct aa_dvec *q_sub)
Check an IK solution.
double aa_rx_ik_opt_fun(void *cx, const double *q, double *dq)
Function type for optimization objectives and contstraints.
Definition: scene_ik.h:362
AA_API void aa_rx_ik_jac_cx_destroy(struct aa_rx_ik_jac_cx *cx)
Destroy a Jacobian IK solver.
AA_API double aa_rx_ik_opt_err_qlnpv_fd(void *cx, const double *q, double *dq)
IK workspace error (for testing only).
signed long aa_rx_frame_id
Type for frame indices.
Definition: scenegraph.h:52
AA_API void aa_rx_ik_set_seed_rand(struct aa_rx_ik_cx *context)
Convenience function to set a random seed.
AA_API void aa_rx_ik_parm_take_gain_config(struct aa_rx_ik_parm *parm, size_t n_q, double *q, enum aa_mem_refop refop)
Set a configuration gain (nullspace).
AA_API void aa_rx_ik_set_restart_time(struct aa_rx_ik_cx *context, double t)
Set the maximum time limit for IK restarts.
AA_API struct aa_dvec * aa_rx_ik_get_start(const struct aa_rx_ik_cx *context)
Return reference to the start state used by the IK solver.
AA_API int aa_rx_ik_solve(const struct aa_rx_ik_cx *context, const struct aa_dmat *TF, struct aa_dvec *q)
Run the IK solver.
aa_rx_ik_algo
Inverse Kinematics Algorithm.
Definition: scene_ik.h:69
AA_API void aa_rx_ik_set_frame_name(struct aa_rx_ik_cx *context, const char *name)
Set the frame to solve for.
AA_API void aa_rx_ik_parm_set_tol_obj_abs(struct aa_rx_ik_parm *parm, double tol)
Set absolute tolerance on objective function.
AA_API void aa_rx_ik_parm_set_tol_obj_rel(struct aa_rx_ik_parm *parm, double tol)
Set relative tolerance on objective function.
AA_API void aa_rx_ik_parm_set_tol_angle_svd(struct aa_rx_ik_parm *parm, double tol)
Set angular tolerance to switch to SVD.
The scenegraph data structure.
AA_API void aa_rx_ik_parm_set_gain_angle(struct aa_rx_ik_parm *parm, double k)
Set angular gain.
AA_API double aa_rx_ik_opt_err_trans(void *cx, const double *q, double *dq)
IK position error.
AA_API void aa_rx_ik_parm_set_gain_trans(struct aa_rx_ik_parm *parm, double k)
Set translational gain.
AA_API double aa_rx_ik_opt_err_dqln(void *cx, double *q, double *dq)
IK Workspace error.
AA_API void aa_rx_ik_parm_take_seed(struct aa_rx_ik_parm *parm, size_t n_q, double *q_all, enum aa_mem_refop refop)
Set a configuration seed (initial) value.
AA_API void aa_rx_ik_parm_set_k_dls(struct aa_rx_ik_parm *parm, double s2min)
Set damping constant for damped least squares (LU decompisition).
AA_API void aa_rx_ik_parm_set_tol_dq(struct aa_rx_ik_parm *parm, double tol)
Set tolerance on joint motion.
Sequential Quadratic Program.
Definition: scene_ik.h:76
AA_API double aa_rx_ik_opt_err_jcenter(void *cx, const double *q, double *dq)
Error from the joint center.
AA_API double aa_rx_ik_get_restart_time(struct aa_rx_ik_cx *context)
Get the maximum time limit for IK restarts.
AA_API void aa_rx_ik_parm_set_eqct(struct aa_rx_ik_parm *parm, aa_rx_ik_opt_fun *fun, double tol)
Set the equality constraint for optimization (e.g., SQP) IK.
AA_API void aa_rx_ik_parm_take_config(struct aa_rx_ik_parm *parm, size_t n_q, double *q, enum aa_mem_refop refop)
Set a reference configuration.
AA_API void aa_rx_ik_parm_set_tol_trans(struct aa_rx_ik_parm *parm, double tol)
Set translational tolerance.
AA_API void aa_rx_ik_parm_set_dt(struct aa_rx_ik_parm *parm, double dt)
Set integration step.
AA_API double aa_rx_ik_opt_err_dqln_fd(void *cx, const double *q, double *dq)
IK workspace error (for testing only)
Jacobian pseudo-inverse w/ adapative integration (kludgey)
Definition: scene_ik.h:81
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_ik_parm *parm)
Create a Jacobian IK solver.
Descriptor for a vector.
Definition: mat.h:60
AA_API void aa_rx_ik_parm_center_configs(struct aa_rx_ik_parm *parm, const struct aa_rx_sg_sub *ssg, double gain)
Convenience function to set IK options to center joints.
#define AA_RESTRICT
Defined restrict keyword based on language flavor.
Definition: amino.h:99
AA_API void aa_rx_ik_parm_set_debug(struct aa_rx_ik_parm *parm, int debug)
Print debugging output for IK solver.
AA_API void aa_rx_ik_parm_set_tol_trans_svd(struct aa_rx_ik_parm *parm, double tol)
Set translational tolerance to switch to SVD.
#define AA_API
calling and name mangling convention for functions
Definition: amino.h:95
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.
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 struct aa_rx_ik_parm * aa_rx_ik_parm_create()
Create options struct for kinematic solver.
AA_API void aa_rx_ik_parm_set_frame(struct aa_rx_ik_parm *parm, aa_rx_frame_id frame)
Set frame to solve.
AA_API void aa_rx_ik_parm_set_obj(struct aa_rx_ik_parm *parm, aa_rx_ik_opt_fun *fun)
Set an error objective function for optimization (e.g., SQP) IK.
AA_API void aa_rx_ik_set_frame_id(struct aa_rx_ik_cx *context, aa_rx_frame_id id)
Set the frame to solve for.
Descriptor for a block matrix.
Definition: mat.h:69
AA_API void aa_rx_ik_parm_set_algo(struct aa_rx_ik_parm *parm, enum aa_rx_ik_algo algo)
Set inverse kinematics algorithm.
AA_API void aa_rx_ik_parm_set_max_iterations(struct aa_rx_ik_parm *parm, size_t n)
Set maximum interations.
AA_API struct aa_rx_ik_cx * aa_rx_ik_cx_create(const struct aa_rx_sg_sub *ssg, const struct aa_rx_ik_parm *parm)
Create an IK solver context.
AA_API void aa_rx_ik_parm_set_s2min(struct aa_rx_ik_parm *parm, double s2min)
Set minimum square singular value for damped least squares (SVD).
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_ik.h:56
Levenberg-Marquardt (unimplemented)
Definition: scene_ik.h:86
AA_API struct aa_dvec * aa_rx_ik_get_seed(const struct aa_rx_ik_cx *context)
Return reference to the seed state used by the IK solver.
AA_API void aa_rx_ik_set_seed(struct aa_rx_ik_cx *context, const struct aa_dvec *q_seed)
Set seed used by the IK solver.
AA_API void aa_rx_ik_set_seed_center(struct aa_rx_ik_cx *context)
Convenience function to set the seed to the centered configuration.
AA_API double aa_rx_ik_opt_err_trans_fd(void *cx, const double *q, double *dq)
IK position error (for testing only)
AA_API void aa_rx_ik_parm_set_tol_angle(struct aa_rx_ik_parm *parm, double tol)
Set angular tolerance.
AA_API void aa_rx_ik_cx_destroy(struct aa_rx_ik_cx *cx)
Destroy an IK solver context.
AA_API void aa_rx_ik_set_start(struct aa_rx_ik_cx *context, const struct aa_dvec *q_start)
Set the start state used by the IK solver.
AA_API double aa_rx_ik_opt_err_qlnpv(void *cx, const double *q, double *dq)
IK workspace error.
AA_API void aa_rx_ik_parm_destroy(struct aa_rx_ik_parm *parm)
Destroy options struct for kinematic solver.
AA_API void aa_rx_ik_parm_center_seed(struct aa_rx_ik_parm *parm, const struct aa_rx_sg_sub *ssg)
Set the configuration seed (initial) to be the center position.