diff --git a/.github/workflows/trac_ik_ci.yml b/.github/workflows/trac_ik_ci.yml index 4307ea9..49af1e7 100644 --- a/.github/workflows/trac_ik_ci.yml +++ b/.github/workflows/trac_ik_ci.yml @@ -35,6 +35,7 @@ jobs: package-name: | trac_ik trac_ik_examples + trac_ik_kinematics trac_ik_lib target-ros2-distro: ${{ matrix.ros_distribution }} diff --git a/trac_ik/package.xml b/trac_ik/package.xml index c81317d..97569b6 100644 --- a/trac_ik/package.xml +++ b/trac_ik/package.xml @@ -17,7 +17,7 @@ ament_cmake trac_ik_examples - trac_ik_kinematics_plugin + trac_ik_kinematics trac_ik_lib trac_ik_python diff --git a/trac_ik_kinematics/CMakeLists.txt b/trac_ik_kinematics/CMakeLists.txt new file mode 100644 index 0000000..f11aa50 --- /dev/null +++ b/trac_ik_kinematics/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.10.2) +project(trac_ik_kinematics) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(orocos_kdl REQUIRED) +find_package(tf2_kdl REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(rclcpp REQUIRED) +find_package(random_numbers REQUIRED) +find_package(class_loader REQUIRED) +find_package(pluginlib REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(trac_ik_lib REQUIRED) +find_package(NLopt REQUIRED) + +# Finds Boost Components +include(ConfigExtras.cmake) + +set(THIS_PACKAGE_INCLUDE_DIRS + trac_ik_kinematics_plugin/include +) + +set(THIS_PACKAGE_LIBRARIES + moveit_trac_ik_kinematics_plugin +) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + moveit_core + moveit_ros_planning + Boost +) + +pluginlib_export_plugin_description_file(moveit_core trac_ik_kinematics_plugin_description.xml) + +include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) + +add_subdirectory(trac_ik_kinematics_plugin) + +install( + TARGETS ${THIS_PACKAGE_LIBRARIES} + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # These don't pass yet, disable them for now + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_pep257_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_xmllint_FOUND TRUE) + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() +endif() + +ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/trac_ik_kinematics/ConfigExtras.cmake b/trac_ik_kinematics/ConfigExtras.cmake new file mode 100644 index 0000000..adca88d --- /dev/null +++ b/trac_ik_kinematics/ConfigExtras.cmake @@ -0,0 +1,3 @@ +# Extras module needed for dependencies to find boost components + +find_package(Boost REQUIRED program_options system) diff --git a/trac_ik_kinematics_plugin/README.md b/trac_ik_kinematics/README.md similarity index 100% rename from trac_ik_kinematics_plugin/README.md rename to trac_ik_kinematics/README.md diff --git a/trac_ik_kinematics/package.xml b/trac_ik_kinematics/package.xml new file mode 100644 index 0000000..49738fe --- /dev/null +++ b/trac_ik_kinematics/package.xml @@ -0,0 +1,31 @@ + + + trac_ik_kinematics + 1.6.7 + A MoveIt! Kinematics plugin using TRAC-IK + Anton Weiss + Patrick Beeson + BSD + + ament_cmake + moveit_common + + moveit_core + class_loader + pluginlib + eigen + tf2 + tf2_kdl + orocos_kdl + moveit_msgs + trac_ik_lib + NLopt + + urdfdom + python-lxml + + + ament_cmake + + + diff --git a/trac_ik_kinematics/trac_ik_kinematics_plugin/CMakeLists.txt b/trac_ik_kinematics/trac_ik_kinematics_plugin/CMakeLists.txt new file mode 100644 index 0000000..894ee9f --- /dev/null +++ b/trac_ik_kinematics/trac_ik_kinematics_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +set(MOVEIT_LIB_NAME moveit_trac_ik_kinematics_plugin) + +add_library(${MOVEIT_LIB_NAME} SHARED + src/trac_ik_kinematics_plugin.cpp) + +ament_target_dependencies(${MOVEIT_LIB_NAME} + rclcpp + random_numbers + pluginlib + moveit_core + moveit_msgs + orocos_kdl + kdl_parser + tf2_kdl + EIGEN3 + trac_ik_lib +) + +# prevent pluginlib from using boost +target_compile_definitions(${MOVEIT_LIB_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${MOVEIT_LIB_NAME} PRIVATE "MOVEIT_TRAC_IK_KINEMATICS_PLUGIN_BUILDING_DLL") + +install(DIRECTORY include/ DESTINATION include) \ No newline at end of file diff --git a/trac_ik_kinematics/trac_ik_kinematics_plugin/include/trac_ik_kinematics_plugin/trac_ik_kinematics_plugin.h b/trac_ik_kinematics/trac_ik_kinematics_plugin/include/trac_ik_kinematics_plugin/trac_ik_kinematics_plugin.h new file mode 100644 index 0000000..1a7d4a8 --- /dev/null +++ b/trac_ik_kinematics/trac_ik_kinematics_plugin/include/trac_ik_kinematics_plugin/trac_ik_kinematics_plugin.h @@ -0,0 +1,219 @@ +/******************************************************************************** +Copyright (c) 2015, TRACLabs, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +OF THE POSSIBILITY OF SUCH DAMAGE. +********************************************************************************/ + +#pragma once + +// ROS +#include +#include + +// ROS msgs +#include +#include +#include +#include +#include + +// MoveIt +#include +// #include +#include +#include + +#include +#include +#include +#include + +#include + +namespace trac_ik_kinematics_plugin +{ + class TRAC_IKKinematicsPlugin : public kinematics::KinematicsBase + { + + + public: + /** @class + * @brief Interface for an TRAC-IK kinematics plugin + */ + TRAC_IKKinematicsPlugin(); + + /** + * @brief Given a desired pose of the end-effector, compute the joint angles to reach it + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param solution the solution vector + * @param error_code an error code that encodes the reason for failure or success + * @return True if a valid solution was found, false otherwise + */ + + // Returns the first IK solution that is within joint limits, this is called by get_ik() service + bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param the distance that the redundancy can be from the current position + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + const std::vector &consistency_limits, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + + /** + * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. + * This particular method is intended for "searching" for a solutions by stepping through the redundancy + * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions + * around those specified in the seed state are admissible and need to be searched. + * @param ik_pose the desired pose of the link + * @param ik_seed_state an initial guess solution for the inverse kinematics + * @param consistency_limit the distance that the redundancy can be from the current position + * @return True if a valid solution was found, false otherwise + */ + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + const std::vector &consistency_limits, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const std::vector &consistency_limits, + const kinematics::KinematicsQueryOptions &options) const; + + /** + * @brief Given a set of joint angles and a set of links, compute their pose + * + * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, + * otherwise ROS TF is used to calculate the forward kinematics + * + * @param link_names A set of links for which FK needs to be computed + * @param joint_angles The state for which FK is being computed + * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) + * @return True if a valid solution was found, false otherwise + */ + bool getPositionFK(const std::vector &link_names, + const std::vector &joint_angles, + std::vector &poses) const override; + + bool initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, + const std::string &group_name, const std::string &base_frame, + const std::vector &tip_frames, double search_discretization) override; + + /** + * @brief Return all the joint names in the order they are used internally + */ + const std::vector &getJointNames() const override; + + /** + * @brief Return all the link names in the order they are represented internally + */ + const std::vector &getLinkNames() const override; + + private: + int getKDLSegmentIndex(const std::string &name) const; + + bool initialized_; ///< Internal variable that indicates whether solver is configured and ready + + unsigned int dimension_; ///< Dimension of the group + moveit_msgs::msg::KinematicSolverInfo solver_info_; ///< Stores information for the inverse kinematics solver + + const moveit::core::JointModelGroup *joint_model_group_; + moveit::core::RobotStatePtr state_; + + KDL::Chain kdl_chain_; + std::unique_ptr fk_solver_; + // std::vector mimic_joints_; + std::vector joint_weights_; + KDL::JntArray joint_min_, joint_max_; ///< joint limits + + std::vector joint_names_; + std::vector link_names_; + + uint num_joints_; + + KDL::Chain chain; + bool position_ik_; + + KDL::JntArray joint_min, joint_max; + + std::string solve_type; + }; // end class +} diff --git a/trac_ik_kinematics/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp b/trac_ik_kinematics/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp new file mode 100644 index 0000000..61e9d23 --- /dev/null +++ b/trac_ik_kinematics/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp @@ -0,0 +1,423 @@ +/******************************************************************************** +Copyright (c) 2015, TRACLabs, Inc. +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + 3. Neither the name of the copyright holder nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +OF THE POSSIBILITY OF SUCH DAMAGE. +********************************************************************************/ + +// ROS +#include +#include + +#include +#include + +// KDL +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +void poseMsgToKDL(const geometry_msgs::msg::Pose &m, KDL::Frame &k) +{ + k.p[0] = m.position.x; + k.p[1] = m.position.y; + k.p[2] = m.position.z; + + k.M = KDL::Rotation::Quaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); +} + +void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::msg::Pose &m) +{ + m.position.x = k.p[0]; + m.position.y = k.p[1]; + m.position.z = k.p[2]; + + k.M.GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w); +} + +namespace trac_ik_kinematics_plugin +{ + static rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trac_ik_kinematics_plugin.trac_ik_kinematics_plugin"); + + TRAC_IKKinematicsPlugin::TRAC_IKKinematicsPlugin() : initialized_(false) + { + } + + bool TRAC_IKKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, + const std::string &group_name, const std::string &base_frame, + const std::vector &tip_frames, double search_discretization) + { + node_ = node; + storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); + joint_model_group_ = robot_model_->getJointModelGroup(group_name); + if (!joint_model_group_) + return false; + + if (!joint_model_group_->isChain()) + { + RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain", group_name.c_str()); + return false; + } + if (!joint_model_group_->isSingleDOFJoints()) + { + RCLCPP_ERROR(LOGGER, "Group '%s' includes joints that have more than 1 DOF", group_name.c_str()); + return false; + } + + KDL::Tree kdl_tree; + + if (!kdl_parser::treeFromUrdfModel(*robot_model.getURDF(), kdl_tree)) + { + RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); + return false; + } + if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_)) + { + RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); + return false; + } + + dimension_ = joint_model_group_->getActiveJointModels().size() + joint_model_group_->getMimicJointModels().size(); + for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i) + { + if (joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || + joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC) + { + solver_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]); + const std::vector &jvec = + joint_model_group_->getJointModels()[i]->getVariableBoundsMsg(); + solver_info_.limits.insert(solver_info_.limits.end(), jvec.begin(), jvec.end()); + } + } + + if (!joint_model_group_->hasLinkModel(getTipFrame())) + { + RCLCPP_ERROR(LOGGER, "Could not find tip name in joint group '%s'", group_name.c_str()); + return false; + } + solver_info_.link_names.push_back(getTipFrame()); + + joint_min_.resize(solver_info_.limits.size()); + joint_max_.resize(solver_info_.limits.size()); + + for (unsigned int i = 0; i < solver_info_.limits.size(); i++) + { + joint_min_(i) = solver_info_.limits[i].min_position; + joint_max_(i) = solver_info_.limits[i].max_position; + } + + num_joints_ = kdl_chain_.getNrOfJoints(); + + RCLCPP_INFO(LOGGER, "number of joints: %i", num_joints_); + + // ### + + state_.reset(new moveit::core::RobotState(robot_model_)); + + fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_)); + + // ### + + lookupParam(node_, "position_only_ik", position_ik_, false); + lookupParam(node_, "solve_type", solve_type, std::string("Speed")); + + initialized_ = true; + return true; + } + + int TRAC_IKKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const + { + int i = 0; + while (i < (int)chain.getNrOfSegments()) + { + if (chain.getSegment(i).getName() == name) + { + return i + 1; + } + i++; + } + return -1; + } + + bool TRAC_IKKinematicsPlugin::getPositionFK(const std::vector &link_names, + const std::vector &joint_angles, + std::vector &poses) const + { + if (!initialized_) + { + RCLCPP_ERROR(LOGGER, "kinematics not active"); + return false; + } + poses.resize(link_names.size()); + if (joint_angles.size() != num_joints_) + { + RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", num_joints_); + return false; + } + + KDL::Frame p_out; + geometry_msgs::msg::PoseStamped pose; + geometry_msgs::msg::Pose tf_pose; + + KDL::JntArray jnt_pos_in(num_joints_); + for (unsigned int i = 0; i < num_joints_; i++) + { + jnt_pos_in(i) = joint_angles[i]; + } + + KDL::ChainFkSolverPos_recursive fk_solver(chain); + + bool valid = true; + for (unsigned int i = 0; i < poses.size(); i++) + { + RCLCPP_DEBUG(LOGGER, "End effector index: %d", getKDLSegmentIndex(link_names[i])); + if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0) + { + poseKDLToMsg(p_out, poses[i]); + } + else + { + RCLCPP_ERROR(LOGGER, "Could not compute FK for %s", link_names[i].c_str()); + valid = false; + } + } + + return valid; + } + + bool TRAC_IKKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options) const + { + const IKCallbackFn solution_callback = 0; + std::vector consistency_limits; + + return searchPositionIK(ik_pose, + ik_seed_state, + default_timeout_, + solution, + solution_callback, + error_code, + consistency_limits, + options); + } + + bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options) const + { + const IKCallbackFn solution_callback = 0; + std::vector consistency_limits; + + return searchPositionIK(ik_pose, + ik_seed_state, + timeout, + solution, + solution_callback, + error_code, + consistency_limits, + options); + } + + bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + const std::vector &consistency_limits, + std::vector &solution, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options) const + { + const IKCallbackFn solution_callback = 0; + return searchPositionIK(ik_pose, + ik_seed_state, + timeout, + solution, + solution_callback, + error_code, + consistency_limits, + options); + } + + bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options) const + { + std::vector consistency_limits; + return searchPositionIK(ik_pose, + ik_seed_state, + timeout, + solution, + solution_callback, + error_code, + consistency_limits, + options); + } + + bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + const std::vector &consistency_limits, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const kinematics::KinematicsQueryOptions &options) const + { + return searchPositionIK(ik_pose, + ik_seed_state, + timeout, + solution, + solution_callback, + error_code, + consistency_limits, + options); + } + + bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, + const std::vector &ik_seed_state, + double timeout, + std::vector &solution, + const IKCallbackFn &solution_callback, + moveit_msgs::msg::MoveItErrorCodes &error_code, + const std::vector &consistency_limits, + const kinematics::KinematicsQueryOptions &options) const + { + RCLCPP_DEBUG_STREAM(LOGGER, "getPositionIK"); + + if (!initialized_) + { + RCLCPP_ERROR(LOGGER, "kinematics not active"); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + if (ik_seed_state.size() != num_joints_) + { + RCLCPP_ERROR_STREAM(LOGGER, "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); + error_code.val = error_code.NO_IK_SOLUTION; + return false; + } + + KDL::Frame frame; + poseMsgToKDL(ik_pose, frame); + + KDL::JntArray in(num_joints_), out(num_joints_); + + for (uint z = 0; z < num_joints_; z++) + in(z) = ik_seed_state[z]; + + KDL::Twist bounds = KDL::Twist::Zero(); + + if (position_ik_) + { + bounds.rot.x(std::numeric_limits::max()); + bounds.rot.y(std::numeric_limits::max()); + bounds.rot.z(std::numeric_limits::max()); + } + + double epsilon = 1e-5; //Same as MoveIt's KDL plugin + + TRAC_IK::SolveType solvetype; + + if (solve_type == "Manipulation1") + solvetype = TRAC_IK::Manip1; + else if (solve_type == "Manipulation2") + solvetype = TRAC_IK::Manip2; + else if (solve_type == "Distance") + solvetype = TRAC_IK::Distance; + else + { + if (solve_type != "Speed") + { + RCLCPP_WARN_STREAM(LOGGER, solve_type << " is not a valid solve_type; setting to default: Speed"); + } + solvetype = TRAC_IK::Speed; + } + + TRAC_IK::TRAC_IK ik_solver(kdl_chain_, joint_min_, joint_max_, timeout, epsilon, solvetype); + + int rc = ik_solver.CartToJnt(in, frame, out, bounds); + + solution.resize(num_joints_); + + if (rc >= 0) + { + for (uint z = 0; z < num_joints_; z++) + solution[z] = out(z); + + // check for collisions if a callback is provided + if (!solution_callback.empty()) + { + solution_callback(ik_pose, solution, error_code); + if (error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_DEBUG(LOGGER, "Solution passes callback"); + return true; + } + else + { + RCLCPP_DEBUG(LOGGER, "Solution has error code %i", error_code); + return false; + } + } + else + return true; // no collision check callback provided + } + + error_code.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION; + return false; + } + + const std::vector &TRAC_IKKinematicsPlugin::getJointNames() const + { + return solver_info_.joint_names; + } + + const std::vector &TRAC_IKKinematicsPlugin::getLinkNames() const + { + return solver_info_.link_names; + } + +} // end namespace + +//register TRAC_IKKinematicsPlugin as a KinematicsBase implementation +#include +CLASS_LOADER_REGISTER_CLASS(trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin, kinematics::KinematicsBase) diff --git a/trac_ik_kinematics_plugin/trac_ik_kinematics_description.xml b/trac_ik_kinematics/trac_ik_kinematics_plugin_description.xml similarity index 82% rename from trac_ik_kinematics_plugin/trac_ik_kinematics_description.xml rename to trac_ik_kinematics/trac_ik_kinematics_plugin_description.xml index ff90ade..f822f9c 100644 --- a/trac_ik_kinematics_plugin/trac_ik_kinematics_description.xml +++ b/trac_ik_kinematics/trac_ik_kinematics_plugin_description.xml @@ -1,7 +1,7 @@ - + A implementation of kinematics as a plugin based on TRAC-IK. - + \ No newline at end of file diff --git a/trac_ik_kinematics_plugin/CMakeLists.txt b/trac_ik_kinematics_plugin/CMakeLists.txt deleted file mode 100644 index 8a7b840..0000000 --- a/trac_ik_kinematics_plugin/CMakeLists.txt +++ /dev/null @@ -1,54 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(trac_ik_kinematics_plugin) - -if(CMAKE_COMPILER_IS_GNUCXX OR ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")) - set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") -endif() - -find_package(PkgConfig REQUIRED) -pkg_check_modules(pkg_nlopt REQUIRED nlopt) - -find_package(catkin REQUIRED - COMPONENTS - moveit_core - pluginlib - roscpp - tf_conversions - trac_ik_lib -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${pkg_nlopt_INCLUDE_DIRS} -) - -catkin_package( - INCLUDE_DIRS - include - CATKIN_DEPENDS - moveit_core - pluginlib - roscpp - tf_conversions - trac_ik_lib -) - -set(TRAC_IK_LIBRARY_NAME trac_ik_kinematics_plugin) - -add_library(${TRAC_IK_LIBRARY_NAME} src/trac_ik_kinematics_plugin.cpp) -target_link_libraries(${TRAC_IK_LIBRARY_NAME} ${catkin_LIBRARIES} ${pkg_nlopt_LIBRARIES}) - -install(DIRECTORY include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} -) - -install(TARGETS ${TRAC_IK_LIBRARY_NAME} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install( - FILES - trac_ik_kinematics_description.xml - DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/trac_ik_kinematics_plugin/COLCON_IGNORE b/trac_ik_kinematics_plugin/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/trac_ik_kinematics_plugin/include/trac_ik/trac_ik_kinematics_plugin.hpp b/trac_ik_kinematics_plugin/include/trac_ik/trac_ik_kinematics_plugin.hpp deleted file mode 100644 index 0fbf2e3..0000000 --- a/trac_ik_kinematics_plugin/include/trac_ik/trac_ik_kinematics_plugin.hpp +++ /dev/null @@ -1,197 +0,0 @@ -/******************************************************************************** -Copyright (c) 2015, TRACLabs, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, - are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - 3. Neither the name of the copyright holder nor the names of its contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -OF THE POSSIBILITY OF SUCH DAMAGE. -********************************************************************************/ - -#ifndef TRAC_IK_KINEMATICS_PLUGIN_ -#define TRAC_IK_KINEMATICS_PLUGIN_ - -#include -#include - -namespace trac_ik_kinematics_plugin -{ - -class TRAC_IKKinematicsPlugin : public kinematics::KinematicsBase -{ - std::vector joint_names_; - std::vector link_names_; - - uint num_joints_; - bool active_; // Internal variable that indicates whether solvers are configured and ready - - KDL::Chain chain; - bool position_ik_; - - KDL::JntArray joint_min, joint_max; - - std::string solve_type; - -public: - const std::vector& getJointNames() const - { - return joint_names_; - } - const std::vector& getLinkNames() const - { - return link_names_; - } - - - /** @class - * @brief Interface for an TRAC-IK kinematics plugin - */ - TRAC_IKKinematicsPlugin(): active_(false), position_ik_(false) {} - - ~TRAC_IKKinematicsPlugin() - { - } - - /** - * @brief Given a desired pose of the end-effector, compute the joint angles to reach it - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param solution the solution vector - * @param error_code an error code that encodes the reason for failure or success - * @return True if a valid solution was found, false otherwise - */ - - // Returns the first IK solution that is within joint limits, this is called by get_ik() service - bool getPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param the distance that the redundancy can be from the current position - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy - * (or other numerical routines). - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; - - /** - * @brief Given a desired pose of the end-effector, search for the joint angles required to reach it. - * This particular method is intended for "searching" for a solutions by stepping through the redundancy - * (or other numerical routines). The consistency_limit specifies that only certain redundancy positions - * around those specified in the seed state are admissible and need to be searched. - * @param ik_pose the desired pose of the link - * @param ik_seed_state an initial guess solution for the inverse kinematics - * @param consistency_limit the distance that the redundancy can be from the current position - * @return True if a valid solution was found, false otherwise - */ - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; - - bool searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const std::vector &consistency_limits, - const kinematics::KinematicsQueryOptions &options) const; - - - /** - * @brief Given a set of joint angles and a set of links, compute their pose - * - * This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node, - * otherwise ROS TF is used to calculate the forward kinematics - * - * @param link_names A set of links for which FK needs to be computed - * @param joint_angles The state for which FK is being computed - * @param poses The resultant set of poses (in the frame returned by getBaseFrame()) - * @return True if a valid solution was found, false otherwise - */ - bool getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const; - - - bool initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, - double search_discretization); - -private: - - int getKDLSegmentIndex(const std::string &name) const; - -}; // end class -} - -#endif diff --git a/trac_ik_kinematics_plugin/package.xml b/trac_ik_kinematics_plugin/package.xml deleted file mode 100644 index 4e961df..0000000 --- a/trac_ik_kinematics_plugin/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - trac_ik_kinematics_plugin - 1.6.6 - A MoveIt! Kinematics plugin using TRAC-IK - Abrar Rahman Protyasha - Patrick Beeson - BSD - - catkin - - moveit_core - pluginlib - roscpp - tf_conversions - trac_ik_lib - libnlopt-dev - libnlopt-cxx-dev - - moveit_core - pluginlib - roscpp - tf_conversions - trac_ik_lib - libnlopt-dev - libnlopt0 - - - - - diff --git a/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp b/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp deleted file mode 100644 index 5a50ce3..0000000 --- a/trac_ik_kinematics_plugin/src/trac_ik_kinematics_plugin.cpp +++ /dev/null @@ -1,416 +0,0 @@ -/******************************************************************************** -Copyright (c) 2015, TRACLabs, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, - are permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - 3. Neither the name of the copyright holder nor the names of its contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. -IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE -OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -OF THE POSSIBILITY OF SUCH DAMAGE. -********************************************************************************/ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace trac_ik_kinematics_plugin -{ - -bool TRAC_IKKinematicsPlugin::initialize(const std::string &robot_description, - const std::string& group_name, - const std::string& base_name, - const std::string& tip_name, - double search_discretization) -{ - std::vector tip_names = {tip_name}; - setValues(robot_description, group_name, base_name, tip_names, search_discretization); - - ros::NodeHandle node_handle("~"); - - urdf::Model robot_model; - std::string xml_string; - - std::string urdf_xml, full_urdf_xml; - node_handle.param("urdf_xml", urdf_xml, robot_description); - node_handle.searchParam(urdf_xml, full_urdf_xml); - - ROS_DEBUG_NAMED("trac_ik", "Reading xml file from parameter server"); - if (!node_handle.getParam(full_urdf_xml, xml_string)) - { - ROS_FATAL_NAMED("trac_ik", "Could not load the xml from parameter server: %s", urdf_xml.c_str()); - return false; - } - - node_handle.param(full_urdf_xml, xml_string, std::string()); - robot_model.initString(xml_string); - - ROS_DEBUG_STREAM_NAMED("trac_ik", "Reading joints and links from URDF"); - - KDL::Tree tree; - - if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) - { - ROS_FATAL("Failed to extract kdl tree from xml robot description"); - return false; - } - - if (!tree.getChain(base_name, tip_name, chain)) - { - ROS_FATAL("Couldn't find chain %s to %s", base_name.c_str(), tip_name.c_str()); - return false; - } - - num_joints_ = chain.getNrOfJoints(); - - std::vector chain_segs = chain.segments; - - urdf::JointConstSharedPtr joint; - - std::vector l_bounds, u_bounds; - - joint_min.resize(num_joints_); - joint_max.resize(num_joints_); - - uint joint_num = 0; - for (unsigned int i = 0; i < chain_segs.size(); ++i) - { - - link_names_.push_back(chain_segs[i].getName()); - joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); - if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) - { - joint_num++; - assert(joint_num <= num_joints_); - float lower, upper; - int hasLimits; - joint_names_.push_back(joint->name); - if (joint->type != urdf::Joint::CONTINUOUS) - { - if (joint->safety) - { - lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); - upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); - } - else - { - lower = joint->limits->lower; - upper = joint->limits->upper; - } - hasLimits = 1; - } - else - { - hasLimits = 0; - } - if (hasLimits) - { - joint_min(joint_num - 1) = lower; - joint_max(joint_num - 1) = upper; - } - else - { - joint_min(joint_num - 1) = std::numeric_limits::lowest(); - joint_max(joint_num - 1) = std::numeric_limits::max(); - } - ROS_INFO_STREAM("IK Using joint " << chain_segs[i].getName() << " " << joint_min(joint_num - 1) << " " << joint_max(joint_num - 1)); - } - } - - ROS_INFO_NAMED("trac-ik plugin", "Looking in common namespaces for param name: %s", (group_name + "/position_only_ik").c_str()); - lookupParam(group_name + "/position_only_ik", position_ik_, false); - ROS_INFO_NAMED("trac-ik plugin", "Looking in common namespaces for param name: %s", (group_name + "/solve_type").c_str()); - lookupParam(group_name + "/solve_type", solve_type, std::string("Speed")); - ROS_INFO_NAMED("trac_ik plugin", "Using solve type %s", solve_type.c_str()); - - active_ = true; - return true; -} - - -int TRAC_IKKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const -{ - int i = 0; - while (i < (int)chain.getNrOfSegments()) - { - if (chain.getSegment(i).getName() == name) - { - return i + 1; - } - i++; - } - return -1; -} - - -bool TRAC_IKKinematicsPlugin::getPositionFK(const std::vector &link_names, - const std::vector &joint_angles, - std::vector &poses) const -{ - if (!active_) - { - ROS_ERROR_NAMED("trac_ik", "kinematics not active"); - return false; - } - poses.resize(link_names.size()); - if (joint_angles.size() != num_joints_) - { - ROS_ERROR_NAMED("trac_ik", "Joint angles vector must have size: %d", num_joints_); - return false; - } - - KDL::Frame p_out; - geometry_msgs::PoseStamped pose; - tf::Stamped tf_pose; - - KDL::JntArray jnt_pos_in(num_joints_); - for (unsigned int i = 0; i < num_joints_; i++) - { - jnt_pos_in(i) = joint_angles[i]; - } - - KDL::ChainFkSolverPos_recursive fk_solver(chain); - - bool valid = true; - for (unsigned int i = 0; i < poses.size(); i++) - { - ROS_DEBUG_NAMED("trac_ik", "End effector index: %d", getKDLSegmentIndex(link_names[i])); - if (fk_solver.JntToCart(jnt_pos_in, p_out, getKDLSegmentIndex(link_names[i])) >= 0) - { - tf::poseKDLToMsg(p_out, poses[i]); - } - else - { - ROS_ERROR_NAMED("trac_ik", "Could not compute FK for %s", link_names[i].c_str()); - valid = false; - } - } - - return valid; -} - - -bool TRAC_IKKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const -{ - const IKCallbackFn solution_callback = 0; - std::vector consistency_limits; - - return searchPositionIK(ik_pose, - ik_seed_state, - default_timeout_, - solution, - solution_callback, - error_code, - consistency_limits, - options); -} - -bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const -{ - const IKCallbackFn solution_callback = 0; - std::vector consistency_limits; - - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - solution, - solution_callback, - error_code, - consistency_limits, - options); -} - -bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const -{ - const IKCallbackFn solution_callback = 0; - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - solution, - solution_callback, - error_code, - consistency_limits, - options); -} - -bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const -{ - std::vector consistency_limits; - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - solution, - solution_callback, - error_code, - consistency_limits, - options); -} - -bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - const std::vector &consistency_limits, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const kinematics::KinematicsQueryOptions &options) const -{ - return searchPositionIK(ik_pose, - ik_seed_state, - timeout, - solution, - solution_callback, - error_code, - consistency_limits, - options); -} - -bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, - const std::vector &ik_seed_state, - double timeout, - std::vector &solution, - const IKCallbackFn &solution_callback, - moveit_msgs::MoveItErrorCodes &error_code, - const std::vector &consistency_limits, - const kinematics::KinematicsQueryOptions &options) const -{ - ROS_DEBUG_STREAM_NAMED("trac_ik", "getPositionIK"); - - if (!active_) - { - ROS_ERROR("kinematics not active"); - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - if (ik_seed_state.size() != num_joints_) - { - ROS_ERROR_STREAM_NAMED("trac_ik", "Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size()); - error_code.val = error_code.NO_IK_SOLUTION; - return false; - } - - KDL::Frame frame; - tf::poseMsgToKDL(ik_pose, frame); - - KDL::JntArray in(num_joints_), out(num_joints_); - - for (uint z = 0; z < num_joints_; z++) - in(z) = ik_seed_state[z]; - - KDL::Twist bounds = KDL::Twist::Zero(); - - if (position_ik_) - { - bounds.rot.x(std::numeric_limits::max()); - bounds.rot.y(std::numeric_limits::max()); - bounds.rot.z(std::numeric_limits::max()); - } - - double epsilon = 1e-5; //Same as MoveIt's KDL plugin - - TRAC_IK::SolveType solvetype; - - if (solve_type == "Manipulation1") - solvetype = TRAC_IK::Manip1; - else if (solve_type == "Manipulation2") - solvetype = TRAC_IK::Manip2; - else if (solve_type == "Distance") - solvetype = TRAC_IK::Distance; - else - { - if (solve_type != "Speed") - { - ROS_WARN_STREAM_NAMED("trac_ik", solve_type << " is not a valid solve_type; setting to default: Speed"); - } - solvetype = TRAC_IK::Speed; - } - - TRAC_IK::TRAC_IK ik_solver(chain, joint_min, joint_max, timeout, epsilon, solvetype); - - int rc = ik_solver.CartToJnt(in, frame, out, bounds); - - - solution.resize(num_joints_); - - if (rc >= 0) - { - for (uint z = 0; z < num_joints_; z++) - solution[z] = out(z); - - // check for collisions if a callback is provided - if (!solution_callback.empty()) - { - solution_callback(ik_pose, solution, error_code); - if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) - { - ROS_DEBUG_STREAM_NAMED("trac_ik", "Solution passes callback"); - return true; - } - else - { - ROS_DEBUG_STREAM_NAMED("trac_ik", "Solution has error code " << error_code); - return false; - } - } - else - return true; // no collision check callback provided - } - - error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION; - return false; -} - - - -} // end namespace - -//register TRAC_IKKinematicsPlugin as a KinematicsBase implementation -#include -PLUGINLIB_EXPORT_CLASS(trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin, kinematics::KinematicsBase);