Common Tesseract Utility Functions. More...

#include <tesseract/common/macros.h>
#include <ctime>
#include <string>
#include <type_traits>
#include <console_bridge/console.h>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <tinyxml2.h>
#include <tesseract/common/utils.h>

Functions
template bool	tesseract::common::toNumeric< double > (const std::string &, double &)

template bool	tesseract::common::toNumeric< float > (const std::string &, float &)

template bool	tesseract::common::toNumeric< int > (const std::string &, int &)

template bool	tesseract::common::toNumeric< long > (const std::string &, long &)

template bool	tesseract::common::isIdentical< std::string > (const std::vector< std::string > &, const std::vector< std::string > &, bool, const std::function< bool(const std::string &, const std::string &)> &, const std::function< bool(const std::string &, const std::string &)> &)

template bool	tesseract::common::isIdentical< Eigen::Index > (const std::vector< Eigen::Index > &, const std::vector< Eigen::Index > &, bool, const std::function< bool(const Eigen::Index &, const Eigen::Index &)> &, const std::function< bool(const Eigen::Index &, const Eigen::Index &)> &)

template<typename E >
std::enable_if_t<!std::is_polymorphic< E >::value >	tesseract::common::my_rethrow_if_nested (const E &)

template<typename E >
std::enable_if_t< std::is_polymorphic< E >::value >	tesseract::common::my_rethrow_if_nested (const E &e)

std::string	tesseract::common::fileToString (const std::filesystem::path &filepath)
	Read in the contents of the file into a string.

void	tesseract::common::twistChangeRefPoint (Eigen::Ref< Eigen::VectorXd > twist, const Eigen::Ref< const Eigen::Vector3d > &ref_point)
	Change the reference point of the provided Twist.

void	tesseract::common::twistChangeBase (Eigen::Ref< Eigen::VectorXd > twist, const Eigen::Isometry3d &change_base)
	Change the base coordinate system of the Twist.

void	tesseract::common::jacobianChangeBase (Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Isometry3d &change_base)
	Change the base coordinate system of the jacobian.

void	tesseract::common::jacobianChangeRefPoint (Eigen::Ref< Eigen::MatrixXd > jacobian, const Eigen::Ref< const Eigen::Vector3d > &ref_point)
	Change the reference point of the jacobian.

Eigen::VectorXd	tesseract::common::concat (const Eigen::VectorXd &a, const Eigen::VectorXd &b)
	Concatenate two vector.

std::pair< Eigen::Vector3d, double >	tesseract::common::calcRotationalErrorDecomposed (const Eigen::Ref< const Eigen::Matrix3d > &R)

Eigen::Vector3d	tesseract::common::calcRotationalError (const Eigen::Ref< const Eigen::Matrix3d > &R)
	Calculate the rotation error vector given a rotation error matrix where the angle is between [-pi, pi].

Eigen::VectorXd	tesseract::common::calcTransformError (const Eigen::Isometry3d &t1, const Eigen::Isometry3d &t2)
	Calculate error between two transforms expressed in t1 coordinate system.

Eigen::VectorXd	tesseract::common::calcJacobianTransformErrorDiff (const Eigen::Isometry3d &target, const Eigen::Isometry3d &source, const Eigen::Isometry3d &source_perturbed)
	Calculate jacobian transform error difference expressed in the target frame coordinate system.

Eigen::VectorXd	tesseract::common::calcJacobianTransformErrorDiff (const Eigen::Isometry3d &target, const Eigen::Isometry3d &target_perturbed, const Eigen::Isometry3d &source, const Eigen::Isometry3d &source_perturbed)
	Calculate jacobian transform error difference expressed in the target frame coordinate system.

Eigen::Vector4d	tesseract::common::computeRandomColor ()
	This computes a random color RGBA [0, 1] with alpha set to 1.

void	tesseract::common::printNestedException (const std::exception &e, int level=0)
	Print a nested exception.

std::string	tesseract::common::getTempPath ()
	Get the host temp directory path.

bool	tesseract::common::isNumeric (const std::string &s)
	Determine if a string is a number.

