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.cpp
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2020/03/27
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #include <cartesian_controller_base/DampedLeastSquaresSolver.h>
 41  
 42  #include <pluginlib/class_list_macros.hpp>
 43  
 44  /**
 45   * \class cartesian_controller_base::DampedLeastSquaresSolver
 46   *
 47   * Users may explicitly specify this solver with \a "damped_least_squares" as \a
 48   * ik_solver in their controllers.yaml configuration file for each controller:
 49   *
 50   * \code{.yaml}
 51   * <name_of_your_controller>:
 52   *   ros__parameters:
 53   *     ik_solver: "damped_least_squares"
 54   *     ...
 55   *
 56   *     solver:
 57   *         ...
 58   *         damped_least_squares:
 59   *             alpha: 0.5
 60   * \endcode
 61   *
 62   */
 63  PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::DampedLeastSquaresSolver,
 64                         cartesian_controller_base::IKSolver)
 65  
 66  namespace cartesian_controller_base
 67  {
 68  DampedLeastSquaresSolver::DampedLeastSquaresSolver() : m_alpha(0.001) {}
 69  
 70  DampedLeastSquaresSolver::~DampedLeastSquaresSolver() {}
 71  
 72  trajectory_msgs::msg::JointTrajectoryPoint DampedLeastSquaresSolver::getJointControlCmds(
 73    rclcpp::Duration period, const ctrl::Vector6D & net_force)
 74  {
 75    // Compute joint jacobian
 76    KDL::Twist twist;
 77    twist.vel.x(net_force(0));
 78    twist.vel.y(net_force(1));
 79    twist.vel.z(net_force(2));
 80    twist.rot.x(net_force(3));
 81    twist.rot.y(net_force(4));
 82    twist.rot.z(net_force(5));
 83  
 84    // Compute joint velocities according to:
 85    // \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
 86    ctrl::MatrixND identity;
 87    identity.setIdentity(m_number_joints, m_number_joints);
 88    m_handle->get_parameter(m_params + ".alpha", m_alpha);
 89  
 90    m_current_velocities.data =
 91      (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data + m_alpha * m_alpha * identity)
 92        .inverse() *
 93      m_jnt_jacobian.data.transpose() * net_force;
 94  
 95    // Integrate once, starting with zero motion
 96    m_current_positions.data =
 97      m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds();
 98  
 99    // Make sure positions stay in allowed margins
100    applyJointLimits();
101  
102    // Apply results
103    trajectory_msgs::msg::JointTrajectoryPoint control_cmd;
104    for (int i = 0; i < m_number_joints; ++i)
105    {
106      control_cmd.positions.push_back(m_current_positions(i));
107      control_cmd.velocities.push_back(m_current_velocities(i));
108  
109      // Accelerations should be left empty. Those values will be interpreted
110      // by most hardware joint drivers as max. tolerated values. As a
111      // consequence, the robot will move very slowly.
112    }
113    control_cmd.time_from_start = period;  // valid for this duration
114  
115    // Update for the next cycle
116    m_last_positions = m_current_positions;
117  
118    return control_cmd;
119  }
120  
121  bool DampedLeastSquaresSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
122                                      const KDL::Chain & chain,
123                                      const KDL::JntArray & upper_pos_limits,
124                                      const KDL::JntArray & lower_pos_limits)
125  {
126    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
127  
128    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
129    m_jnt_jacobian.resize(m_number_joints);
130  
131    auto_declare(m_params + ".alpha", 1.0);
132  
133    return true;
134  }
135  
136  }  // namespace cartesian_controller_base