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    JacobianTransposeSolver.cpp
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2020/03/26
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #include <cartesian_controller_base/JacobianTransposeSolver.h>
 41  
 42  #include <pluginlib/class_list_macros.hpp>
 43  
 44  /**
 45   * \class cartesian_controller_base::JacobianTransposeSolver
 46   *
 47   * Users may explicitly specify this solver with \a "jacobian_transpose" 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: "jacobian_transpose"
 54   *     ...
 55   * \endcode
 56   *
 57   */
 58  PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::JacobianTransposeSolver,
 59                         cartesian_controller_base::IKSolver)
 60  
 61  namespace cartesian_controller_base
 62  {
 63  JacobianTransposeSolver::JacobianTransposeSolver() {}
 64  
 65  JacobianTransposeSolver::~JacobianTransposeSolver() {}
 66  
 67  trajectory_msgs::msg::JointTrajectoryPoint JacobianTransposeSolver::getJointControlCmds(
 68    rclcpp::Duration period, const ctrl::Vector6D & net_force)
 69  {
 70    // Compute joint jacobian
 71    m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian);
 72  
 73    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
 74    m_current_accelerations.data = m_jnt_jacobian.data.transpose() * net_force;
 75  
 76    // Integrate once, starting with zero motion
 77    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.seconds();
 78  
 79    // Integrate twice, starting with zero motion
 80    m_current_positions.data =
 81      m_last_positions.data + 0.5 * m_current_velocities.data * period.seconds();
 82  
 83    // Make sure positions stay in allowed margins
 84    applyJointLimits();
 85  
 86    // Apply results
 87    trajectory_msgs::msg::JointTrajectoryPoint control_cmd;
 88    for (int i = 0; i < m_number_joints; ++i)
 89    {
 90      control_cmd.positions.push_back(m_current_positions(i));
 91      control_cmd.velocities.push_back(m_current_velocities(i));
 92  
 93      // Accelerations should be left empty. Those values will be interpreted
 94      // by most hardware joint drivers as max. tolerated values. As a
 95      // consequence, the robot will move very slowly.
 96    }
 97    control_cmd.time_from_start = period;  // valid for this duration
 98  
 99    // Update for the next cycle
100    m_last_positions = m_current_positions;
101  
102    return control_cmd;
103  }
104  
105  bool JacobianTransposeSolver::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh,
106                                     const KDL::Chain & chain, const KDL::JntArray & upper_pos_limits,
107                                     const KDL::JntArray & lower_pos_limits)
108  {
109    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);
110  
111    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
112    m_jnt_jacobian.resize(m_number_joints);
113  
114    return true;
115  }
116  }  // namespace cartesian_controller_base