Kinematics utility functions. More...

#include <tesseract/common/macros.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <console_bridge/console.h>
#include <tesseract/common/utils.h>
#include <tesseract/common/kinematic_limits.h>

Classes
struct	tesseract::kinematics::ManipulabilityEllipsoid
	Used to store Manipulability and Force Ellipsoid data. More...

struct	tesseract::kinematics::Manipulability
	Contains both manipulability ellipsoid and force ellipsoid data. More...

Typedefs
template<typename FloatType >
using	tesseract::kinematics::VectorX = Eigen::Matrix< FloatType, Eigen::Dynamic, 1 >

Functions
void	tesseract::kinematics::numericalJacobian (Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base, const ForwardKinematics &kin, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name, const Eigen::Ref< const Eigen::Vector3d > &link_point)
	Numerically calculate a jacobian. This is mainly used for testing.

void	tesseract::kinematics::numericalJacobian (Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base, const JointGroup &joint_group, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &link_name, const Eigen::Ref< const Eigen::Vector3d > &link_point)
	Numerically calculate a jacobian. This is mainly used for testing.

void	tesseract::kinematics::numericalJacobian (Eigen::Ref< Eigen::MatrixXd > jacobian, const JointGroup &joint_group, const Eigen::Ref< const Eigen::VectorXd > &joint_values, const std::string &base_link_name, const Eigen::Isometry3d &base_link_offset, const std::string &link_name, const Eigen::Isometry3d &link_offset)
	Numerically calculate a jacobian when both source and target are active links.

bool	tesseract::kinematics::solvePInv (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::VectorXd > &b, Eigen::Ref< Eigen::VectorXd > x)
	Solve equation Ax=b for x Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

bool	tesseract::kinematics::dampedPInv (const Eigen::Ref< const Eigen::MatrixXd > &A, Eigen::Ref< Eigen::MatrixXd > P, double eps=0.011, double lambda=0.01)
	Calculate Damped Pseudoinverse Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

bool	tesseract::kinematics::isNearSingularity (const Eigen::Ref< const Eigen::MatrixXd > &jacobian, double threshold=0.01)
	Check if the provided jacobian is near a singularity.

Manipulability	tesseract::kinematics::calcManipulability (const Eigen::Ref< const Eigen::MatrixXd > &jacobian)
	Calculate manipulability data about the provided jacobian.

template<typename FloatType >
void	tesseract::kinematics::getRedundantSolutionsHelper (std::vector< VectorX< FloatType > > &redundant_sols, const Eigen::Ref< const Eigen::VectorXd > &sol, const Eigen::MatrixX2d &limits, std::vector< Eigen::Index >::const_iterator current_index, std::vector< Eigen::Index >::const_iterator end_index)
	This a recursive function for calculating all permutations of the redundant solutions.

template<typename FloatType >
void	tesseract::kinematics::getRedundantSolutions (std::vector< VectorX< FloatType > > &solutions, const Eigen::Ref< const VectorX< FloatType > > &sol, const Eigen::MatrixX2d &limits, const std::vector< Eigen::Index > &redundancy_capable_joints)
	Kinematics only return solution between PI and -PI. Provided the limits it will append redundant solutions.

template<typename FloatType >
std::vector< VectorX< FloatType > >	tesseract::kinematics::getRedundantSolutions (const Eigen::Ref< const VectorX< FloatType > > &sol, const Eigen::MatrixX2d &limits, const std::vector< Eigen::Index > &redundancy_capable_joints)
	Kinematics only return solution between PI and -PI. Provided the limits it will append redundant solutions.

template<typename FloatType >
bool	tesseract::kinematics::isValid (const std::array< FloatType, 6 > &qs)
	Given a vector of floats, this check if they are finite.

template<typename FloatType >
void	tesseract::kinematics::harmonizeTowardZero (Eigen::Ref< VectorX< FloatType > > qs, const std::vector< Eigen::Index > &redundancy_capable_joints)
	This takes an array of floats and modifies them in place to be between [-PI, PI].

