Utility functions related to finding Inverse Kinematics solutions. More...
Classes | |
| struct | KinematicConfig |
Functions | |
| static 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. | |
| static 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. | |
| static 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. | |
| static bool | createKinematicConfig (const moveit::core::JointModelGroup *group, const moveit_msgs::PositionConstraint &pc, const moveit_msgs::OrientationConstraint &oc, const Eigen::VectorXd &init_joint_pose, KinematicConfig &kc) |
| Populates a Kinematic Config struct from the position and orientation constraints requested;. | |
| static bool | createKinematicConfig (const moveit::core::JointModelGroup *group, const moveit_msgs::PositionConstraint &pc, const moveit_msgs::OrientationConstraint &oc, const moveit_msgs::RobotState &start_state, KinematicConfig &kc) |
| Populates a Kinematic Config struct from the position and orientation constraints requested;. | |
| static 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. | |
| static bool | solveIK (moveit::core::RobotStatePtr robot_state, const std::string &group_name, const Eigen::Array< int, 6, 1 > &constrained_dofs, const Eigen::ArrayXd &joint_update_rates, const Eigen::Array< double, 6, 1 > &cartesian_convergence_thresholds, const Eigen::ArrayXd &null_proj_weights, const Eigen::VectorXd &null_space_vector, int max_iterations, const Eigen::Affine3d &tool_goal_pose, const Eigen::VectorXd &init_joint_pose, Eigen::VectorXd &joint_pose) |
| Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose and can also apply a vector onto the null space of the jacobian in order to meet a secondary objective. It also checks for joint limits. | |
| static bool | solveIK (moveit::core::RobotStatePtr robot_state, const std::string &group_name, const KinematicConfig &config, Eigen::VectorXd &joint_pose) |
| Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose and can also apply a vector onto the null space of the jacobian in order to meet a secondary objective. It also checks for joint limits. | |
| static bool | solveIK (moveit::core::RobotStatePtr robot_state, const std::string &group_name, const Eigen::ArrayXi &constrained_dofs, const Eigen::ArrayXd &joint_update_rates, const Eigen::ArrayXd &cartesian_convergence_thresholds, int max_iterations, const Eigen::Affine3d &tool_goal_pose, const Eigen::VectorXd &init_joint_pose, Eigen::VectorXd &joint_pose) |
| Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose. | |
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.
| static void stomp_moveit::utils::kinematics::calculateDampedPseudoInverse | ( | const Eigen::MatrixXd & | jacb, |
| Eigen::MatrixXd & | jacb_pseudo_inv, | ||
| double | eps = 0.011, |
||
| double | lambda = 0.01 |
||
| ) | [static] |
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 250 of file kinematics.h.
| static 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 | ||
| ) | [static] |
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 492 of file kinematics.h.
| static void stomp_moveit::utils::kinematics::computeTwist | ( | const Eigen::Affine3d & | p0, |
| const Eigen::Affine3d & | pf, | ||
| const Eigen::ArrayXi & | nullity, | ||
| Eigen::VectorXd & | twist | ||
| ) | [static] |
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 191 of file kinematics.h.
| static bool stomp_moveit::utils::kinematics::createKinematicConfig | ( | const moveit::core::JointModelGroup * | group, |
| const moveit_msgs::PositionConstraint & | pc, | ||
| const moveit_msgs::OrientationConstraint & | oc, | ||
| const Eigen::VectorXd & | init_joint_pose, | ||
| KinematicConfig & | kc | ||
| ) | [static] |
Populates a Kinematic Config struct from the position and orientation constraints requested;.
| group | A pointer to the JointModelGroup |
| pc | The position constraint message |
| oc | The orientation constraint message. Absolute tolerances greater than 2PI will be considered unconstrained. |
| init_joint_pose | The initial joint values |
| kc | The output KinematicConfig object |
Definition at line 93 of file kinematics.h.
| static bool stomp_moveit::utils::kinematics::createKinematicConfig | ( | const moveit::core::JointModelGroup * | group, |
| const moveit_msgs::PositionConstraint & | pc, | ||
| const moveit_msgs::OrientationConstraint & | oc, | ||
| const moveit_msgs::RobotState & | start_state, | ||
| KinematicConfig & | kc | ||
| ) | [static] |
Populates a Kinematic Config struct from the position and orientation constraints requested;.
| group | A pointer to the JointModelGroup |
| pc | The position constraint message |
| oc | The orientation constraint message. Absolute tolerances greater than 2PI will be considered unconstrained. |
| start_state | The start robot state message |
| kc | The output KinematicConfig object |
Definition at line 159 of file kinematics.h.
| static void stomp_moveit::utils::kinematics::reduceJacobian | ( | const Eigen::MatrixXd & | jacb, |
| const std::vector< int > & | indices, | ||
| Eigen::MatrixXd & | jacb_reduced | ||
| ) | [static] |
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 233 of file kinematics.h.
| static bool stomp_moveit::utils::kinematics::solveIK | ( | moveit::core::RobotStatePtr | robot_state, |
| const std::string & | group_name, | ||
| const Eigen::Array< int, 6, 1 > & | constrained_dofs, | ||
| const Eigen::ArrayXd & | joint_update_rates, | ||
| const Eigen::Array< double, 6, 1 > & | cartesian_convergence_thresholds, | ||
| const Eigen::ArrayXd & | null_proj_weights, | ||
| const Eigen::VectorXd & | null_space_vector, | ||
| int | max_iterations, | ||
| const Eigen::Affine3d & | tool_goal_pose, | ||
| const Eigen::VectorXd & | init_joint_pose, | ||
| Eigen::VectorXd & | joint_pose | ||
| ) | [static] |
Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose and can also apply a vector onto the null space of the jacobian in order to meet a secondary objective. It also checks for joint limits.
| robot_state | A pointer to the robot state. |
| group_name | The name of the kinematic group. The tool link name is assumed to be the last link in this group. |
| 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_update_rates | The weights to be applied to each update during every iteration [num_dimensions x 1]. |
| cartesian_convergence_thresholds | The error margin for each dimension of the twist vector [6 x 1]. |
| null_proj_weights | The weights to be multiplied to the null space vector [num_dimension x 1]. |
| null_space_vector | The null space vector which is applied into the jacobian's null space [num_dimension x 1]. |
| max_iterations | The maximum number of iterations that the algorithm will run until convergence is reached. |
| tool_goal_pose | The desired tool pose. |
| init_joint_pose | Seed joint pose [num_dimension x 1]. |
| joint_pose | IK joint solution [num_dimension x 1]. |
Definition at line 299 of file kinematics.h.
| static bool stomp_moveit::utils::kinematics::solveIK | ( | moveit::core::RobotStatePtr | robot_state, |
| const std::string & | group_name, | ||
| const KinematicConfig & | config, | ||
| Eigen::VectorXd & | joint_pose | ||
| ) | [static] |
Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose and can also apply a vector onto the null space of the jacobian in order to meet a secondary objective. It also checks for joint limits.
| robot_state | A pointer to the robot state. |
| group_name | The name of the kinematic group. The tool link name is assumed to be the last link in this group. |
| config | A structure containing the variables to be used in finding a solution. |
| joint_pose | IK joint solution [num_dimension x 1]. |
Definition at line 466 of file kinematics.h.
| static bool stomp_moveit::utils::kinematics::solveIK | ( | moveit::core::RobotStatePtr | robot_state, |
| const std::string & | group_name, | ||
| const Eigen::ArrayXi & | constrained_dofs, | ||
| const Eigen::ArrayXd & | joint_update_rates, | ||
| const Eigen::ArrayXd & | cartesian_convergence_thresholds, | ||
| int | max_iterations, | ||
| const Eigen::Affine3d & | tool_goal_pose, | ||
| const Eigen::VectorXd & | init_joint_pose, | ||
| Eigen::VectorXd & | joint_pose | ||
| ) | [static] |
Solves the inverse kinematics for a given tool pose using a gradient descent method. It can handle under constrained DOFs for the cartesian tool pose.
| robot_state | A pointer to the robot state. |
| group_name | The name of the kinematic group. The tool link name is assumed to be the last link in this group. |
| 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_update_rates | The weights to be applied to each update during every iteration [num_dimensions x 1]. |
| cartesian_convergence_thresholds | The error margin for each dimension of the twist vector [6 x 1]. |
| max_iterations | The maximum number of iterations that the algorithm will run to reach convergence. |
| tool_goal_pose | The cartesian pose of the tool relative to the world. |
| init_joint_pose | Seed joint pose [num_dimension x 1]. |
| joint_pose | IK joint solution [num_dimension x 1]. |
Definition at line 555 of file kinematics.h.
const double stomp_moveit::utils::kinematics::EPSILON = 0.011 [static] |
Used in dampening the matrix pseudo inverse calculation.
Definition at line 52 of file kinematics.h.
const double stomp_moveit::utils::kinematics::LAMBDA = 0.01 [static] |
Used in dampening the matrix pseudo inverse calculation.
Definition at line 53 of file kinematics.h.