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    DampedLeastSquaresSolver.h
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2020/03/27
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #ifndef DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED
 41  #define DAMPED_LEAST_SQUARES_SOLVER_H_INCLUDED
 42  
 43  #include <cartesian_controller_base/IKSolver.h>
 44  
 45  #include <kdl/jacobian.hpp>
 46  #include <kdl/chainiksolvervel_wdls.hpp>
 47  #include <memory>
 48  
 49  #include "rclcpp/node.hpp"
 50  
 51  namespace cartesian_controller_base
 52  {
 53  /**
 54     * \brief A damped least squares IK solver for Cartesian controllers
 55     *
 56     *
 57     *  The resulting joint velocities are computed according to
 58     *  \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
 59     *  Where \f$ J \f$ denotes the manipulator's joint Jacobian and \f$ f \f$ is
 60     *  the applied force to the end effector.
 61     *  \f$ \alpha \f$ is a damping term.
 62     *  For controlling end effector motion, e.g. in the
 63     *  \ref cartesian_motion_controller::CartesianMotionController \f$ f \f$ should be
 64     *  thought of as an error direction vector that is mapped to wrench space
 65     *  with a unit stiffness.
 66     *
 67     *  The damped least squares formulation is according to Wampler
 68     *  https://ieeexplore.ieee.org/abstract/document/4075580
 69     */
 70  class DampedLeastSquaresSolver : public IKSolver
 71  {
 72  public:
 73    DampedLeastSquaresSolver();
 74    ~DampedLeastSquaresSolver();
 75  
 76    /**
 77       * \brief Compute joint target commands with damped least squares
 78       *
 79       * \param period The duration in sec for this simulation step
 80       * \param net_force The applied net force, expressed in the root frame
 81       *
 82       * \return A point holding positions, velocities and accelerations of each joint
 83       */
 84    trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds(
 85      rclcpp::Duration period, const ctrl::Vector6D & net_force) override;
 86  
 87    /**
 88       * \brief Initialize the solver
 89       *
 90       * \param nh A node handle for namespace-local parameter management
 91       * \param chain The kinematic chain of the robot
 92       * \param upper_pos_limits Tuple with max positive joint angles
 93       * \param lower_pos_limits Tuple with max negative joint angles
 94       *
 95       * \return True, if everything went well
 96       */
 97  
 98    bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
 99              const KDL::JntArray & upper_pos_limits,
100              const KDL::JntArray & lower_pos_limits) override;
101  
102  private:
103    std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
104    std::shared_ptr<KDL::ChainIkSolverVel_wdls> m_kdl_chainiksolver;
105    KDL::Jacobian m_jnt_jacobian;
106  
107    // Dynamic parameters
108    const std::string m_params = "solver.damped_least_squares";  ///< namespace for parameter access
109    double m_alpha;                                              ///< damping coefficient
110  };
111  
112  }  // namespace cartesian_controller_base
113  
114  #endif