Contains inverse kinematics algorithms. More...

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <nlopt.hpp>
#include <unsupported/Eigen/AutoDiff>
#include "kinematics.hpp"
#include "math.hpp"
#include "model.hpp"

Classes
struct	tinyrobotics::InverseKinematicsOptions< Scalar, nq >
	Options for inverse kinematics solver. More...

Typedefs
template<typename Scalar , int nv>
using	tinyrobotics::ObjectiveFunction = std::function< Scalar(const Eigen::Matrix< Scalar, nv, 1 > &, Eigen::Matrix< Scalar, nv, 1 > &, void *)>
	Type definition for an objective function that takes an Eigen vector as input and returns a scalar value.

Enumerations
enum class	tinyrobotics::InverseKinematicsMethod { JACOBIAN , NLOPT , LEVENBERG_MARQUARDT , PARTICLE_SWARM , BFGS }
	Solver methods for inverse kinematics. More...

Functions
template<typename Scalar , int n>
void	tinyrobotics::eigen_to_nlopt (const Eigen::Matrix< Scalar, n, 1 > &eigen_vec, std::vector< Scalar > &nlopt_vec)
	Converts an Eigen vector to an std::vector.

template<typename Scalar , int n>
void	tinyrobotics::nlopt_to_eigen (const std::vector< Scalar > &nlopt_vec, Eigen::Matrix< Scalar, n, 1 > &eigen_vec)
	Converts an std::vector to an Eigen vector.

template<typename Scalar , int nv>
Scalar	tinyrobotics::eigen_objective_wrapper (unsigned n, const Scalar x, Scalar grad, void *data)
	Wrapper function that converts input and output between NLopt and Eigen formats for an objective function.

