diff --git a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h index 1d36f4f8c..7eb03bcb9 100644 --- a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h +++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h @@ -121,6 +121,12 @@ namespace ur_kinematics moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const; + virtual bool getPositionIK(const std::vector< geometry_msgs::Pose > &ik_poses, + const std::vector< double > &ik_seed_state, + std::vector< std::vector< double > > &solutions, + kinematics::KinematicsResult &result, + const kinematics::KinematicsQueryOptions &options) const; + virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, @@ -201,6 +207,10 @@ namespace ur_kinematics virtual bool setRedundantJoints(const std::vector &redundant_joint_indices); + bool getAllPositionIK(const geometry_msgs::Pose &ik_pose, + const std::vector &ik_seed_state, + std::vector > &solutions) const; + private: bool timedOut(const ros::WallTime &start_time, double duration) const; diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index a565d19e1..39bd483fb 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * -* Copyright (c) 2014, Georgia Tech +* Copyright (c) 2014, 2017 Georgia Tech * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Kelsey Hawkins */ +/* Author: Kelsey Hawkins, Andrew Price */ /* Based on orignal source from Willow Garage. License copied below */ @@ -94,7 +94,56 @@ CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::Kinem namespace ur_kinematics { - URKinematicsPlugin::URKinematicsPlugin():active_(false) {} +typedef std::vector Solution; +typedef std::vector > SolutionContainer; +typedef moveit_msgs::KinematicSolverInfo::_limits_type Limits; + +void enumerateSolutionsHelper(const Solution& initial_sln, const Limits& limits, Solution& partial_sln, SolutionContainer& container, const int num_joints, const int j) +{ + if (j == num_joints) + { + // A new solution is constructed and ready to be inserted + container.push_back(partial_sln); + } + else + { + double q = initial_sln[j]; + + // Add the current joint to the partial solution + if (limits[j].min_position <= q && q <= limits[j].max_position) + { + partial_sln[j] = q; + enumerateSolutionsHelper(initial_sln, limits, partial_sln, container, num_joints, j+1); + } + + // Search up the configuration space + q = initial_sln[j] + 2.0*M_PI; + while (q <= limits[j].max_position) + { + partial_sln[j] = q; + enumerateSolutionsHelper(initial_sln, limits, partial_sln, container, num_joints, j+1); + q += 2.0*M_PI; + } + + // Search down the configuration space + q = initial_sln[j] - 2.0*M_PI; + while (q >= limits[j].min_position) + { + partial_sln[j] = q; + enumerateSolutionsHelper(initial_sln, limits, partial_sln, container, num_joints, j+1); + q -= 2.0*M_PI; + } + } +} + +void enumeratePeriodicSolutions(const Solution& initial_sln, const Limits& limits, SolutionContainer& container, const int num_joints) +{ + assert(limits.size() == num_joints); + Solution partial(num_joints, 0.0); + enumerateSolutionsHelper(initial_sln, limits, partial, container, num_joints, 0); +} + +URKinematicsPlugin::URKinematicsPlugin():active_(false) {} void URKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const { @@ -173,6 +222,7 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, { setValues(robot_description, group_name, base_frame, tip_frame, search_discretization); + ros::NodeHandle private_handle("~"); rdf_loader::RDFLoader rdf_loader(robot_description_); const srdf::ModelSharedPtr &srdf = rdf_loader.getSRDF(); const urdf::ModelInterfaceSharedPtr &urdf_model = rdf_loader.getURDF(); @@ -249,9 +299,12 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, double epsilon; bool position_ik; - lookupParam("max_solver_iterations", max_solver_iterations, 500); - lookupParam("epsilon", epsilon, 1e-5); - lookupParam(group_name+"/position_only_ik", position_ik, false); + private_handle.param("max_solver_iterations", max_solver_iterations, 500); + private_handle.param("epsilon", epsilon, 1e-5); + private_handle.param(group_name+"/position_only_ik", position_ik, false); + ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s", + private_handle.getNamespace().c_str(), + (group_name+"/position_only_ik").c_str()); if(position_ik) ROS_INFO_NAMED("kdl","Using position only ik"); @@ -319,7 +372,7 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, max_solver_iterations_ = max_solver_iterations; epsilon_ = epsilon; - lookupParam("arm_prefix", arm_prefix_, std::string("")); + private_handle.param("arm_prefix", arm_prefix_, ""); ur_joint_names_.push_back(arm_prefix_ + "shoulder_pan_joint"); ur_joint_names_.push_back(arm_prefix_ + "shoulder_lift_joint"); @@ -365,13 +418,15 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, // weights for redundant solution selection ik_weights_.resize(6); - if(!lookupParam("ik_weights", ik_weights_, ik_weights_)) { + if(private_handle.hasParam("ik_weights")) { + private_handle.getParam("ik_weights", ik_weights_); + } else { ik_weights_[0] = 1.0; ik_weights_[1] = 1.0; - ik_weights_[2] = 1.0; - ik_weights_[3] = 1.0; - ik_weights_[4] = 1.0; - ik_weights_[5] = 1.0; + ik_weights_[2] = 0.1; + ik_weights_[3] = 0.1; + ik_weights_[4] = 0.3; + ik_weights_[5] = 0.3; } active_ = true; @@ -468,6 +523,18 @@ bool URKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose, options); } +bool URKinematicsPlugin::getPositionIK(const std::vector< geometry_msgs::Pose > &ik_poses, + const std::vector< double > &ik_seed_state, + std::vector< std::vector< double > > &solutions, + kinematics::KinematicsResult &result, + const kinematics::KinematicsQueryOptions &options) const +{ + if (ik_poses.size() == 1) + return getAllPositionIK(ik_poses.front(), ik_seed_state, solutions); + else + return false; +} + bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, @@ -641,7 +708,6 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, jnt_pos_test(ur_joint_inds_start_+5)); - uint16_t num_valid_sols; std::vector< std::vector > q_ik_valid_sols; for(uint16_t i=0; i= ik_chain_info_.limits[j].min_position)) - { - valid_solution[j] = q_ik_sols[i][j]; - valid = true; - continue; - } - else if ((q_ik_sols[i][j] > ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position)) + double distance = q_ik_sols[i][j] - ik_seed_state[ur_joint_inds_start_+j]; + if ((distance > M_PI || (q_ik_sols[i][j] > ik_chain_info_.limits[j].max_position)) && (q_ik_sols[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position)) { valid_solution[j] = q_ik_sols[i][j]-2*M_PI; - valid = true; - continue; } - else if ((q_ik_sols[i][j] < ik_chain_info_.limits[j].min_position) && (q_ik_sols[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position)) + else if ((distance < -M_PI || (q_ik_sols[i][j] < ik_chain_info_.limits[j].min_position)) && (q_ik_sols[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position)) { valid_solution[j] = q_ik_sols[i][j]+2*M_PI; - valid = true; - continue; + } + else if((q_ik_sols[i][j] <= ik_chain_info_.limits[j].max_position) && (q_ik_sols[i][j] >= ik_chain_info_.limits[j].min_position)) + { + valid_solution[j] = q_ik_sols[i][j]; } else { valid = false; - break; } } @@ -770,6 +830,78 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, return false; } +bool URKinematicsPlugin::getAllPositionIK(const geometry_msgs::Pose &ik_pose, + const std::vector &ik_seed_state, + std::vector > &solutions) const +{ + if(!active_) { + ROS_ERROR_NAMED("kdl","kinematics not active"); + return false; + } + + if(ik_seed_state.size() != dimension_) { + ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size()); + return false; + } + + KDL::JntArray jnt_seed_state(dimension_); + for(int i=0; i solution; + solution.resize(dimension_); + + KDL::ChainFkSolverPos_recursive fk_solver_base(kdl_base_chain_); + KDL::ChainFkSolverPos_recursive fk_solver_tip(kdl_tip_chain_); + + KDL::JntArray jnt_pos_test(jnt_seed_state); + KDL::JntArray jnt_pos_base(ur_joint_inds_start_); + KDL::JntArray jnt_pos_tip(dimension_ - 6 - ur_joint_inds_start_); + KDL::Frame pose_base, pose_tip; + + KDL::Frame kdl_ik_pose; + KDL::Frame kdl_ik_pose_ur_chain; + double homo_ik_pose[4][4]; + double q_ik_sols[8][6]; // maximum of 8 IK solutions + uint16_t num_sols; + + ///////////////////////////////////////////////////////////////////////////// + // find transformation from robot base to UR base and UR tip to robot tip + for(uint32_t i=0; i kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); + enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, solutions, 6); + } + return solutions.size() > 0; +} + bool URKinematicsPlugin::getPositionFK(const std::vector &link_names, const std::vector &joint_angles, std::vector &poses) const