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

Author

Jorge Nicho

Date

June 26, 2017

Version

TODO

Bug:

No known bugs

Copyright

Copyright (c) 2016, Southwest Research Institute

License

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 http://www.apache.org/licenses/LICENSE-2.0

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.