template<typename Scalar , int nq>
Scalar	tinyrobotics::cost (const Eigen::Matrix< Scalar, nq, 1 > &q, Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > &q0, Eigen::Matrix< Scalar, nq, 1 > &gradient, const InverseKinematicsOptions< Scalar, nq > &options)
	Cost function for general inverse kinematics with analytical jacobian.

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics_nlopt (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using NLopt.

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics_jacobian (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using the Jacobian method.

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics_levenberg_marquardt (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using the Levenberg-Marquardt method.

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics_pso (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using Particle Swarm Optimization.

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics_bfgs (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using Broyden-Fletcher-Goldfarb-Shanno (BFGS).

template<typename Scalar , int nq>
Eigen::Matrix< Scalar, nq, 1 >	tinyrobotics::inverse_kinematics (Model< Scalar, nq > &model, const std::string &target_link_name, const std::string &source_link_name, const Eigen::Transform< Scalar, 3, Eigen::Isometry > &desired_pose, const Eigen::Matrix< Scalar, nq, 1 > q0, const InverseKinematicsOptions< Scalar, nq > &options)
	Solves the inverse kinematics problem between two links using user specified method.

Detailed Description

Contains inverse kinematics algorithms.

Typedef Documentation

◆ ObjectiveFunction

template<typename Scalar , int nv>

using tinyrobotics::ObjectiveFunction = typedef std::function<Scalar(const Eigen::Matrix<Scalar, nv, 1>&, Eigen::Matrix<Scalar, nv, 1>&, void*)>

Type definition for an objective function that takes an Eigen vector as input and returns a scalar value.

Template Parameters

Scalar	The scalar type of the Eigen vector and the scalar return value.
nv	The size of the Eigen vector.

Enumeration Type Documentation

◆ InverseKinematicsMethod

enum class tinyrobotics::InverseKinematicsMethod

strong

Solver methods for inverse kinematics.

Enumerator
JACOBIAN	Jacobian method.
NLOPT	NLopt method.
LEVENBERG_MARQUARDT	Levenberg-Marquardt method.
PARTICLE_SWARM	Particle swarm optimization method.
BFGS	BFGS method.

Function Documentation

◆ cost()

template<typename Scalar , int nq>

Scalar tinyrobotics::cost	(	const Eigen::Matrix< Scalar, nq, 1 > &	q,
		Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 > &	q0,
		Eigen::Matrix< Scalar, nq, 1 > &	gradient,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Cost function for general inverse kinematics with analytical jacobian.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	Initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ eigen_objective_wrapper()

template<typename Scalar , int nv>

Scalar tinyrobotics::eigen_objective_wrapper	(	unsigned	n,
		const Scalar *	x,
		Scalar *	grad,
		void *	data
	)

inline

Wrapper function that converts input and output between NLopt and Eigen formats for an objective function.

Parameters

n	The size of the input vector.
x	The input vector in NLopt format.
grad	The gradient vector in NLopt format.
data	Pointer to additional data that is passed to the objective function.

Template Parameters

Scalar	The scalar type of the Eigen vector and the scalar return value.
nv	The size of the Eigen vector.

Returns: The scalar value of the objective function.

◆ eigen_to_nlopt()

template<typename Scalar , int n>

void tinyrobotics::eigen_to_nlopt	(	const Eigen::Matrix< Scalar, n, 1 > &	eigen_vec,
		std::vector< Scalar > &	nlopt_vec
	)

inline

Converts an Eigen vector to an std::vector.

Parameters

eigen_vec	The Eigen vector to convert.
nlopt_vec	The resulting std::vector.

Template Parameters

Scalar	The scalar type of the Eigen vector.
n	The size of the Eigen vector.

◆ inverse_kinematics()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using user specified method.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ inverse_kinematics_bfgs()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics_bfgs	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using Broyden-Fletcher-Goldfarb-Shanno (BFGS).

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ inverse_kinematics_jacobian()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics_jacobian	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using the Jacobian method.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ inverse_kinematics_levenberg_marquardt()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics_levenberg_marquardt	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using the Levenberg-Marquardt method.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ inverse_kinematics_nlopt()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics_nlopt	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using NLopt.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ inverse_kinematics_pso()

template<typename Scalar , int nq>

Eigen::Matrix< Scalar, nq, 1 > tinyrobotics::inverse_kinematics_pso	(	Model< Scalar, nq > &	model,
		const std::string &	target_link_name,
		const std::string &	source_link_name,
		const Eigen::Transform< Scalar, 3, Eigen::Isometry > &	desired_pose,
		const Eigen::Matrix< Scalar, nq, 1 >	q0,
		const InverseKinematicsOptions< Scalar, nq > &	options
	)

Solves the inverse kinematics problem between two links using Particle Swarm Optimization.

Parameters

model	tinyrobotics model.
target_link_name	{t} Link to which the transform is computed.
source_link_name	{s} Link from which the transform is computed.
desired_pose	Desired pose of the target link in the source link frame.
q0	The initial guess for the configuration vector.

Template Parameters

Scalar	type of the tinyrobotics model.
nq	Number of configuration coordinates (degrees of freedom).

Returns: The configuration vector of the robot model which achieves the desired pose.

◆ nlopt_to_eigen()

template<typename Scalar , int n>

void tinyrobotics::nlopt_to_eigen	(	const std::vector< Scalar > &	nlopt_vec,
		Eigen::Matrix< Scalar, n, 1 > &	eigen_vec
	)

inline

Converts an std::vector to an Eigen vector.

Parameters

nlopt_vec	The std::vector to convert.
eigen_vec	The resulting Eigen vector.

Template Parameters

Scalar	The scalar type of the Eigen vector.
n	The size of the Eigen vector.

Classes

Typedefs

Enumerations

Functions

Detailed Description

Typedef Documentation

◆ ObjectiveFunction

Enumeration Type Documentation

◆ InverseKinematicsMethod

Function Documentation

◆ cost()

◆ eigen_objective_wrapper()

◆ eigen_to_nlopt()

◆ inverse_kinematics()

◆ inverse_kinematics_bfgs()

◆ inverse_kinematics_jacobian()

◆ inverse_kinematics_levenberg_marquardt()

◆ inverse_kinematics_nlopt()

◆ inverse_kinematics_pso()

◆ nlopt_to_eigen()