stomp_moveit::utils::kinematics Namespace Reference

Utility functions related to finding Inverse Kinematics solutions. More...

Classes

class  IKSolver
Wrapper around an IK solver implementation. More...

Functions

void calculateDampedPseudoInverse (const Eigen::MatrixXd &jacb, Eigen::MatrixXd &jacb_pseudo_inv, double eps=0.011, double lambda=0.01)
Calculates the damped pseudo inverse of a matrix using singular value decomposition. More...

bool computeJacobianNullSpace (moveit::core::RobotStatePtr state, std::string group, std::string tool_link, const Eigen::ArrayXi &constrained_dofs, const Eigen::VectorXd &joint_pose, Eigen::MatrixXd &jacb_nullspace)
Convenience function to calculate the Jacobian's null space matrix for an under constrained tool cartesian pose. More...

void computeTwist (const Eigen::Affine3d &p0, const Eigen::Affine3d &pf, const Eigen::ArrayXi &nullity, Eigen::VectorXd &twist)
Computes the twist vector [vx vy vz wx wy wz]' relative to the current tool coordinate system. The rotational part is composed of the product between the angle times the axis about which it rotates. More...

boost::optional< moveit_msgs::Constraints > curateCartesianConstraints (const moveit_msgs::Constraints &c, const Eigen::Affine3d &ref_pose, double default_pos_tol=0.0005, double default_rot_tol=M_PI)
Populates the missing parts of a Cartesian constraints in order to provide a constraint that can be used by the Ik solver. More...

bool decodeCartesianConstraint (moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints &constraints, Eigen::Affine3d &tool_pose, std::vector< double > &tolerance, std::string target_frame="")
Extracts the cartesian data from the constraint message. More...

bool decodeCartesianConstraint (moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints &constraints, Eigen::Affine3d &tool_pose, Eigen::VectorXd &tolerance, std::string target_frame="")
Extracts the cartesian data from the constraint message. More...

bool isCartesianConstraints (const moveit_msgs::Constraints &c)
Checks if the constraint structured contains valid data from which a proper cartesian constraint can be produced. More...

MOVEIT_CLASS_FORWARD (IKSolver)

void reduceJacobian (const Eigen::MatrixXd &jacb, const std::vector< int > &indices, Eigen::MatrixXd &jacb_reduced)
Convenience function to remove entire rows of the Jacobian matrix. More...

EigenSTL::vector_Affine3d sampleCartesianPoses (const moveit_msgs::Constraints &c, const std::vector< double > sampling_resolution={0.05, 0.05, 0.05, M_PI_2, M_PI_2, M_PI_2}, int max_samples=20)
Creates cartesian poses in accordance to the constraint and sampling resolution values. More...

Variables

static const double EPSILON = 0.011
Used in dampening the matrix pseudo inverse calculation.

static const double LAMBDA = 0.01
Used in dampening the matrix pseudo inverse calculation.

Detailed Description

Utility functions related to finding Inverse Kinematics solutions.

Function Documentation

 void stomp_moveit::utils::kinematics::calculateDampedPseudoInverse ( const Eigen::MatrixXd & jacb, Eigen::MatrixXd & jacb_pseudo_inv, double eps = 0.011, double lambda = 0.01 )

Calculates the damped pseudo inverse of a matrix using singular value decomposition.

Parameters
 jacb The jacobian matrix jacb_pseudo_inv The pseudo inverse of the matrix eps Used to threshold the singular values lambda Used in preventing division by small singular values from generating large numbers.

Definition at line 628 of file kinematics.cpp.

 bool stomp_moveit::utils::kinematics::computeJacobianNullSpace ( moveit::core::RobotStatePtr state, std::string group, std::string tool_link, const Eigen::ArrayXi & constrained_dofs, const Eigen::VectorXd & joint_pose, Eigen::MatrixXd & jacb_nullspace )

Convenience function to calculate the Jacobian's null space matrix for an under constrained tool cartesian pose.

Parameters
 state A pointer to the robot state. group The name of the kinematic group. tool_link The tool link name constrained_dofs A vector of the form [x y z rx ry rz] filled with 0's and 1's to indicate an unconstrained or fully constrained DOF. joint_pose The joint pose at which to compute the jacobian matrix. jacb_nullspace The jacobian null space matrix [num_dimensions x num_dimensions]
Returns
True if a solution was found, false otherwise.

