Kinematics utility functions. More...

#include <tesseract/common/macros.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <string>
#include <vector>
#include <tesseract/common/logging.h>
#include <tesseract/common/utils.h>
#include <tesseract/common/kinematic_limits.h>
#include <tesseract/common/types.h>
#include <tesseract/scene_graph/fwd.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 tesseract::common::LinkId &link_id, 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 tesseract::common::LinkId &link_id, 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 tesseract::common::LinkId &base_link_id, const Eigen::Isometry3d &base_link_offset, const tesseract::common::LinkId &link_id, const Eigen::Isometry3d &link_offset)
	Numerically calculate the jacobian of a frame on one link relative to a frame on another link.

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.

double	tesseract::kinematics::computeChainReachUpperBound (const tesseract::scene_graph::SceneGraph &scene_graph, const tesseract::common::LinkId &base_link_id, const tesseract::common::LinkId &tip_link_id)
	Compute an upper bound on the Cartesian distance from `base_link_id` to `tip_link_id` across all valid joint configurations of the chain.

Eigen::MatrixX2d	tesseract::kinematics::gatherJointLimits (const tesseract::scene_graph::SceneGraph &scene_graph, const std::vector< tesseract::common::JointId > &joint_ids)
	Look up each joint in `scene_graph` and return their position limits as a (N,2) matrix.

std::vector< Eigen::VectorXd >	tesseract::kinematics::buildSampleGrid (const Eigen::MatrixX2d &range, const Eigen::VectorXd &resolution)
	Build a per-joint sample grid by uniformly subdividing each row of `range` using `resolution`.

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 tesseract::common::LinkId &	link_id,
		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_id	The link id 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 tesseract::common::LinkId &	link_id,
		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_id	The link id 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 tesseract::common::LinkId &	base_link_id,
		const Eigen::Isometry3d &	base_link_offset,
		const tesseract::common::LinkId &	link_id,
		const Eigen::Isometry3d &	link_offset
	)

Numerically calculate the jacobian of a frame on one link relative to a frame on another link.

It is the twist of the offset frame on the link as seen from the offset frame on the base link, expressed in the latter, per unit joint velocity. Either link may be active or static.

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_id	The link id the jacobian is relative to
base_link_offset	The frame on the base link, given in the base link frame, the jacobian is expressed in
link_id	The link id for which the jacobian is calculated
link_offset	The frame on the link, given in the link frame, whose origin the jacobian refers to

◆ 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

◆ computeChainReachUpperBound()

double tesseract::kinematics::computeChainReachUpperBound	(	const tesseract::scene_graph::SceneGraph &	scene_graph,
		const tesseract::common::LinkId &	base_link_id,
		const tesseract::common::LinkId &	tip_link_id
	)

Compute an upper bound on the Cartesian distance from base_link_id to tip_link_id across all valid joint configurations of the chain.

Walks the shortest path from base_link_id to tip_link_id and sums, per joint along the path:

parent_to_joint_origin_transform.translation().norm() (fixed geometric offset)
max(|lower|, |upper|) (only for PRISMATIC joints)

Revolute, continuous, and fixed joints contribute zero beyond their offset - rotation does not translate the tip in the chain's own frame. The sum upper-bounds T_base_to_tip.translation().norm() by the triangle inequality.

Intended use: sizing the early-exit reach filter in RTPInvKin so the filter never fires on genuinely reachable targets.

Parameters

scene_graph	Scene graph to query. Must contain both `base_link_id` and `tip_link_id` and have a path from one to the other.
base_link_id	Chain start.
tip_link_id	Chain end.

Returns: Upper bound in metres. Always > 0 if the chain contains at least one joint with a non-zero offset or a prismatic extension; returns 0 for a zero-length chain (base_link_id == tip_link_id).

Exceptions

std::runtime_error if either link is missing from scene_graph, if no path exists between them, if any joint along the path is FLOATING / PLANAR (unbounded translation), or if a PRISMATIC joint along the path is a mimic joint or has no finite limits.

◆ gatherJointLimits()

Eigen::MatrixX2d tesseract::kinematics::gatherJointLimits	(	const tesseract::scene_graph::SceneGraph &	scene_graph,
		const std::vector< tesseract::common::JointId > &	joint_ids
	)

Look up each joint in scene_graph and return their position limits as a (N,2) matrix.

Column 0 is the lower limit, column 1 the upper. A CONTINUOUS joint is unbounded, so it reports one full turn, [-pi, pi], whatever its limits hold. Used by RTPInvKin to derive a default sampling range from joint limits when the caller did not supply one.

Exceptions

std::runtime_error if any joint id is missing from scene_graph or if any matched non-continuous joint has a null limits member.

◆ buildSampleGrid()

std::vector< Eigen::VectorXd > tesseract::kinematics::buildSampleGrid	(	const Eigen::MatrixX2d &	range,
		const Eigen::VectorXd &	resolution
	)

Build a per-joint sample grid by uniformly subdividing each row of range using resolution.

For each joint i, returns LinSpaced(cnt, range(i,0), range(i,1)) where cnt = ceil((range(i,1) - range(i,0)) / resolution(i)) + 1. The number of samples is chosen so the actual step never exceeds the requested resolution.

Used by RTPInvKin to discretise the tool chain.

Parameters

range	(N,2) matrix; column 0 is the lower bound, column 1 the upper, per joint. Both bounds must be finite and ordered - joints whose limits were never set (or were left infinite) must be given an explicit range by the caller.
resolution	(N,) vector; per-joint maximum step size. Must be finite and > 0 elementwise.

Returns: Vector of N Eigen::VectorXd, each containing the sample points for one joint.

Exceptions

std::runtime_error if resolution and range disagree on the joint count, if any bound is non-finite or inverted, if any resolution is non-finite or not greater than zero, or if a range/resolution pair would need more than 10 million samples.

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()

◆ computeChainReachUpperBound()

◆ gatherJointLimits()

◆ buildSampleGrid()