kinematics.cpp File Reference

This defines kinematic related utilities. More...

`#include <stomp_moveit/utils/kinematics.h>`
`#include <kdl_parser/kdl_parser.hpp>`
`#include <eigen_conversions/eigen_kdl.h>`
`#include <math.h>`
`#include <random>`
Include dependency graph for kinematics.cpp:

Go to the source code of this file.

## Namespaces

stomp_moveit

stomp_moveit::utils::kinematics
Utility functions related to finding Inverse Kinematics solutions.

## Functions

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. More...

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. More...

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. More...

std::shared_ptr< TRAC_IK::TRAC_IKcreateTRACIKSolver (moveit::core::RobotModelConstPtr robot_model, const moveit::core::JointModelGroup *group, double max_time=0.01)

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. More...

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. More...

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. More...

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. More...

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. More...

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. More...

KDL::JntArray toKDLJntArray (const std::vector< double > &vals)

## Variables

static const std::string DEBUG_NS = "stomp_moveit_kinematics"

## Detailed Description

This defines kinematic related utilities.

Date
June 26, 2017
Version
TODO
Bug:
No known bugs