Inverse Position Kinematics. More...

#include "scenegraph.h"

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_dvec *	aa_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_dvec *	aa_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

enum 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

parm	options structure
fun	error function
tol_abs	absolute 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.