scene_ik.h

Go to the documentation of this file.

/* -*- mode: C; c-basic-offset: 4; -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2015, Rice University
 * Copyright (c) 2019, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@rice.edu>
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of copyright holder the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */
 
#ifndef AMINO_RX_SCENE_IK_H
#define AMINO_RX_SCENE_IK_H
 
#include "scenegraph.h"
 
struct aa_rx_sg_sub;
 
/*-- Kinematic Solvers --*/
 
typedef int aa_rx_ik_fun( void *context,
                          size_t n_tf, const double *TF, size_t ld_TF,
                          size_t n_q, double *q );
 
 
struct aa_rx_ik_parm;
 
enum aa_rx_ik_algo {
    AA_RX_IK_SQP,
 
    AA_RX_IK_JPINV,
 
    AA_RX_IK_LMA
 
};
 
AA_API struct aa_rx_ik_parm*
aa_rx_ik_parm_create();
 
AA_API void
aa_rx_ik_parm_destroy( struct aa_rx_ik_parm *parm);
 
AA_API void
aa_rx_ik_parm_set_dt( struct aa_rx_ik_parm *parm, double dt);
 
AA_API void
aa_rx_ik_parm_set_algo( struct aa_rx_ik_parm *parm,
                        enum aa_rx_ik_algo algo );
 
 
 
 
 
 
AA_API void
aa_rx_ik_parm_set_tol_angle( struct aa_rx_ik_parm *parm, double tol);
 
AA_API void
aa_rx_ik_parm_set_tol_trans( struct aa_rx_ik_parm *parm, double tol);
 
AA_API void
aa_rx_ik_parm_set_tol_angle_svd( struct aa_rx_ik_parm *parm, double tol);
 
AA_API void
aa_rx_ik_parm_set_tol_trans_svd( struct aa_rx_ik_parm *parm, double tol);
 
AA_API void
aa_rx_ik_parm_set_tol_dq( struct aa_rx_ik_parm *parm, double tol);
 
AA_API void
aa_rx_ik_parm_set_tol_obj_abs( struct aa_rx_ik_parm *parm, double tol );
 
AA_API void
aa_rx_ik_parm_set_tol_obj_rel( struct aa_rx_ik_parm *parm, double tol );
 
AA_API void
aa_rx_ik_parm_set_k_dls( struct aa_rx_ik_parm *parm, double s2min);
 
AA_API void
aa_rx_ik_parm_set_s2min( struct aa_rx_ik_parm *parm, double s2min);
 
AA_API void
aa_rx_ik_parm_set_gain_angle( struct aa_rx_ik_parm *parm, double k );
 
AA_API void
aa_rx_ik_parm_set_gain_trans( struct aa_rx_ik_parm *parm, double k );
 
AA_API void
aa_rx_ik_parm_set_max_iterations( struct aa_rx_ik_parm *parm, size_t n );
 
AA_API void
aa_rx_ik_parm_set_frame( struct aa_rx_ik_parm *parm, aa_rx_frame_id frame );
 
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 );
 
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 );
 
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 );
 
 
AA_API void
aa_rx_ik_parm_center_seed( struct aa_rx_ik_parm *parm,
                             const struct aa_rx_sg_sub *ssg );
 
AA_API void
aa_rx_ik_parm_center_configs( struct aa_rx_ik_parm *parm,
                                const struct aa_rx_sg_sub *ssg,
                                double gain );
 
AA_API void
aa_rx_ik_parm_set_debug( struct aa_rx_ik_parm *parm, int debug );
 
 
 
/*-- Jacobian IK Solver --*/
 
AA_API int
aa_rx_ik_jac_x2dq ( const struct aa_rx_ik_parm *parm, size_t n_q,
                    const double *AA_RESTRICT q_act, const double *AA_RESTRICT E_act,
                    const double E_ref[7], const double dx_ref[6],
                    const double *J, double *AA_RESTRICT dq );
 
 
 
 
 
struct aa_rx_ik_cx;
 
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 );
 
