From e303b5a041fb8e43b0af98a662e2e64793a1c58c Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Sun, 24 Apr 2016 23:28:01 -0500 Subject: [PATCH 1/7] 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 From 38e27c389fbc449801e39e2b6d746c42d1a4e5af Mon Sep 17 00:00:00 2001 From: Andrew Price Date: Thu, 20 Jul 2017 18:36:06 -0400 Subject: [PATCH 2/7] Added function to enumerate all possible joint angles to achieve a given kinematic configuration. --- ur_kinematics/src/ur_moveit_plugin.cpp | 94 +++++++++++++++----------- 1 file changed, 55 insertions(+), 39 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 160558603..bb639c5ac 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 */ @@ -91,9 +91,59 @@ //register KDLKinematics as a KinematicsBase implementation CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::KinematicsBase) + namespace ur_kinematics { +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 @@ -649,43 +699,9 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 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); - } + for(uint16_t i=0; i kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); + enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, q_ik_valid_sols, 6); } From 9a6783900ec8507163c8065a9f6166bbf026f7e9 Mon Sep 17 00:00:00 2001 From: Dennis Hartmann Date: Tue, 22 Aug 2017 13:26:39 +0200 Subject: [PATCH 3/7] =?UTF-8?q?Changes=20ur=5Fmoveit=5Fplugin=20to=20get?= =?UTF-8?q?=20solutions=20closest=20to=20the=20seed=20in=20the=20full=20ra?= =?UTF-8?q?nge=20of=20joint=20limits.=20Rotates=20joint=20by=20+-360=C2=B0?= =?UTF-8?q?=20if=20it=20is=20possible=20and=20gets=20the=20solution=20clos?= =?UTF-8?q?er=20to=20the=20seed.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ur_kinematics/src/ur_moveit_plugin.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 160558603..fdd8da16c 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -657,28 +657,22 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, 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)) + 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; } } From c7402e4245ca5a0a9537447a224c21045a92505c Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 11 Jul 2018 15:33:54 +0800 Subject: [PATCH 4/7] Remove outdated declaration --- ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h index 0a075f3e2..f847c2809 100644 --- a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h +++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h @@ -248,11 +248,6 @@ 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 */ From 27d89072ccbf3fa34cbb3214dd91a36c270cb660 Mon Sep 17 00:00:00 2001 From: Grey Date: Tue, 8 Dec 2020 14:24:15 +0800 Subject: [PATCH 5/7] Remove useless placeholder variable Co-authored-by: Simon Schmeisser --- ur_kinematics/src/ur_moveit_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 545b5fe0d..8a54fa97f 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -894,7 +894,7 @@ bool URKinematicsPlugin::getAllPositionIK(const geometry_msgs::Pose &ik_pose, num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, jnt_pos_test(ur_joint_inds_start_+5)); - std::vector< std::vector > q_ik_valid_sols; + for(uint16_t i=0; i kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, q_ik_valid_sols, 6); From ba0ca9911897d00304cbf13fc8d1c1113d43f0f7 Mon Sep 17 00:00:00 2001 From: Grey Date: Tue, 8 Dec 2020 14:24:32 +0800 Subject: [PATCH 6/7] Fill in the correct output argument Co-authored-by: Simon Schmeisser --- ur_kinematics/src/ur_moveit_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 8a54fa97f..d7b9a9252 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -897,7 +897,7 @@ bool URKinematicsPlugin::getAllPositionIK(const geometry_msgs::Pose &ik_pose, for(uint16_t i=0; i kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); - enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, q_ik_valid_sols, 6); + enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, solutions, 6); } return solutions.size() > 0; } From 9b1c804ddb6cdbe3d47d338ef5e92caa49f564cd Mon Sep 17 00:00:00 2001 From: Grey Date: Tue, 8 Dec 2020 14:28:07 +0800 Subject: [PATCH 7/7] Clear the output argument before filling it in --- ur_kinematics/src/ur_moveit_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index d7b9a9252..39bd483fb 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -894,7 +894,7 @@ bool URKinematicsPlugin::getAllPositionIK(const geometry_msgs::Pose &ik_pose, num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, jnt_pos_test(ur_joint_inds_start_+5)); - + solutions.clear(); for(uint16_t i=0; i kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, solutions, 6);