Kinematics utility functions. More...

#include <tesseract/common/macros.h>
#include <Eigen/Dense>
#include <tesseract/kinematics/utils.h>
#include <tesseract/kinematics/joint_group.h>
#include <tesseract/kinematics/forward_kinematics.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.

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

Functions

Detailed Description

Function Documentation

◆ numericalJacobian() [1/3]

◆ numericalJacobian() [2/3]

◆ numericalJacobian() [3/3]

◆ solvePInv()

◆ dampedPInv()

◆ isNearSingularity()

◆ calcManipulability()