Definition at line 660 of file kinematics.cpp.

 void stomp_moveit::utils::kinematics::computeTwist ( const Eigen::Affine3d & p0, const Eigen::Affine3d & pf, const Eigen::ArrayXi & nullity, Eigen::VectorXd & twist )

Computes the twist vector [vx vy vz wx wy wz]' relative to the current tool coordinate system. The rotational part is composed of the product between the angle times the axis about which it rotates.

Parameters
 p0 start tool pose in world coordinates pf final tool pose in world coordinates nullity array of 0's and 1's indicating which cartesian DOF's are unconstrained (0) twist the twist vector in tool coordinates (change from p0 to pf) [6 x 1].

Definition at line 581 of file kinematics.cpp.

 boost::optional< moveit_msgs::Constraints > stomp_moveit::utils::kinematics::curateCartesianConstraints ( const moveit_msgs::Constraints & c, const Eigen::Affine3d & ref_pose, double default_pos_tol = 0.0005, double default_rot_tol = M_PI )

Populates the missing parts of a Cartesian constraints in order to provide a constraint that can be used by the Ik solver.

Parameters
 c The constraint object. It may define position, orientation or both constraint types. ref_pose If no orientation or position constraints are given then this pose will be use to fill the missing information. default_pos_tol Used when no position tolerance is specified. default_rot_tol Used when no rotation tolerance is specified
Returns
A constraint object

Definition at line 240 of file kinematics.cpp.

 bool stomp_moveit::utils::kinematics::decodeCartesianConstraint ( moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints & constraints, Eigen::Affine3d & tool_pose, std::vector< double > & tolerance, std::string target_frame = "" )

Extracts the cartesian data from the constraint message.

Parameters
 model The robot model constraints A moveit_msgs message that encapsulates the cartesian constraints specifications. tool_pose The tool pose as specified in the constraint in the target_frame. tolerance The tolerance values on the tool pose as specified in the constraint message. target_frame The coordinate frame of the tool pose. If black the model root is used
Returns
True if succeeded, false otherwise.

Definition at line 322 of file kinematics.cpp.

 bool stomp_moveit::utils::kinematics::decodeCartesianConstraint ( moveit::core::RobotModelConstPtr model, const moveit_msgs::Constraints & constraints, Eigen::Affine3d & tool_pose, Eigen::VectorXd & tolerance, std::string target_frame = "" )

Extracts the cartesian data from the constraint message.

Parameters
 model The robot model constraints A moveit_msgs message that encapsulates the cartesian constraints specifications. tool_pose The tool pose as specified in the constraint message. tolerance The tolerance values on the tool pose as specified in the constraint message. target_frame The coordinate frame of the tool pose. If black the model root is used
Returns
True if succeeded, false otherwise.

Definition at line 309 of file kinematics.cpp.

 bool stomp_moveit::utils::kinematics::isCartesianConstraints ( const moveit_msgs::Constraints & c )

Checks if the constraint structured contains valid data from which a proper cartesian constraint can be produced.

Parameters
 c The constraint object. It may define position, orientation or both constraint types.
Returns
True when it is Cartesian, false otherwise.

Definition at line 208 of file kinematics.cpp.

 void stomp_moveit::utils::kinematics::reduceJacobian ( const Eigen::MatrixXd & jacb, const std::vector< int > & indices, Eigen::MatrixXd & jacb_reduced )

Convenience function to remove entire rows of the Jacobian matrix.

Parameters
 jacb The jacobian matrix of size [num_dimensions x 6] indices An indices vector where each entry indicates an row index of the jacobian that will be kept. jacb_reduced The reduced jacobian containing the only the rows indicated by the 'indices' array.

Definition at line 617 of file kinematics.cpp.

 EigenSTL::vector_Affine3d stomp_moveit::utils::kinematics::sampleCartesianPoses ( const moveit_msgs::Constraints & c, const std::vector< double > sampling_resolution = {0.05, 0.05, 0.05, M_PI_2,M_PI_2,M_PI_2}, int max_samples = 20 )

Creates cartesian poses in accordance to the constraint and sampling resolution values.

Parameters
 c The constraints which provides information on how to sample the poses. sampling_resolution The incremental values used to sample along each dimension of position and orientation (dx, dy, dz, d_rx, d_ry, d_rz) max_samples Maximum number of samples to be generated
Returns
The sampled poses

Definition at line 451 of file kinematics.cpp.

stomp_moveit
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:47