bool	tesseract::common::isNumeric (const std::vector< std::string > &sv)
	Determine if each string in vector is a number.

Eigen::VectorXd	tesseract::common::generateRandomNumber (const Eigen::Ref< const Eigen::MatrixX2d > &limits)
	Given a set of limits it will generate a vector of random numbers between the limit.

void	tesseract::common::ltrim (std::string &s)
	Left trim string.

void	tesseract::common::rtrim (std::string &s)
	Right trim string.

void	tesseract::common::trim (std::string &s)
	Trim left and right of string.

std::string	tesseract::common::getTimestampString ()
	Get Timestamp string.

void	tesseract::common::reorder (Eigen::Ref< Eigen::VectorXd > v, std::vector< Eigen::Index > order)
	Reorder Eigen::VectorXd implace given index list.

int	tesseract::common::QueryStringValue (const tinyxml2::XMLElement *xml_element, std::string &value)
	Query a string value from xml element.

int	tesseract::common::QueryStringText (const tinyxml2::XMLElement *xml_element, std::string &text)
	Query a string Text from xml element.

int	tesseract::common::QueryStringValue (const tinyxml2::XMLAttribute *xml_attribute, std::string &value)
	Query a string value from xml attribute.

int	tesseract::common::QueryStringAttribute (const tinyxml2::XMLElement xml_element, const char name, std::string &value)
	Query a string attribute from an xml element.

std::string	tesseract::common::StringAttribute (const tinyxml2::XMLElement xml_element, const char name, std::string default_value)
	Get string attribute if exist. If it does not exist it returns the default value.

int	tesseract::common::QueryStringAttributeRequired (const tinyxml2::XMLElement xml_element, const char name, std::string &value)
	Query a string attribute from an xml element and print error log.

int	tesseract::common::QueryDoubleAttributeRequired (const tinyxml2::XMLElement xml_element, const char name, double &value)
	Query a double attribute from an xml element and print error log.

int	tesseract::common::QueryIntAttributeRequired (const tinyxml2::XMLElement xml_element, const char name, int &value)
	Query a int attribute from an xml element and print error log.

bool	tesseract::common::almostEqualRelativeAndAbs (double a, double b, double max_diff=1e-6, double max_rel_diff=std::numeric_limits< double >::epsilon())
	Check if two double are relatively equal.

bool	tesseract::common::almostEqualRelativeAndAbs (const Eigen::Ref< const Eigen::VectorXd > &v1, const Eigen::Ref< const Eigen::VectorXd > &v2, double max_diff=1e-6, double max_rel_diff=std::numeric_limits< double >::epsilon())
	Check if two Eigen VectorXd are relatively and absolute equal.

bool	tesseract::common::almostEqualRelativeAndAbs (const Eigen::Ref< const Eigen::VectorXd > &v1, const Eigen::Ref< const Eigen::VectorXd > &v2, const Eigen::Ref< const Eigen::VectorXd > &max_diff, const Eigen::Ref< const Eigen::VectorXd > &max_rel_diff)
	Check if two Eigen VectorXd are relatively and absolute equal.

std::vector< std::string >	tesseract::common::getAllowedCollisions (const std::vector< std::string > &link_names, const AllowedCollisionEntries &acm_entries, bool remove_duplicates=true)
	Gets allowed collisions for a set of link names.

Detailed Description

Common Tesseract Utility Functions.

Author: Levi Armstrong

Date: January 18, 2018

Copyright: Copyright (c) 2017, 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

◆ fileToString()

std::string tesseract::common::fileToString ( const std::filesystem::path & filepath )

Read in the contents of the file into a string.

Parameters

filepath The file to read

Returns: The contents of the file

◆ twistChangeRefPoint()

void tesseract::common::twistChangeRefPoint	(	Eigen::Ref< Eigen::VectorXd >	twist,
		const Eigen::Ref< const Eigen::Vector3d > &	ref_point
	)

Change the reference point of the provided Twist.

Parameters

twist	The current Twist which gets modified in place
ref_point	Is expressed in the same base frame of the Twist and is a vector from the old point to the new point.

◆ twistChangeBase()

