WIP: PR #358 rebased for melodic devel staging

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<double> &ik_seed_state,
                                   double timeout,
@@ -201,6 +207,10 @@ namespace ur_kinematics
 
     virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
 
+    bool getAllPositionIK(const geometry_msgs::Pose &ik_pose,
+                          const std::vector<double> &ik_seed_state,
+                          std::vector<std::vector<double> > &solutions) const;
+
   private:
 
     bool timedOut(const ros::WallTime &start_time, double duration) const;

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<double> Solution;
+typedef std::vector<std::vector<double> > 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<std::string>("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<double> &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<double> > q_ik_valid_sols;
     for(uint16_t i=0; i<num_sols; i++)
     {
@@ -651,28 +717,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;
         }
       }
 
@@ -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<double> &ik_seed_state,
+                                          std::vector<std::vector<double> > &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<dimension_; i++)
+    jnt_seed_state(i) = ik_seed_state[i];
+
+  std::vector<double> 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<jnt_pos_base.rows(); i++)
+    jnt_pos_base(i) = jnt_pos_test(i);
+  for(uint32_t i=0; i<jnt_pos_tip.rows(); i++)
+    jnt_pos_tip(i) = jnt_pos_test(i + ur_joint_inds_start_ + 6);
+  for(uint32_t i=0; i<jnt_seed_state.rows(); i++)
+    solution[i] = jnt_pos_test(i);
+
+  if(fk_solver_base.JntToCart(jnt_pos_base, pose_base) < 0) {
+    ROS_ERROR_NAMED("kdl", "Could not compute FK for base chain");
+    return false;
+  }
+
+  if(fk_solver_tip.JntToCart(jnt_pos_tip, pose_tip) < 0) {
+    ROS_ERROR_NAMED("kdl", "Could not compute FK for tip chain");
+    return false;
+  }
+
+  // Convert into query for analytic solver
+  tf::poseMsgToKDL(ik_pose, kdl_ik_pose);
+  kdl_ik_pose_ur_chain = pose_base.Inverse() * kdl_ik_pose * pose_tip.Inverse();
+
+  kdl_ik_pose_ur_chain.Make4x4((double*) homo_ik_pose);
+
+  // Do the analytic IK
+  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<num_sols; i++) {
+    std::vector<double> 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<std::string> &link_names,
                                         const std::vector<double> &joint_angles,
                                         std::vector<geometry_msgs::Pose> &poses) const