From e303b5a041fb8e43b0af98a662e2e64793a1c58c Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Sun, 24 Apr 2016 23:28:01 -0500 Subject: [PATCH] Added an overload for the new 'getAllIK' function in KinematicsBase and added an appropriate helper function. --- .../include/ur_kinematics/ur_moveit_plugin.h | 15 ++ ur_kinematics/src/ur_moveit_plugin.cpp | 167 +++++++++++++----- 2 files changed, 142 insertions(+), 40 deletions(-) diff --git a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h index 2fee59189..a55d817d1 100644 --- a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h +++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h @@ -123,6 +123,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, @@ -203,6 +209,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; @@ -238,6 +248,11 @@ namespace ur_kinematics bool isRedundantJoint(unsigned int index) const; + void filterSolutionsByLimits(const double (&solutions)[8][6], + uint16_t num_sols, + std::vector >& valid_solutions) const; + + bool active_; /** Internal variable that indicates whether solvers are configured and ready */ moveit_msgs::KinematicSolverInfo ik_chain_info_; /** Stores information for the inverse kinematics solver */ diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 160558603..bc7d40bc7 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -94,7 +94,7 @@ CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::Kinem namespace ur_kinematics { - URKinematicsPlugin::URKinematicsPlugin():active_(false) {} +URKinematicsPlugin::URKinematicsPlugin():active_(false) {} void URKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const { @@ -474,6 +474,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, @@ -555,6 +567,50 @@ typedef std::pair idx_double; bool comparator(const idx_double& l, const idx_double& r) { return l.second < r.second; } +void URKinematicsPlugin::filterSolutionsByLimits(const double (&solutions)[8][6], + uint16_t num_sols, + std::vector >& valid_solutions) const +{ + for(uint16_t i=0; i valid_solution; + valid_solution.assign(6,0.0); + + for(uint16_t j=0; j<6; j++) + { + if((solutions[i][j] <= ik_chain_info_.limits[j].max_position) && (solutions[i][j] >= ik_chain_info_.limits[j].min_position)) + { + valid_solution[j] = solutions[i][j]; + valid = true; + continue; + } + else if ((solutions[i][j] > ik_chain_info_.limits[j].max_position) && (solutions[i][j]-2*M_PI > ik_chain_info_.limits[j].min_position)) + { + valid_solution[j] = solutions[i][j]-2*M_PI; + valid = true; + continue; + } + else if ((solutions[i][j] < ik_chain_info_.limits[j].min_position) && (solutions[i][j]+2*M_PI < ik_chain_info_.limits[j].max_position)) + { + valid_solution[j] = solutions[i][j]+2*M_PI; + valid = true; + continue; + } + else + { + valid = false; + break; + } + } + + if(valid) + { + valid_solutions.push_back(valid_solution); + } + } +} + bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector &ik_seed_state, double timeout, @@ -647,46 +703,8 @@ 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 valid_solution; - valid_solution.assign(6,0.0); - - for(uint16_t j=0; j<6; j++) - { - 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]; - 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)) - { - 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)) - { - valid_solution[j] = q_ik_sols[i][j]+2*M_PI; - valid = true; - continue; - } - else - { - valid = false; - break; - } - } - - if(valid) - { - q_ik_valid_sols.push_back(valid_solution); - } - } + filterSolutionsByLimits(q_ik_sols, num_sols, q_ik_valid_sols); // use weighted absolute deviations to determine the solution closest the seed state @@ -776,6 +794,75 @@ 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 0; +} + bool URKinematicsPlugin::getPositionFK(const std::vector &link_names, const std::vector &joint_angles, std::vector &poses) const