AA_API void
aa_rx_ik_cx_destroy(struct aa_rx_ik_cx *cx);
 
AA_API int
aa_rx_ik_solve( const struct aa_rx_ik_cx *context,
                const struct aa_dmat *TF,
                struct aa_dvec *q );
 
AA_API struct aa_dvec *
aa_rx_ik_get_start( const struct aa_rx_ik_cx *context );
 
AA_API struct aa_dvec *
aa_rx_ik_get_seed( const struct aa_rx_ik_cx *context );
 
AA_API void
aa_rx_ik_set_start( struct aa_rx_ik_cx *context, const struct aa_dvec *q_start );
 
AA_API void
aa_rx_ik_set_seed( struct aa_rx_ik_cx *context, const struct aa_dvec *q_seed );
 
AA_API void
aa_rx_ik_set_seed_center( struct aa_rx_ik_cx *context );
 
AA_API void
aa_rx_ik_set_seed_rand( struct aa_rx_ik_cx *context );
 
AA_API void
aa_rx_ik_set_restart_time( struct aa_rx_ik_cx *context, double t );
 
AA_API double
aa_rx_ik_get_restart_time( struct aa_rx_ik_cx *context );
 
AA_API void
aa_rx_ik_set_frame_name( struct aa_rx_ik_cx *context, const char *name );
 
AA_API void
aa_rx_ik_set_frame_id( struct aa_rx_ik_cx *context, aa_rx_frame_id id );
 
typedef double aa_rx_ik_opt_fun(void *cx, const double *q, double *dq);
 
AA_API void
aa_rx_ik_parm_set_obj( struct aa_rx_ik_parm *parm,
                         aa_rx_ik_opt_fun *fun );
 
AA_API void
aa_rx_ik_parm_set_eqct( struct aa_rx_ik_parm *parm,
                          aa_rx_ik_opt_fun *fun,
                          double tol);
 
 
 
AA_API double
aa_rx_ik_opt_err_dqln( void *cx, double *q, double *dq );
 
AA_API double
aa_rx_ik_opt_err_dqln_fd( void *cx, const double *q, double *dq );
 
AA_API double
aa_rx_ik_opt_err_trans_fd( void *cx, const double *q, double *dq );
 
AA_API double
aa_rx_ik_opt_err_qlnpv( void *cx, const double *q, double *dq );
 
AA_API double
aa_rx_ik_opt_err_trans( void *cx, const double *q, double *dq );
 
AA_API double
aa_rx_ik_opt_err_qlnpv_fd( void *cx, const double *q, double *dq );
 
 
AA_API double
aa_rx_ik_opt_err_jcenter( void *cx, const double *q, double *dq );
 
 
AA_API int
aa_rx_ik_check( const struct aa_rx_ik_cx *context,
                const struct aa_dmat *TF,
                struct aa_dvec *q_sub );
 
 
struct aa_rx_ik_jac_cx;
 
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 );
 
AA_API void
aa_rx_ik_jac_cx_destroy( struct aa_rx_ik_jac_cx *cx );
 
 
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 );
 
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 );
 
 
 
/* AA_API int */
/* aa_rx_sg_sub_ksol_dls( const struct aa_rx_sg_sub *ssg, */
/*                        const struct aa_rx_ik_parm *parm, */
/*                        size_t n_tf, const double *TF, size_t ld_TF, */
/*                        size_t n_q_all, const double *q_start_all, */
/*                        size_t n_q, double *q_subset ); */
 
/* static inline int */
/* aa_rx_sg_chain_ksol_dls( const struct aa_rx_sg_sub *ssg, */
/*                          const struct aa_rx_ik_parm *parm, */
/*                          const double *TF, */
/*                          size_t n_q_all, const double *q_start_all, */
/*                          size_t n_qs, double *q_subset ) */
/* { */
/*     return aa_rx_sg_sub_ksol_dls( ssg, parm, 1, TF, 7, */
/*                                   n_q_all, q_start_all, */
/*                                   n_qs, q_subset ); */
/* } */
 
 
#endif /*AMINO_RX_SCENE_IK_H*/