template<typename FloatType >
void	tesseract::kinematics::harmonizeTowardMedian (Eigen::Ref< VectorX< FloatType > > qs, const std::vector< Eigen::Index > &redundancy_capable_joints, const Eigen::Ref< const Eigen::Matrix< FloatType, Eigen::Dynamic, 2 > > &position_limits)
	This takes the array of floats and modifies them in place to be between [-PI, PI] relative to limits median.

Detailed Description

Kinematics utility functions.

Author: Levi Armstrong

Date: April 15, 2018

Copyright: Copyright (c) 2013, Southwest Research Institute

License: Software License Agreement (Apache License)

: Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0

: Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

Function Documentation

◆ numericalJacobian() [1/3]

void tesseract::kinematics::numericalJacobian	(	Eigen::Ref< Eigen::MatrixXd >	jacobian,
		const Eigen::Isometry3d &	change_base,
		const ForwardKinematics &	kin,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	link_name,
		const Eigen::Ref< const Eigen::Vector3d > &	link_point
	)

Numerically calculate a jacobian. This is mainly used for testing.

Parameters

jacobian	(Return) The jacobian which gets filled out.
change_base	The transform from the desired frame to the current base frame of the jacobian
kin	The kinematics object
joint_values	The joint values for which to calculate the jacobian
link_name	The link_name for which the jacobian should be calculated
link_point	The point on the link for which to calculate the jacobian

◆ numericalJacobian() [2/3]

void tesseract::kinematics::numericalJacobian	(	Eigen::Ref< Eigen::MatrixXd >	jacobian,
		const Eigen::Isometry3d &	change_base,
		const JointGroup &	joint_group,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	link_name,
		const Eigen::Ref< const Eigen::Vector3d > &	link_point
	)

Numerically calculate a jacobian. This is mainly used for testing.

Parameters

jacobian	(Return) The jacobian which gets filled out.
change_base	The transform from the desired frame to the current base frame of the jacobian
joint_group	The joint group object
joint_values	The joint values for which to calculate the jacobian
link_name	The link_name for which the jacobian should be calculated
link_point	The point on the link for which to calculate the jacobian

◆ numericalJacobian() [3/3]

void tesseract::kinematics::numericalJacobian	(	Eigen::Ref< Eigen::MatrixXd >	jacobian,
		const JointGroup &	joint_group,
		const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		const std::string &	base_link_name,
		const Eigen::Isometry3d &	base_link_offset,
		const std::string &	link_name,
		const Eigen::Isometry3d &	link_offset
	)

Numerically calculate a jacobian when both source and target are active links.

Parameters

jacobian	(Return) The jacobian which gets filled out.
joint_group	The joint group object
joint_values	The joint values for which to calculate the jacobian
base_link_name	The link name for which the jacobian is calculated in
base_link_offset	The offset on the base link for which to calculate the jacobian in
link_name	The link name for which the jacobian is calculated for
link_offset	The offset on the link for which the jacobian is calcualted for

◆ solvePInv()

bool tesseract::kinematics::solvePInv	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::VectorXd > &	b,
		Eigen::Ref< Eigen::VectorXd >	x
	)

Solve equation Ax=b for x Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

Parameters

A	Input matrix (represents Jacobian)
b	Input vector (represents desired pose)
x	Output vector (represents joint values)

Returns: True if solver completes properly

◆ dampedPInv()

bool tesseract::kinematics::dampedPInv	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		Eigen::Ref< Eigen::MatrixXd >	P,
		double	eps = `0.011`,
		double	lambda = `0.01`
	)

Calculate Damped Pseudoinverse Use this SVD to compute A+ (pseudoinverse of A). Weighting still TBD.

Parameters

A	Input matrix (represents Jacobian)
P	Output matrix (represents pseudoinverse of A)
eps	Singular value threshold
lambda	Damping factor

Returns: True if Pseudoinverse completes properly

◆ isNearSingularity()

bool tesseract::kinematics::isNearSingularity	(	const Eigen::Ref< const Eigen::MatrixXd > &	jacobian,
		double	threshold = `0.01`
	)

Check if the provided jacobian is near a singularity.

This is keep separated from the forward kinematics because special consideration may need to be made based on the kinematics arrangement.

Parameters

jacobian	The jacobian to check if near a singularity
threshold	The threshold that all singular values must be greater than or equal to not be considered near a singularity

◆ calcManipulability()

