KDL IK solver improvements

moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/chainiksolver_vel_mimic_svd.hpp

@@ -57,11 +57,11 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel
    * @param num_mimic_joints The number of joints that are setup to follow other joints
    * @param position_ik false if you want to solve for the full 6 dof end-effector pose,
    *        true if you want to solve only for the 3 dof end-effector position.
-   * @param threshold if a singular value is below this value, its inverse is set to zero, default: 0.00001
+   * @param threshold if a singular value is below this value, its inverse is set to zero, default: 0.001
    */
   explicit ChainIkSolverVelMimicSVD(const Chain& chain_,
                                     const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
-                                    bool position_ik = false, double threshold = 0.00001);
+                                    bool position_ik = false, double threshold = 0.001);
 
 // TODO: simplify after kinetic support is dropped
 #define KDL_VERSION_LESS(a, b, c) (KDL_VERSION < ((a << 16) | (b << 8) | c))
@@ -74,14 +74,31 @@ class ChainIkSolverVelMimicSVD : public ChainIkSolverVel
 
   ~ChainIkSolverVelMimicSVD() override;
 
-  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override;
+  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override
+  {
+    return CartToJnt(q_in, v_in, qdot_out, Eigen::VectorXd::Constant(svd_.cols(), 1.0),
+                     Eigen::Matrix<double, 6, 1>::Constant(1.0));
+  }
+
+  /** Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in
+   *
+   * where W_q and W_x are joint- and Cartesian weights respectively.
+   * A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution. */
+  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights,
+                const Eigen::Matrix<double, 6, 1>& cartesian_weights);
 
   /// not implemented.
   int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override
   {
     return -1;
   }
 
+  /// Return true iff we ignore orientation but only consider position for inverse kinematics
+  bool isPositionOnly() const
+  {
+    return svd_.rows() == 3;
+  }
+
 private:
   bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced);
 

moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/joint_mimic.hpp

@@ -49,7 +49,7 @@ class JointMimic
   JointMimic()
   {
     this->reset(0);
-  };
+  }
 
   /** \brief Offset for this joint value from the joint that it mimics */
   double offset;
@@ -68,7 +68,7 @@ class JointMimic
     multiplier = 1.0;
     map_index = index;
     active = false;
-  };
+  }
 };
 }
 

moveit_kinematics/kdl_kinematics_plugin/include/moveit/kdl_kinematics_plugin/kdl_kinematics_plugin.h

@@ -59,6 +59,11 @@
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
 
+namespace KDL
+{
+class ChainIkSolverVelMimicSVD;
+}
+
 namespace kdl_kinematics_plugin
 {
 /**
@@ -135,11 +140,15 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
                         moveit_msgs::MoveItErrorCodes& error_code, const std::vector<double>& consistency_limits,
                         const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
 
+  typedef Eigen::Matrix<double, 6, 1> Twist;
+
   /// Solve position IK given initial joint values
-  int CartToJnt(KDL::ChainIkSolverVel& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in,
-                KDL::JntArray& q_out, const unsigned int max_iter) const;
+  int CartToJnt(KDL::ChainIkSolverVelMimicSVD& ik_solver, const KDL::JntArray& q_init, const KDL::Frame& p_in,
+                KDL::JntArray& q_out, const unsigned int max_iter, const Eigen::VectorXd& joint_weights,
+                const Twist& cartesian_weights) const;
 
 private:
+  void getJointWeights();
   bool timedOut(const ros::WallTime& start_time, double duration) const;
 
   /** @brief Check whether the solution lies within the consistency limits of the seed state
@@ -161,6 +170,9 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
   void getRandomConfiguration(const Eigen::VectorXd& seed_state, const std::vector<double>& consistency_limits,
                               Eigen::VectorXd& jnt_array) const;
 
+  /// clip q_delta such that joint limits will not be violated
+  void clipToJointLimits(const KDL::JntArray& q, KDL::JntArray& q_delta, Eigen::ArrayXd& weighting) const;
+
   bool initialized_;  ///< Internal variable that indicates whether solver is configured and ready
 
   unsigned int dimension_;                        ///< Dimension of the group
@@ -171,11 +183,17 @@ class KDLKinematicsPlugin : public kinematics::KinematicsBase
   KDL::Chain kdl_chain_;
   std::unique_ptr<KDL::ChainFkSolverPos> fk_solver_;
   std::vector<JointMimic> mimic_joints_;
+  std::vector<double> joint_weights_;
   Eigen::VectorXd joint_min_, joint_max_;  ///< joint limits
 
-  bool position_ik_;  ///< whether this solver is only being used for position ik
   int max_solver_iterations_;
   double epsilon_;
+  /** weight of orientation error vs position error
+   *
+   * < 1.0: orientation has less importance than position
+   * > 1.0: orientation has more importance than position
+   * = 0.0: perform position-only IK */
+  double orientation_vs_position_weight_;
 };
 }
 

moveit_kinematics/kdl_kinematics_plugin/src/chainiksolver_vel_mimic_svd.cpp

@@ -87,7 +87,9 @@ bool ChainIkSolverVelMimicSVD::jacToJacReduced(const Jacobian& jac, Jacobian& ja
   return true;
 }
 
-int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
+int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out,
+                                        const Eigen::VectorXd& joint_weights,
+                                        const Eigen::Matrix<double, 6, 1>& cartesian_weights)
 {
   // Let the ChainJntToJacSolver calculate the Jacobian for the current joint positions q_in.
   if (num_mimic_joints_ > 0)
@@ -99,24 +101,32 @@ int ChainIkSolverVelMimicSVD::CartToJnt(const JntArray& q_in, const Twist& v_in,
   else
     jnt2jac_.JntToJac(q_in, jac_reduced_);
 
+  // weight Jacobian
+  auto& J = jac_reduced_.data;
+  const Eigen::Index rows = svd_.rows();  // only operate on position rows?
+  J.topRows(rows) *= joint_weights.asDiagonal();
+  J.topRows(rows).transpose() *= cartesian_weights.topRows(rows).asDiagonal();
+
   // transform v_in to 6D Eigen::Vector
   Eigen::Matrix<double, 6, 1> vin;
-  vin.topRows<3>() = Eigen::Map<const Eigen::Vector3d>(v_in.vel.data, 3);
-  vin.bottomRows<3>() = Eigen::Map<const Eigen::Vector3d>(v_in.rot.data, 3);
-
-  const Eigen::MatrixXd& J = jac_reduced_.data;
+  vin.topRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.vel.data, 3) * cartesian_weights.topRows<3>().array();
+  vin.bottomRows<3>() = Eigen::Map<const Eigen::Array3d>(v_in.rot.data, 3) * cartesian_weights.bottomRows<3>().array();
 
   // Do a singular value decomposition: J = U*S*V^t
-  svd_.compute(J.topRows(svd_.rows()));
+  svd_.compute(J.topRows(rows));
 
   if (num_mimic_joints_ > 0)
   {
-    qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(svd_.rows()));
+    qdot_out_reduced_.noalias() = svd_.solve(vin.topRows(rows));
+    qdot_out_reduced_.array() *= joint_weights.array();
     for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i)
       qdot_out(i) = qdot_out_reduced_[mimic_joints_[i].map_index] * mimic_joints_[i].multiplier;
   }
   else
-    qdot_out.data.noalias() = svd_.solve(vin.topRows(svd_.rows()));
+  {
+    qdot_out.data.noalias() = svd_.solve(vin.topRows(rows));
+    qdot_out.data.array() *= joint_weights.array();
+  }
 
   return 0;
 }