![]() |
Tesseract 0.28.4
|
Common Tesseract Utility Functions. More...
#include <tesseract/common/macros.h>#include <array>#include <vector>#include <set>#include <string>#include <sstream>#include <stdexcept>#include <random>#include <algorithm>#include <unordered_set>#include <Eigen/Core>#include <Eigen/Geometry>#include <tesseract/common/allowed_collision_matrix.h>#include <filesystem>Functions | |
| template<typename... Args> | |
| std::string | tesseract::common::strFormat (const std::string &format, Args... args) |
| 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. | |
| 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. | |
| template<typename T > | |
| bool | tesseract::common::isIdentical (const std::vector< T > &vec1, const std::vector< T > &vec2, bool ordered=true, const std::function< bool(const T &, const T &)> &equal_pred=[](const T &v1, const T &v2) { return v1==v2;}, const std::function< bool(const T &, const T &)> &comp=[](const T &v1, const T &v2) { return v1< v2;}) |
| Check if two vector of strings are identical. | |
| template<typename KeyValueContainerType , typename ValueType > | |
| bool | tesseract::common::isIdenticalMap (const KeyValueContainerType &map_1, const KeyValueContainerType &map_2, const std::function< bool(const ValueType &, const ValueType &)> &value_eq=[](const ValueType &v1, const ValueType &v2) { return v1==v2;}) |
| Checks if 2 maps are identical. | |
| template<typename ValueType > | |
| bool | tesseract::common::isIdenticalSet (const std::set< ValueType > &set_1, const std::set< ValueType > &set_2, const std::function< bool(const ValueType &, const ValueType &)> &value_eq=[](const ValueType &v1, const ValueType &v2) { return v1==v2;}) |
| Checks if 2 sets are identical. | |
| template<typename ValueType , std::size_t Size> | |
| bool | tesseract::common::isIdenticalArray (const std::array< ValueType, Size > &array_1, const std::array< ValueType, Size > &array_2, const std::function< bool(const ValueType &, const ValueType &)> &value_eq=[](const ValueType &v1, const ValueType &v2) { return v1==v2;}) |
| Checks if 2 arrays are identical. | |
| template<typename T > | |
| bool | tesseract::common::pointersEqual (const std::shared_ptr< T > &p1, const std::shared_ptr< T > &p2) |
| Checks if 2 pointers point to objects that are ==. | |
| template<typename T > | |
| bool | tesseract::common::pointersComparison (const std::shared_ptr< T > &p1, const std::shared_ptr< T > &p2) |
| Comparison operator for the objects 2 points point to. | |
| 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. | |
| template<typename FloatType > | |
| bool | tesseract::common::toNumeric (const std::string &s, FloatType &value) |
| Convert a string to a numeric value type. | |
| template<class T , class Hash = std::hash<T>, class Eq = std::equal_to<T>> | |
| void | tesseract::common::removeDuplicates (std::vector< T > &v) |
| Remove duplicates and keep first. | |
| 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. | |
Common Tesseract Utility Functions.
| std::string tesseract::common::fileToString | ( | const std::filesystem::path & | filepath | ) |
Read in the contents of the file into a string.
| filepath | The file to read |
| 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.
| 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. |
| void tesseract::common::twistChangeBase | ( | Eigen::Ref< Eigen::VectorXd > | twist, |
| const Eigen::Isometry3d & | change_base | ||
| ) |
Change the base coordinate system of the Twist.
| 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 |
| void tesseract::common::jacobianChangeBase | ( | Eigen::Ref< Eigen::MatrixXd > | jacobian, |
| const Eigen::Isometry3d & | change_base | ||
| ) |
Change the base coordinate system of the jacobian.
| 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 |
| void tesseract::common::jacobianChangeRefPoint | ( | Eigen::Ref< Eigen::MatrixXd > | jacobian, |
| const Eigen::Ref< const Eigen::Vector3d > & | ref_point | ||
| ) |
Change the reference point of the jacobian.
| 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. |
| 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
| R | rotation error matrix |
| Eigen::VectorXd tesseract::common::calcTransformError | ( | const Eigen::Isometry3d & | t1, |
| const Eigen::Isometry3d & | t2 | ||
| ) |
Calculate error between two transforms expressed in t1 coordinate system.
| t1 | Target Transform |
| t2 | Current Transform |
| 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
| 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 |
| 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
| 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 |
| 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.
| e | The exception to print |
| level | The exception level which controls the indentation |
| 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.
| s | The string to evaluate |
| bool tesseract::common::isNumeric | ( | const std::vector< std::string > & | sv | ) |
Determine if each string in vector is a number.
| sv | The vector of strings to evaluate |
| 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.
| limits | The limits to generated numbers based on. |
| void tesseract::common::ltrim | ( | std::string & | s | ) |
Left trim string.
| s | The string to left trim |
| void tesseract::common::rtrim | ( | std::string & | s | ) |
Right trim string.
| s | The string to right trim |
| void tesseract::common::trim | ( | std::string & | s | ) |
Trim left and right of string.
| s | The string to trim |
| bool tesseract::common::isIdentical | ( | const std::vector< T > & | vec1, |
| const std::vector< T > & | vec2, | ||
| bool | ordered = true, |
||
| const std::function< bool(const T &, const T &)> & | equal_pred = [](const T& v1, const T& v2) { return v1 == v2; }, |
||
| const std::function< bool(const T &, const T &)> & | comp = [](const T& v1, const T& v2) { return v1 < v2; } |
||
| ) |
Check if two vector of strings are identical.
| vec1 | Vector strings |
| vec2 | Vector strings |
| ordered | If true order is relavent, othwise if false order is not relavent |
| equal_pred | Binary predicate passed into std::equals to determine if an element is equal. Useful for vectors of pointers |
| comp | Binary function passed into std::sort. Only used it ordered == false. Useful for vectors of pointers |
| bool tesseract::common::isIdenticalMap | ( | const KeyValueContainerType & | map_1, |
| const KeyValueContainerType & | map_2, | ||
| const std::function< bool(const ValueType &, const ValueType &)> & | value_eq = [](const ValueType& v1, const ValueType& v2) { return v1 == v2; } |
||
| ) |
Checks if 2 maps are identical.
| map_1 | First map |
| map_2 | Second map |
| bool tesseract::common::isIdenticalSet | ( | const std::set< ValueType > & | set_1, |
| const std::set< ValueType > & | set_2, | ||
| const std::function< bool(const ValueType &, const ValueType &)> & | value_eq = [](const ValueType& v1, const ValueType& v2) { return v1 == v2; } |
||
| ) |
Checks if 2 sets are identical.
| map_1 | First map |
| map_2 | Second map |
| bool tesseract::common::isIdenticalArray | ( | const std::array< ValueType, Size > & | array_1, |
| const std::array< ValueType, Size > & | array_2, | ||
| const std::function< bool(const ValueType &, const ValueType &)> & | value_eq = [](const ValueType& v1, const ValueType& v2) { return v1 == v2; } |
||
| ) |
Checks if 2 arrays are identical.
| array_1 | First array |
| array_2 | Second array |
| bool tesseract::common::pointersEqual | ( | const std::shared_ptr< T > & | p1, |
| const std::shared_ptr< T > & | p2 | ||
| ) |
Checks if 2 pointers point to objects that are ==.
| p1 | First pointer |
| p2 | Second pointer |
| bool tesseract::common::pointersComparison | ( | const std::shared_ptr< T > & | p1, |
| const std::shared_ptr< T > & | p2 | ||
| ) |
Comparison operator for the objects 2 points point to.
| p1 | First pointer |
| p2 | Second pointer |
| 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.
| v | The vector to reorder |
| order | A vector of index which define the new order |
| int tesseract::common::QueryStringValue | ( | const tinyxml2::XMLElement * | xml_element, |
| std::string & | value | ||
| ) |
Query a string value from xml element.
| xml_element | The xml element to query string from |
| value | The value to update from the xml element |
| int tesseract::common::QueryStringText | ( | const tinyxml2::XMLElement * | xml_element, |
| std::string & | text | ||
| ) |
Query a string Text from xml element.
| xml_element | The xml element to query string from |
| test | The value to update from the xml element |
| int tesseract::common::QueryStringValue | ( | const tinyxml2::XMLAttribute * | xml_attribute, |
| std::string & | value | ||
| ) |
Query a string value from xml attribute.
| xml_attribute | The xml attribute to query string from |
| value | The value to update from the 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.
| 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 |
| 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.
| 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 |
| 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.
| 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 |
| 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.
| 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 |
| 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.
| 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 |
| 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
| 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. |
| 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
| 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. |
| 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
| 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. |
| bool tesseract::common::toNumeric | ( | const std::string & | s, |
| FloatType & | value | ||
| ) |
Convert a string to a numeric value type.
| s | The string to be converted |
| value | The value to be loaded with converted string |
| void tesseract::common::removeDuplicates | ( | std::vector< T > & | v | ) |
Remove duplicates and keep first.
| v | The vector to remove duplicates from |
| 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.
| 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 |