fixup! cleanup LMA kinematics solver

ros-planning · Jan 30, 2019 · 29d7d9c · 29d7d9c
1 parent 1d775cf
commit 29d7d9c
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 26 deletions.
diff --git a/...matics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h b/...matics/lma_kinematics_plugin/include/moveit/lma_kinematics_plugin/lma_kinematics_plugin.h
@@ -145,6 +145,10 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase
    */
   bool checkConsistency(const Eigen::VectorXd& seed_state, const std::vector<double>& consistency_limits,
                         const Eigen::VectorXd& solution) const;
+  /** Check whether joint values satisfy joint limits */
+  bool obeysLimits(const Eigen::VectorXd& values) const;
+  /** Harmonize revolute joint values into the range -2 Pi .. 2 Pi */
+  void harmonize(Eigen::VectorXd &values) const;
 
   void getRandomConfiguration(Eigen::VectorXd& jnt_array) const;
 
@@ -165,7 +169,7 @@ class LMAKinematicsPlugin : public kinematics::KinematicsBase
   robot_state::RobotStatePtr state_;
   KDL::Chain kdl_chain_;
   std::unique_ptr<KDL::ChainFkSolverPos> fk_solver_;
-  Eigen::VectorXd joint_min_, joint_max_;  /// joint limits
+  std::vector<const robot_model::JointModel*> joints_;
 
   int max_solver_iterations_;
   double epsilon_;

diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp
@@ -118,30 +118,9 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model
   {
     if (joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE ||
         joint_model_group_->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
-    {
-      solver_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
-      const std::vector<moveit_msgs::JointLimits>& jvec =
-          joint_model_group_->getJointModels()[i]->getVariableBoundsMsg();
-      solver_info_.limits.insert(solver_info_.limits.end(), jvec.begin(), jvec.end());
-    }
-  }
-  dimension_ = solver_info_.joint_names.size();
-
-  if (!joint_model_group_->hasLinkModel(getTipFrame()))
-  {
-    ROS_ERROR_NAMED("lma", "Could not find tip name in joint group '%s'", group_name.c_str());
-    return false;
-  }
-  solver_info_.link_names.push_back(getTipFrame());
-
-  joint_min_.resize(solver_info_.limits.size());
-  joint_max_.resize(solver_info_.limits.size());
-
-  for (unsigned int i = 0; i < solver_info_.limits.size(); i++)
-  {
-    joint_min_(i) = solver_info_.limits[i].min_position;
-    joint_max_(i) = solver_info_.limits[i].max_position;
+      joints_.push_back(joint_model_group_->getJointModels()[i]);
   }
+  dimension_ = joints_.size();
 
   // Get Solver Parameters
   lookupParam("max_solver_iterations", max_solver_iterations_, 500);
@@ -222,6 +201,22 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c
                           options);
 }
 
+void LMAKinematicsPlugin::harmonize(Eigen::VectorXd& values) const
+{
+  size_t i = 0;
+  for (auto* jm : joints_)
+    jm->harmonizePosition(&values[i++]);
+}
+
+bool LMAKinematicsPlugin::obeysLimits(const Eigen::VectorXd& values) const
+{
+  size_t i = 0;
+  for (const auto& jm : joints_)
+    if (!jm->satisfiesPositionBounds(&values[i++]))
+      return false;
+  return true;
+}
+
 bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
                                            double timeout, std::vector<double>& solution,
                                            const IKCallbackFn& solution_callback,
@@ -293,8 +288,11 @@ bool LMAKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c
     int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
     if (ik_valid == 0 || options.return_approximate_solution)  // found acceptable solution
     {
+      harmonize(jnt_pos_out.data);
       if (!consistency_limits.empty() && !checkConsistency(jnt_seed_state.data, consistency_limits, jnt_pos_out.data))
         continue;
+      if (!obeysLimits(jnt_pos_out.data))
+        continue;
 
       Eigen::Map<Eigen::VectorXd>(solution.data(), solution.size()) = jnt_pos_out.data;
       if (!solution_callback.empty())
@@ -356,12 +354,12 @@ bool LMAKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_nam
 
 const std::vector<std::string>& LMAKinematicsPlugin::getJointNames() const
 {
-  return solver_info_.joint_names;
+  return joint_model_group_->getJointModelNames();
 }
 
 const std::vector<std::string>& LMAKinematicsPlugin::getLinkNames() const
 {
-  return solver_info_.link_names;
+  return joint_model_group_->getLinkModelNames();
 }
 
 }  // namespace