amino  1.0-beta2
Lightweight Robot Utility Library
scene_ik.h File Reference

Inverse Position Kinematics. More...

#include "scenegraph.h"

Go to the source code of this file.

Typedefs

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)
 General type for an IK solver function.
 
typedef double aa_rx_ik_opt_fun(void *cx, const double *q, double *dq)
 Function type for optimization objectives and contstraints. More...
 

Enumerations

enum  aa_rx_ik_algo { AA_RX_IK_SQP, AA_RX_IK_JPINV, AA_RX_IK_LMA }
 Inverse Kinematics Algorithm. More...
 

Functions

AA_API struct aa_rx_ik_parm * aa_rx_ik_parm_create ()
 Create options struct for kinematic solver. More...
 
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_set_dt (struct aa_rx_ik_parm *parm, double dt)
 Set integration step.
 
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. More...
 
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_parm_set_tol_trans (struct aa_rx_ik_parm *parm, double tol)
 Set translational tolerance.
 
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.
 
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.
 
AA_API void aa_rx_ik_parm_set_tol_dq (struct aa_rx_ik_parm *parm, double tol)
 Set tolerance on joint motion.
 
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_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_s2min (struct aa_rx_ik_parm *parm, double s2min)
 Set minimum square singular value for damped least squares (SVD).
 
AA_API void aa_rx_ik_parm_set_gain_angle (struct aa_rx_ik_parm *parm, double k)
 Set angular gain.
 
AA_API void aa_rx_ik_parm_set_gain_trans (struct aa_rx_ik_parm *parm, double k)
 Set translational gain.
 
AA_API void aa_rx_ik_parm_set_max_iterations (struct aa_rx_ik_parm *parm, size_t n)
 Set maximum interations.
 
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_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_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_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_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.
 
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.
 
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 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)
 
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_cx_destroy (struct aa_rx_ik_cx *cx)
 Destroy an IK solver context.
 
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_API struct aa_dvecaa_rx_ik_get_start (const struct aa_rx_ik_cx *context)
 Return reference to the start state used by the IK solver. More...
 
AA_API struct aa_dvecaa_rx_ik_get_seed (const struct aa_rx_ik_cx *context)
 Return reference to the seed state used by the IK solver. More...
 
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. More...
 
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. More...
 
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 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_set_restart_time (struct aa_rx_ik_cx *context, double t)
 Set the maximum time limit for IK restarts.
 
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_set_frame_name (struct aa_rx_ik_cx *context, const char *name)
 Set the frame to solve for.
 
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.
 
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. More...
 
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. More...
 
AA_API double aa_rx_ik_opt_err_dqln (void *cx, double *q, double *dq)
 IK Workspace error. More...
 
AA_API double aa_rx_ik_opt_err_dqln_fd (void *cx, const double *q, double *dq)
 IK workspace error (for testing only) More...
 
AA_API double aa_rx_ik_opt_err_trans_fd (void *cx, const double *q, double *dq)
 IK position error (for testing only) More...
 
AA_API double aa_rx_ik_opt_err_qlnpv (void *cx, const double *q, double *dq)
 IK workspace error. More...
 
AA_API double aa_rx_ik_opt_err_trans (void *cx, const double *q, double *dq)
 IK position error. More...
 
AA_API double aa_rx_ik_opt_err_qlnpv_fd (void *cx, const double *q, double *dq)
 IK workspace error (for testing only). More...
 
AA_API double aa_rx_ik_opt_err_jcenter (void *cx, const double *q, double *dq)
 Error from the joint center. More...
 
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. More...
 
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.
 
AA_API void aa_rx_ik_jac_cx_destroy (struct aa_rx_ik_jac_cx *cx)
 Destroy a 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 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.
 

Detailed Description

Inverse Position Kinematics.

Definition in file scene_ik.h.

Typedef Documentation

◆ aa_rx_ik_opt_fun

typedef double aa_rx_ik_opt_fun(void *cx, const double *q, double *dq)

Function type for optimization objectives and contstraints.

See also
aa_rx_ik_opt_err_dqln
aa_rx_ik_opt_err_qlnpv
aa_rx_ik_opt_err_jcenter
aa_rx_ik_opt_err_trans
aa_rx_ik_opt_err_trans_fd

Definition at line 362 of file scene_ik.h.

Enumeration Type Documentation

◆ aa_rx_ik_algo

Inverse Kinematics Algorithm.

Enumerator
AA_RX_IK_SQP 

Sequential Quadratic Program.

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct
AA_RX_IK_JPINV 

Jacobian pseudo-inverse w/ adapative integration (kludgey)