void tesseract::common::twistChangeBase	(	Eigen::Ref< Eigen::VectorXd >	twist,
		const Eigen::Isometry3d &	change_base
	)

Change the base coordinate system of the Twist.

Parameters

twist	The current Twist which gets modified in place
change_base	The transform from the desired frame to the current base frame of the Twist

◆ jacobianChangeBase()

void tesseract::common::jacobianChangeBase	(	Eigen::Ref< Eigen::MatrixXd >	jacobian,
		const Eigen::Isometry3d &	change_base
	)

Change the base coordinate system of the jacobian.

Parameters

jacobian	The current Jacobian which gets modified in place
change_base	The transform from the desired frame to the current base frame of the jacobian

◆ jacobianChangeRefPoint()

void tesseract::common::jacobianChangeRefPoint	(	Eigen::Ref< Eigen::MatrixXd >	jacobian,
		const Eigen::Ref< const Eigen::Vector3d > &	ref_point
	)

Change the reference point of the jacobian.

Parameters

jacobian	The current Jacobian which gets modified in place
ref_point	Is expressed in the same base frame of the jacobian and is a vector from the old point to the new point.

◆ calcRotationalError()

Eigen::Vector3d tesseract::common::calcRotationalError ( const Eigen::Ref< const Eigen::Matrix3d > & R )

Calculate the rotation error vector given a rotation error matrix where the angle is between [-pi, pi].

This should be used only for calculating the error. Do not use for numerically calculating jacobians because it breaks down a -PI and PI

Parameters

R	rotation error matrix

Returns: Rotation error vector = Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()

◆ calcTransformError()

Eigen::VectorXd tesseract::common::calcTransformError	(	const Eigen::Isometry3d &	t1,
		const Eigen::Isometry3d &	t2
	)

Calculate error between two transforms expressed in t1 coordinate system.

Warning: This should not be used to calculate numerical jacobian, see calcJacobianTransformErrorDiff

Parameters

t1	Target Transform
t2	Current Transform

Returns: error [Position, calcRotationalError(Angle Axis)]

◆ calcJacobianTransformErrorDiff() [1/2]

Eigen::VectorXd tesseract::common::calcJacobianTransformErrorDiff	(	const Eigen::Isometry3d &	target,
		const Eigen::Isometry3d &	source,
		const Eigen::Isometry3d &	source_perturbed
	)

Calculate jacobian transform error difference expressed in the target frame coordinate system.

This is used when the target is a fixed frame in the environment

Parameters

target	Target The desired transform to express the transform error difference in
source	The current location of the source transform
source_perturbed	The perturbed location of the source transform

Returns: The change in error represented in the target frame

◆ calcJacobianTransformErrorDiff() [2/2]

Eigen::VectorXd tesseract::common::calcJacobianTransformErrorDiff	(	const Eigen::Isometry3d &	target,
		const Eigen::Isometry3d &	target_perturbed,
		const Eigen::Isometry3d &	source,
		const Eigen::Isometry3d &	source_perturbed
	)

Calculate jacobian transform error difference expressed in the target frame coordinate system.

This is used when the target and source are both dynamic links

Parameters

target	Target The desired transform to express the transform error difference in
target_perturbed	The perturbed location of the target transform
source	The current location of the source transform
source_perturbed	The perturbed location of the source transform

Returns: The change in error represented in the target frame

◆ computeRandomColor()

Eigen::Vector4d tesseract::common::computeRandomColor ( )

This computes a random color RGBA [0, 1] with alpha set to 1.

Returns: A random RGBA color

◆ printNestedException()

void tesseract::common::printNestedException	(	const std::exception &	e,
		int	level = `0`
	)

Print a nested exception.

Parameters

e	The exception to print
level	The exception level which controls the indentation

◆ getTempPath()

std::string tesseract::common::getTempPath ( )

Get the host temp directory path.

Returns: The host temp directory path

◆ isNumeric() [1/2]

bool tesseract::common::isNumeric ( const std::string & s )

Determine if a string is a number.

Parameters

s	The string to evaluate

Returns: True if numeric, otherwise false.

◆ isNumeric() [2/2]

