1  ////////////////////////////////////////////////////////////////////////////////
  2  // Copyright 2019 FZI Research Center for Information Technology
  3  //
  4  // Redistribution and use in source and binary forms, with or without
  5  // modification, are permitted provided that the following conditions are met:
  6  //
  7  // 1. Redistributions of source code must retain the above copyright notice,
  8  // this list of conditions and the following disclaimer.
  9  //
 10  // 2. Redistributions in binary form must reproduce the above copyright notice,
 11  // this list of conditions and the following disclaimer in the documentation
 12  // and/or other materials provided with the distribution.
 13  //
 14  // 3. Neither the name of the copyright holder nor the names of its
 15  // contributors may be used to endorse or promote products derived from this
 16  // software without specific prior written permission.
 17  //
 18  // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19  // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20  // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21  // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 22  // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23  // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24  // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25  // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26  // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27  // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28  // POSSIBILITY OF SUCH DAMAGE.
 29  ////////////////////////////////////////////////////////////////////////////////
 30  
 31  //-----------------------------------------------------------------------------
 32  /*!\file    SelectivelyDampedLeastSquaresSolver.h
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2020/06/21
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #ifndef SELECTIVELY_DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED
 41  #define SELECTIVELY_DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED
 42  
 43  #include <cartesian_controller_base/IKSolver.h>
 44  
 45  #include <kdl/jacobian.hpp>
 46  #include <memory>
 47  
 48  namespace cartesian_controller_base
 49  {
 50  /**
 51     * \brief A selectively damped least squares (SDLS) IK solver for Cartesian controllers
 52     *
 53     *  This implements the SDLS method by Buss and Kim from 2005.
 54     *
 55     *  The implementation is according to their paper:
 56     *  https://www.tandfonline.com/doi/abs/10.1080/2151237X.2005.10129202
 57     *
 58     *  which is available here:
 59     *  https://www.researchgate.net/profile/Samuel_Buss/publication/220494116_Selectively_Damped_Least_Squares_for_Inverse_Kinematics/links/09e4150cc04794d9d0000000/Selectively-Damped-Least-Squares-for-Inverse-Kinematics.pdf
 60     *
 61     *  It has the advantage over the DLS method in that it converges faster and
 62     *  does not require ad-hoc damping terms, i.e. users do not need to specify
 63     *  task dependent damping values, which can otherwise require numerous
 64     *  trials and expertise.  It is, however, more computationally evolved.
 65     *
 66     */
 67  class SelectivelyDampedLeastSquaresSolver : public IKSolver
 68  {
 69  public:
 70    SelectivelyDampedLeastSquaresSolver();
 71    ~SelectivelyDampedLeastSquaresSolver();
 72  
 73    /**
 74       * \brief Compute joint target commands with selectively damped least squares
 75       *
 76       * \param period The duration in sec for this simulation step
 77       * \param net_force The applied net force, expressed in the root frame
 78       *
 79       * \return A point holding positions, velocities and accelerations of each joint
 80       */
 81    trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds(
 82      rclcpp::Duration period, const ctrl::Vector6D & net_force) override;
 83  
 84    /**
 85       * \brief Initialize the solver
 86       *
 87       * \param nh A node handle for namespace-local parameter management
 88       * \param chain The kinematic chain of the robot
 89       * \param upper_pos_limits Tuple with max positive joint angles
 90       * \param lower_pos_limits Tuple with max negative joint angles
 91       *
 92       * \return True, if everything went well
 93       */
 94  
 95    bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
 96              const KDL::JntArray & upper_pos_limits,
 97              const KDL::JntArray & lower_pos_limits) override;
 98  
 99  private:
100    /**
101       * @brief Helper function to clamp a column vector
102       *
103       * This literally implements ClampMaxAbs() from Buss' and Kim's paper.
104       *
105       * @param w The vector to clamp
106       * @param d The threshold for the max allowed value
107       *
108       * @return The clamped vector
109       */
110    Eigen::Matrix<double, Eigen::Dynamic, 1> clampMaxAbs(
111      const Eigen::Matrix<double, Eigen::Dynamic, 1> & w, double d);
112  
113    std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
114    KDL::Jacobian m_jnt_jacobian;
115  };
116  
117  }  // namespace cartesian_controller_base
118  
119  #endif