Namespaces | Functions | Variables
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.


 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. 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...
KDL::Chain createKDLChain (moveit::core::RobotModelConstPtr robot_model, std::string base_link, std::string tip_link)
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)


static const std::string DEBUG_NS = "stomp_moveit_kinematics"

Detailed Description

This defines kinematic related utilities.

Jorge Nicho
June 26, 2017
No known bugs
Software License Agreement (Apache License)
Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

Definition in file kinematics.cpp.

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