bool tesseract::common::isNumeric ( const std::vector< std::string > & sv )

Determine if each string in vector is a number.

Parameters

sv	The vector of strings to evaluate

Returns: True if all strings are numeric, otherwise false.

◆ generateRandomNumber()

Eigen::VectorXd tesseract::common::generateRandomNumber ( const Eigen::Ref< const Eigen::MatrixX2d > & limits )

Given a set of limits it will generate a vector of random numbers between the limit.

Parameters

limits The limits to generated numbers based on.

Returns: A vector of random numbers between the provided limits.

◆ ltrim()

void tesseract::common::ltrim ( std::string & s )

Left trim string.

Parameters

s	The string to left trim

◆ rtrim()

void tesseract::common::rtrim ( std::string & s )

Right trim string.

Parameters

s	The string to right trim

◆ trim()

void tesseract::common::trim ( std::string & s )

Trim left and right of string.

Parameters

s	The string to trim

◆ getTimestampString()

std::string tesseract::common::getTimestampString ( )

Get Timestamp string.

Returns: Timestamp string

◆ reorder()

void tesseract::common::reorder	(	Eigen::Ref< Eigen::VectorXd >	v,
		std::vector< Eigen::Index >	order
	)

Reorder Eigen::VectorXd implace given index list.

Parameters

v	The vector to reorder
order	A vector of index which define the new order

◆ QueryStringValue() [1/2]

int tesseract::common::QueryStringValue	(	const tinyxml2::XMLElement *	xml_element,
		std::string &	value
	)

Query a string value from xml element.

Parameters

xml_element	The xml element to query string from
value	The value to update from the xml element

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE

◆ QueryStringText()

int tesseract::common::QueryStringText	(	const tinyxml2::XMLElement *	xml_element,
		std::string &	text
	)

Query a string Text from xml element.

Parameters

xml_element	The xml element to query string from
test	The value to update from the xml element

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE

◆ QueryStringValue() [2/2]

int tesseract::common::QueryStringValue	(	const tinyxml2::XMLAttribute *	xml_attribute,
		std::string &	value
	)

Query a string value from xml attribute.

Parameters

xml_attribute	The xml attribute to query string from
value	The value to update from the xml attribute

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_WRONG_ATTRIBUTE_TYPE

◆ QueryStringAttribute()

int tesseract::common::QueryStringAttribute	(	const tinyxml2::XMLElement *	xml_element,
		const char *	name,
		std::string &	value
	)

Query a string attribute from an xml element.

Parameters

xml_element	The xml attribute to query attribute
name	The name of the attribute to query
value	The value to update from the xml attribute

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or tinyxml2::XML_WRONG_ATTRIBUTE_TYPE

◆ StringAttribute()

std::string tesseract::common::StringAttribute	(	const tinyxml2::XMLElement *	xml_element,
		const char *	name,
		std::string	default_value
	)

Get string attribute if exist. If it does not exist it returns the default value.

Parameters

xml_element	The xml element to attempt to parse attribute
name	The name of the attribute
default_value	The default value to return if attribute does not exist

Returns: Parsed string attribute or default value if attribute does not exist.

◆ QueryStringAttributeRequired()

int tesseract::common::QueryStringAttributeRequired	(	const tinyxml2::XMLElement *	xml_element,
		const char *	name,
		std::string &	value
	)

Query a string attribute from an xml element and print error log.

This is the same QueryStringAttribute but it will print out error messages for the failure conditions so the user only needs to check for the tinyxml2::XML_SUCCESS since it is a required attribute.

Parameters

xml_element	The xml attribute to query attribute
name	The name of the attribute to query
value	The value to update from the xml attribute

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or tinyxml2::XML_WRONG_ATTRIBUTE_TYPE

◆ QueryDoubleAttributeRequired()

int tesseract::common::QueryDoubleAttributeRequired	(	const tinyxml2::XMLElement *	xml_element,
		const char *	name,
		double &	value
	)

Query a double attribute from an xml element and print error log.

This is the same QueryDoubleAttribute but it will print out error messages for the failure conditions so the user only needs to check for the tinyxml2::XML_SUCCESS since it is a required attribute.

