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. | |
Utility functions related to finding Inverse Kinematics solutions.
| 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.
| 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.
| 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] |
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.
| 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.
| 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 |
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.
| 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 |
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.
| 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 |
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.
| c | The constraint object. It may define position, orientation or both constraint types. |
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.
| 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.
| 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 |
Definition at line 451 of file kinematics.cpp.