utils.cpp File Reference

Kinematics utility functions. More...

#include <tesseract/common/macros.h>
#include <Eigen/Dense>
#include <cmath>
#include <tesseract/kinematics/utils.h>
#include <tesseract/kinematics/joint_group.h>
#include <tesseract/kinematics/forward_kinematics.h>
#include <tesseract/scene_graph/graph.h>
#include <tesseract/scene_graph/joint.h>

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 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.
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 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_id	The link id for which the jacobian is calculated in
base_link_offset	The offset on the base link for which to calculate the jacobian in
link_id	The link id 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

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.