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    ForwardDynamicsSolver.h
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2016/02/14
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #ifndef FORWARD_DYNAMICS_SOLVER_H_INCLUDED
 41  #define FORWARD_DYNAMICS_SOLVER_H_INCLUDED
 42  
 43  #include <cartesian_controller_base/IKSolver.h>
 44  #include <cartesian_controller_base/Utility.h>
 45  
 46  #include <kdl/chain.hpp>
 47  #include <kdl/chaindynparam.hpp>
 48  #include <kdl/chainfksolverpos_recursive.hpp>
 49  #include <kdl/chainfksolvervel_recursive.hpp>
 50  #include <kdl/chainjnttojacsolver.hpp>
 51  #include <kdl/frames.hpp>
 52  #include <kdl/jacobian.hpp>
 53  #include <memory>
 54  #include <vector>
 55  
 56  #include "rclcpp/node.hpp"
 57  
 58  namespace cartesian_controller_base
 59  {
 60  /*! \brief The default IK solver for Cartesian controllers
 61   *
 62   *  This class computes manipulator joint motion from Cartesian force inputs.
 63   *  As inputs, both forces and torques are applied to the mechanical system,
 64   *  representing the manipulator. The system is modeled as a chain of rigid
 65   *  bodies. All bodies except for the last one are massless. This is to
 66   *  achieve a nearly linear behavior in all joint configurations.
 67   *  The resulting joint accelerations are computed according to
 68   *  \f$ \ddot{q} = H^{-1} ( J^T f) \f$
 69   *  Where \f$ H \f$ denotes the joint space inertia matrix of the virtually
 70   *  conditioned system, \f$ J \f$ denotes the joint Jacobian and \f$ f \f$ is
 71   *  the applied force to the end effector.  The joint accelerations are
 72   *  integrated twice to obtain joint velocities and joint positions
 73   *  respectively.
 74   *  Check more details behind the solver here: https://arxiv.org/pdf/1908.06252.pdf
 75   */
 76  class ForwardDynamicsSolver : public IKSolver
 77  {
 78  public:
 79    ForwardDynamicsSolver();
 80    ~ForwardDynamicsSolver();
 81  
 82    /**
 83       * @brief Compute joint target commands with approximate forward dynamics
 84       *
 85       * The resulting motion is the output of a forward dynamics simulation. It
 86       * can be forwarded to a real controller to mimic the simulated behavior.
 87       *
 88       * @param period The duration in sec for this simulation step
 89       * @param net_force The applied net force, expressed in the root frame
 90       *
 91       * @return A point holding positions, velocities and accelerations of each joint
 92       */
 93    trajectory_msgs::msg::JointTrajectoryPoint getJointControlCmds(
 94      rclcpp::Duration period, const ctrl::Vector6D & net_force) override;
 95  
 96    /**
 97       * @brief Initialize the solver
 98       *
 99       * @param nh A node handle for namespace-local parameter management
100       * @param chain The kinematic chain of the robot
101       * @param upper_pos_limits Tuple with max positive joint angles
102       * @param lower_pos_limits Tuple with max negative joint angles
103       *
104       * @return True, if everything went well
105       */
106  
107    bool init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> nh, const KDL::Chain & chain,
108              const KDL::JntArray & upper_pos_limits,
109              const KDL::JntArray & lower_pos_limits) override;
110  
111  private:
112    //! Build a generic robot model for control
113    bool buildGenericModel();
114  
115    // Forward dynamics
116    std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;
117    std::shared_ptr<KDL::ChainDynParam> m_jnt_space_inertia_solver;
118    KDL::Jacobian m_jnt_jacobian;
119    KDL::JntSpaceInertiaMatrix m_jnt_space_inertia;
120  
121    // Dynamic parameters
122    const std::string m_params = "solver.forward_dynamics";  ///< namespace for parameter access
123  
124    /**
125       * Virtual link mass
126       * Virtual mass of the manipulator's links. The smaller this value, the
127       * more does the end-effector (which has a unit mass of 1.0) dominate dynamic
128       * behavior. Near singularities, a bigger value leads to smoother motion.
129       */
130    std::atomic<double> m_min = 0.1;
131  };
132  
133  }  // namespace cartesian_controller_base
134  
135  #endif