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.cpp
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2020/03/27
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #include <cartesian_controller_base/SelectivelyDampedLeastSquaresSolver.h>
 41  
 42  #include <memory>
 43  #include <pluginlib/class_list_macros.hpp>
 44  
 45  /**
 46   * \class cartesian_controller_base::SelectivelyDampedLeastSquaresSolver
 47   *
 48   * Users may explicitly specify this solver with \a "selectively_damped_least_squares" as \a
 49   * ik_solver in their controllers.yaml configuration file for each controller:
 50   *
 51   * \code{.yaml}
 52   * <name_of_your_controller>:
 53   *   ros__parameters:
 54   *     ik_solver: "selectively_damped_least_squares"
 55   *     ...
 56   * \endcode
 57   *
 58   */
 59  PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::SelectivelyDampedLeastSquaresSolver,
 60                         cartesian_controller_base::IKSolver)
 61  
 62  namespace cartesian_controller_base
 63  {
 64  SelectivelyDampedLeastSquaresSolver::SelectivelyDampedLeastSquaresSolver() {}
 65  
 66  SelectivelyDampedLeastSquaresSolver::~SelectivelyDampedLeastSquaresSolver() {}
 67  
 68  trajectory_msgs::msg::JointTrajectoryPoint SelectivelyDampedLeastSquaresSolver::getJointControlCmds(
 69    rclcpp::Duration period, const ctrl::Vector6D & net_force)
 70  {
 71    // Compute joint Jacobian
 72    m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian);
 73  
 74    Eigen::JacobiSVD<Eigen::Matrix<double, 6, Eigen::Dynamic> > JSVD(
 75      m_jnt_jacobian.data, Eigen::ComputeFullU | Eigen::ComputeFullV);
 76    Eigen::Matrix<double, 6, 6> U = JSVD.matrixU();
 77    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> V = JSVD.matrixV();
 78    Eigen::Matrix<double, Eigen::Dynamic, 1> s = JSVD.singularValues();
 79  
 80    // Default recommendation by Buss and Kim.
 81    const double gamma_max = 3.141592653 / 4;
 82  
 83    Eigen::Matrix<double, Eigen::Dynamic, 1> sum_phi =
 84      Eigen::Matrix<double, Eigen::Dynamic, 1>::Zero(m_number_joints);
 85  
 86    // Compute each joint velocity with the SDLS method.  This implements the
 87    // algorithm as described in the paper (but for only one end-effector).
 88    // Also see Buss' own implementation:
 89    // https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/index.html
 90  
 91    for (int i = 0; i < m_number_joints; ++i)
 92    {
 93      double alpha = U.col(i).transpose() * net_force;
 94  
 95      double N = U.col(i).head(3).norm();
 96      double M = 0;
 97      for (int j = 0; j < m_number_joints; ++j)
 98      {
 99        double rho = m_jnt_jacobian.data.col(j).head(3).norm();
100        M += std::abs(V.col(i)[j]) * rho;
101      }
102      M *= 1.0 / s[i];
103  
104      double gamma = std::min(1.0, N / M) * gamma_max;
105  
106      Eigen::Matrix<double, Eigen::Dynamic, 1> phi =
107        clampMaxAbs(1.0 / s[i] * alpha * V.col(i), gamma);
108      sum_phi += phi;
109    }
110  
111    m_current_velocities.data = clampMaxAbs(sum_phi, gamma_max);
112  
113    // Integrate once, starting with zero motion
114    m_current_positions.data =
115      m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds();
116  
117    // Make sure positions stay in allowed margins
118    applyJointLimits();
119  
120    // Apply results
121    trajectory_msgs::msg::JointTrajectoryPoint control_cmd;
122    for (int i = 0; i < m_number_joints; ++i)
123    {
124      control_cmd.positions.push_back(m_current_positions(i));
125      control_cmd.velocities.push_back(m_current_velocities(i));
126  
127      // Accelerations should be left empty. Those values will be interpreted
128      // by most hardware joint drivers as max. tolerated values. As a
129      // consequence, the robot will move very slowly.
130    }
131    control_cmd.time_from_start = period;  // valid for this duration
132  
133    // Update for the next cycle
134    m_last_positions = m_current_positions;
135  
136    return control_cmd;
137  }
138  
139  bool SelectivelyDampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
140                                                 const KDL::Chain & chain,
141                                                 const KDL::JntArray & upper_pos_limits,
142                                                 const KDL::JntArray & lower_pos_limits)
143  {
144    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
145  
146    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
147    m_jnt_jacobian.resize(m_number_joints);
148  
149    return true;
150  }
151  
152  Eigen::Matrix<double, Eigen::Dynamic, 1> SelectivelyDampedLeastSquaresSolver::clampMaxAbs(
153    const Eigen::Matrix<double, Eigen::Dynamic, 1> & w, double d)
154  {
155    if (w.cwiseAbs().maxCoeff() <= d)
156    {
157      return w;
158    }
159    else
160    {
161      return d * w / w.cwiseAbs().maxCoeff();
162    }
163  }
164  
165  }  // namespace cartesian_controller_base