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



stomp_moveit
Author(s): Jorge Nicho
autogenerated on Sat Jun 8 2019 19:24:01