AA_RX_IK_LMA 

Levenberg-Marquardt (unimplemented)

Definition at line 69 of file scene_ik.h.

Function Documentation

◆ aa_rx_ik_check()

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.

Returns
0 when the IK solution is within tolerances.

◆ aa_rx_ik_get_seed()

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.

The seed state holds the sub-scenegraph configuration.

◆ aa_rx_ik_get_start()

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.

The start state holds the full scene configuration.

◆ aa_rx_ik_opt_err_dqln()

AA_API double aa_rx_ik_opt_err_dqln ( void *  cx,
double *  q,
double *  dq 
)

IK Workspace error.

Logarithm of the pose error:

\[ f(q) = \| \ln S_{\rm act}(q) \otimes S_{\rm ref} \|^2 \]

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_opt_err_dqln_fd()

AA_API double aa_rx_ik_opt_err_dqln_fd ( void *  cx,
const double *  q,
double *  dq 
)

IK workspace error (for testing only)

This function uses finite difference and will be slow.

Logarithm of the pose error:

\[ f(q) = \| \ln S_{\rm act}(q) \otimes S_{\rm ref} \|^2 \]

From: Beeson, Patrick, and Barrett Ames. "TRAC-IK: An open-source library for improved solving of generic inverse kinematics." 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids). IEEE, 2015.

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_opt_err_jcenter()

AA_API double aa_rx_ik_opt_err_jcenter ( void *  cx,
const double *  q,
double *  dq 
)

Error from the joint center.

Use this function as the objective in conjunction with a workspace error function as the constraint

\[ f(q) = \frac{1}{2} \| \ln q_{\rm act} - q_{\rm center}\|^2 \]

See also
aa_rx_ik_parm_set_obj

◆ aa_rx_ik_opt_err_qlnpv()

AA_API double aa_rx_ik_opt_err_qlnpv ( void *  cx,
const double *  q,
double *  dq 
)

IK workspace error.

Logarithm of the quaternion error, plus translation error

\[ f(q) = \| \ln h_{\rm act}(q) \otimes h_{\rm ref} \|^2 + \|v_{\rm act}(q) - v_{\rm ref}\|^2 \]

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_opt_err_qlnpv_fd()

AA_API double aa_rx_ik_opt_err_qlnpv_fd ( void *  cx,
const double *  q,
double *  dq 
)

IK workspace error (for testing only).

This function uses finite difference and will be slow.

Logarithm of the quaternion error, plus translation error

\[ f(q) = \| \ln h_{\rm act}(q) \otimes h_{\rm ref} \|^2 + \|v_{\rm act}(q) - v_{\rm ref}\|^2 \]

From: Beeson, Patrick, and Barrett Ames. "TRAC-IK: An open-source library for improved solving of generic inverse kinematics." 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids). IEEE, 2015.

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_opt_err_trans()

AA_API double aa_rx_ik_opt_err_trans ( void *  cx,
const double *  q,
double *  dq 
)

IK position error.

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_opt_err_trans_fd()

AA_API double aa_rx_ik_opt_err_trans_fd ( void *  cx,
const double *  q,
double *  dq 
)

IK position error (for testing only)

This function uses finite difference and will be slow. It only calculates the error in position.

See also
aa_rx_ik_parm_set_obj
aa_rx_ik_parm_set_eqct

◆ aa_rx_ik_parm_create()

AA_API struct aa_rx_ik_parm* aa_rx_ik_parm_create ( )

Create options struct for kinematic solver.

The default options should offer good performance and robustness.

◆ aa_rx_ik_parm_set_algo()

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.

See also
aa_rx_ik_algorithm

◆ aa_rx_ik_parm_set_eqct()

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.

Consider using a workspace constraint and a jointspace objective.

See also
aa_rx_ik_err_dqln
aa_rx_ik_err_qlnpv

◆ aa_rx_ik_parm_set_obj()

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.

If using a workspace objective, no additional constraints are needed (position limit bound constraints are automatiically added). If using a jointspace objective, a workspace constraint may be necessary.

Parameters
parmoptions structure
funerror function
tol_absabsolute tolerance on error
See also
aa_rx_ik_opt_err_dqln
aa_rx_ik_opt_err_qlnpv
aa_rx_ik_opt_err_jcenter
aa_rx_ik_opt_err_trans
aa_rx_ik_opt_err_trans_fd

◆ aa_rx_ik_set_seed()

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.

The seed state holds the sub-scenegraph configuration.

◆ aa_rx_ik_set_start()

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.

The start state holds the full scene configuration.