Parameters

xml_element	The xml attribute to query attribute
name	The name of the attribute to query
value	The value to update from the xml attribute

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or tinyxml2::XML_WRONG_ATTRIBUTE_TYPE

◆ QueryIntAttributeRequired()

int tesseract::common::QueryIntAttributeRequired	(	const tinyxml2::XMLElement *	xml_element,
		const char *	name,
		int &	value
	)

Query a int attribute from an xml element and print error log.

This is the same QueryIntAttribute but it will print out error messages for the failure conditions so the user only needs to check for the tinyxml2::XML_SUCCESS since it is a required attribute.

Parameters

xml_element	The xml attribute to query attribute
name	The name of the attribute to query
value	The value to update from the xml attribute

Returns: tinyxml2::XML_SUCCESS if successful, otherwise returns tinyxml2::XML_NO_ATTRIBUTE or tinyxml2::XML_WRONG_ATTRIBUTE_TYPE

◆ almostEqualRelativeAndAbs() [1/3]

bool tesseract::common::almostEqualRelativeAndAbs	(	double	a,
		double	b,
		double	max_diff = `1e-6`,
		double	max_rel_diff = `std::numeric_limits<double>::epsilon()`
	)

Check if two double are relatively equal.

The max_diff is for handling when comparing numbers near zero

Parameters

a	Double
b	Double
max_diff	The max diff when comparing std::abs(a - b) <= max_diff, if true they are considered equal
max_rel_diff	The max relative diff std::abs(a - b) <= largest * max_rel_diff, if true considered equal. The largest is the largest of the absolute values of a and b.

Returns: True if they are relatively equal, otherwise false.

◆ almostEqualRelativeAndAbs() [2/3]

bool tesseract::common::almostEqualRelativeAndAbs	(	const Eigen::Ref< const Eigen::VectorXd > &	v1,
		const Eigen::Ref< const Eigen::VectorXd > &	v2,
		double	max_diff = `1e-6`,
		double	max_rel_diff = `std::numeric_limits<double>::epsilon()`
	)

Check if two Eigen VectorXd are relatively and absolute equal.

This is a vectorized implementation of the function above. The max_diff is for handling when comparing numbers near zero

Parameters

a	A vector of double
b	A vector of double
max_diff	The max diff when comparing max(abs(a - b)) <= max_diff, if true they are considered equal
max_rel_diff	The max relative diff abs(a - b) <= largest * max_rel_diff, if true considered equal. The largest is the largest of the absolute values of a and b.

Returns: True if they are relatively equal, otherwise false.

◆ almostEqualRelativeAndAbs() [3/3]

bool tesseract::common::almostEqualRelativeAndAbs	(	const Eigen::Ref< const Eigen::VectorXd > &	v1,
		const Eigen::Ref< const Eigen::VectorXd > &	v2,
		const Eigen::Ref< const Eigen::VectorXd > &	max_diff,
		const Eigen::Ref< const Eigen::VectorXd > &	max_rel_diff
	)

Check if two Eigen VectorXd are relatively and absolute equal.

This is a vectorized implementation of the function above. The max_diff is for handling when comparing numbers near zero

Parameters

a	A vector of double
b	A vector of double
max_diff	The max diff when comparing max(abs(a - b)) <= max_diff, if true they are considered equal
max_rel_diff	The max relative diff abs(a - b) <= largest * max_rel_diff, if true considered equal. The largest is the largest of the absolute values of a and b.

Returns: True if they are relatively equal, otherwise false.

◆ getAllowedCollisions()

std::vector< std::string > tesseract::common::getAllowedCollisions	(	const std::vector< std::string > &	link_names,
		const AllowedCollisionEntries &	acm_entries,
		bool	remove_duplicates = `true`
	)

Gets allowed collisions for a set of link names.

Parameters

link_names	Vector of link names for which we want the allowed collisions
acm_entries	Entries in the ACM. Get this with AllowedCollisionMatrix::getAllAllowedCollisions()
remove_duplicates	If true, duplicates will be removed. Default: true

Returns: vector of links that are allowed to collide with links given