kinematics.h File Reference

This defines kinematic related utilities. More...

#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <XmlRpcException.h>
#include <moveit_msgs/PositionConstraint.h>
#include <moveit_msgs/OrientationConstraint.h>

Include dependency graph for kinematics.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	stomp_moveit::utils::kinematics::KinematicConfig
Namespaces
namespace	stomp_moveit
namespace	stomp_moveit::utils
namespace	stomp_moveit::utils::kinematics
	Utility functions related to finding Inverse Kinematics solutions.
Functions
static 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.
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)
	Convenience function to calculate the Jacobian's null space matrix for an under constrained tool cartesian pose.
static 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.
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)
	Populates a Kinematic Config struct from the position and orientation constraints requested;.
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)
	Populates a Kinematic Config struct from the position and orientation constraints requested;.
static 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.
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)
	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	stomp_moveit::utils::kinematics::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	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)
	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	stomp_moveit::utils::kinematics::EPSILON = 0.011
	Used in dampening the matrix pseudo inverse calculation.
static const double	stomp_moveit::utils::kinematics::LAMBDA = 0.01
	Used in dampening the matrix pseudo inverse calculation.

---

Detailed Description

This defines kinematic related utilities.

Author:

Jorge Nicho

Date:

June 3, 2016

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