Manipulability tesseract::kinematics::calcManipulability ( const Eigen::Ref< const Eigen::MatrixXd > & jacobian )

Calculate manipulability data about the provided jacobian.

Parameters

jacobian The jacobian used to calculate manipulability

Returns: The manipulability data

◆ getRedundantSolutionsHelper()

template<typename FloatType >

void tesseract::kinematics::getRedundantSolutionsHelper	(	std::vector< VectorX< FloatType > > &	redundant_sols,
		const Eigen::Ref< const Eigen::VectorXd > &	sol,
		const Eigen::MatrixX2d &	limits,
		std::vector< Eigen::Index >::const_iterator	current_index,
		std::vector< Eigen::Index >::const_iterator	end_index
	)

inline

This a recursive function for calculating all permutations of the redundant solutions.

This should not be used directly, use getRedundantSolutions function.

Making this thread_local does not help

◆ getRedundantSolutions() [1/2]

template<typename FloatType >

void tesseract::kinematics::getRedundantSolutions	(	std::vector< VectorX< FloatType > > &	solutions,
		const Eigen::Ref< const VectorX< FloatType > > &	sol,
		const Eigen::MatrixX2d &	limits,
		const std::vector< Eigen::Index > &	redundancy_capable_joints
	)

inline

Kinematics only return solution between PI and -PI. Provided the limits it will append redundant solutions.

The list of redundant solutions does not include the provided solutions.

Parameters

solutions	The object to populate with redundant solutions
sol	The solution to calculate redundant solutions about
limits	The joint limits of the robot
redundancy_capable_joints	The indices of the redundancy capable joints

◆ getRedundantSolutions() [2/2]

template<typename FloatType >

std::vector< VectorX< FloatType > > tesseract::kinematics::getRedundantSolutions	(	const Eigen::Ref< const VectorX< FloatType > > &	sol,
		const Eigen::MatrixX2d &	limits,
		const std::vector< Eigen::Index > &	redundancy_capable_joints
	)

inline

Kinematics only return solution between PI and -PI. Provided the limits it will append redundant solutions.

The list of redundant solutions does not include the provided solutions.

Parameters

sol	The solution to calculate redundant solutions about
limits	The joint limits of the robot
redundancy_capable_joints	The indices of the redundancy capable joints

◆ isValid()

template<typename FloatType >

bool tesseract::kinematics::isValid ( const std::array< FloatType, 6 > & qs )

inline

Given a vector of floats, this check if they are finite.

In the case of OPW and IKFast they can return NANS so this is used to check that solutions are valid.

Parameters

qs	A pointer to a floats array
dof	The length of the float array

Returns: True if the array is valid, otherwise false

◆ harmonizeTowardZero()

template<typename FloatType >

void tesseract::kinematics::harmonizeTowardZero	(	Eigen::Ref< VectorX< FloatType > >	qs,
		const std::vector< Eigen::Index > &	redundancy_capable_joints
	)

inline

This takes an array of floats and modifies them in place to be between [-PI, PI].

Parameters

qs	A pointer to a float array
dof	The length of the float array

◆ harmonizeTowardMedian()

template<typename FloatType >

void tesseract::kinematics::harmonizeTowardMedian	(	Eigen::Ref< VectorX< FloatType > >	qs,
		const std::vector< Eigen::Index > &	redundancy_capable_joints,
		const Eigen::Ref< const Eigen::Matrix< FloatType, Eigen::Dynamic, 2 > > &	position_limits
	)

inline

This takes the array of floats and modifies them in place to be between [-PI, PI] relative to limits median.

Parameters

qs	A pointer to a float array
position_limits	The limits leveraged for calculating median

Classes

Typedefs

Functions

Detailed Description

Function Documentation

◆ numericalJacobian() [1/3]

◆ numericalJacobian() [2/3]

◆ numericalJacobian() [3/3]

◆ solvePInv()

◆ dampedPInv()

◆ isNearSingularity()

◆ calcManipulability()

◆ getRedundantSolutionsHelper()

◆ getRedundantSolutions() [1/2]

◆ getRedundantSolutions() [2/2]

◆ isValid()

◆ harmonizeTowardZero()

◆ harmonizeTowardMedian()