From 32174c5268ec79a7168bb1b2e89281d7c4e2af20 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 1d36f4f8c..575303984 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; @@ -236,6 +246,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 a565d19e1..1c5ca8b78 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 { @@ -468,6 +468,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, @@ -549,6 +561,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, @@ -641,46 +697,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 @@ -770,6 +788,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 71295eb710fbf20af4ed102190406c997ecc1ac4 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 | 215 +++++++++---------------- 1 file changed, 75 insertions(+), 140 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index 1c5ca8b78..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,10 +91,60 @@ //register KDLKinematics as a KinematicsBase implementation CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::KinematicsBase) + 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,9 +223,10 @@ 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(); + const boost::shared_ptr &srdf = rdf_loader.getSRDF(); + const boost::shared_ptr& urdf_model = rdf_loader.getURDF(); if (!urdf_model || !srdf) { @@ -249,9 +300,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 +373,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 +419,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,18 +524,6 @@ 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, @@ -561,50 +605,6 @@ 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, @@ -697,8 +697,12 @@ 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; - filterSolutionsByLimits(q_ik_sols, num_sols, 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); + } // use weighted absolute deviations to determine the solution closest the seed state @@ -788,75 +792,6 @@ 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 b870c495f5cb3090f675bbe19c7e16cbfca74066 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 | 88 ++++++++++---------------- 1 file changed, 33 insertions(+), 55 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index bb639c5ac..fdd8da16c 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, 2017 Georgia Tech +* Copyright (c) 2014, 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, Andrew Price */ +/* Author: Kelsey Hawkins */ /* Based on orignal source from Willow Garage. License copied below */ @@ -91,59 +91,9 @@ //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 @@ -699,9 +649,37 @@ 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 kinematic_solution(q_ik_sols[i], q_ik_sols[i] + 6); - enumeratePeriodicSolutions(kinematic_solution, ik_chain_info_.limits, q_ik_valid_sols, 6); + for(uint16_t i=0; i valid_solution; + valid_solution.assign(6,0.0); + + for(uint16_t j=0; j<6; j++) + { + 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; + } + 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; + } + 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; + } + } + + if(valid) + { + q_ik_valid_sols.push_back(valid_solution); + } } From 8215cfbfee37675cb962c79ff54fc99327a7a64e 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 575303984..7eb03bcb9 100644 --- a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h +++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h @@ -246,11 +246,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 c0a12b2686a3d240113c3e3d1350efea07b7313e 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 | 144 +++++++++++++++++++++++-- 1 file changed, 138 insertions(+), 6 deletions(-) diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index fdd8da16c..8a54fa97f 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 { @@ -175,8 +224,8 @@ bool URKinematicsPlugin::initialize(const std::string &robot_description, ros::NodeHandle private_handle("~"); rdf_loader::RDFLoader rdf_loader(robot_description_); - const boost::shared_ptr &srdf = rdf_loader.getSRDF(); - const boost::shared_ptr& urdf_model = rdf_loader.getURDF(); + const srdf::ModelSharedPtr &srdf = rdf_loader.getSRDF(); + const urdf::ModelInterfaceSharedPtr &urdf_model = rdf_loader.getURDF(); if (!urdf_model || !srdf) { @@ -474,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, @@ -647,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_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, q_ik_valid_sols, 6); + } + return solutions.size() > 0; +} + bool URKinematicsPlugin::getPositionFK(const std::vector &link_names, const std::vector &joint_angles, std::vector &poses) const From 50664d75962cf24158517859be3da78a00e9a429 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 6ad08987b3120087c4c35633b4fcfb55150c6782 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);