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..34796e864 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 { @@ -468,6 +517,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 +702,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 +824,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