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    cartesian_controller_base.h
 33   *
 34   * \author  Stefan Scherzinger <scherzin@fzi.de>
 35   * \date    2017/07/27
 36   *
 37   */
 38  //-----------------------------------------------------------------------------
 39  
 40  #ifndef CARTESIAN_CONTROLLER_BASE_H_INCLUDED
 41  #define CARTESIAN_CONTROLLER_BASE_H_INCLUDED
 42  
 43  #include <cartesian_controller_base/IKSolver.h>
 44  #include <cartesian_controller_base/SpatialPDController.h>
 45  #include <cartesian_controller_base/Utility.h>
 46  #include <realtime_tools/realtime_publisher.h>
 47  
 48  #include <controller_interface/controller_interface.hpp>
 49  #include <functional>
 50  #include <geometry_msgs/msg/pose_stamped.hpp>
 51  #include <geometry_msgs/msg/twist_stamped.hpp>
 52  #include <geometry_msgs/msg/wrench_stamped.hpp>
 53  #include <hardware_interface/loaned_command_interface.hpp>
 54  #include <hardware_interface/loaned_state_interface.hpp>
 55  #include <kdl/treefksolverpos_recursive.hpp>
 56  #include <memory>
 57  #include <pluginlib/class_loader.hpp>
 58  #include <rclcpp/rclcpp.hpp>
 59  #include <string>
 60  #include <trajectory_msgs/msg/joint_trajectory_point.hpp>
 61  #include <vector>
 62  
 63  #include "ROS2VersionConfig.h"
 64  #include "rclcpp_lifecycle/lifecycle_node.hpp"
 65  
 66  namespace cartesian_controller_base
 67  {
 68  /**
 69   * @brief Base class for each cartesian controller
 70   *
 71   * This class implements a common forward dynamics based solver for Cartesian
 72   * end effector error correction. Different child class controllers will define
 73   * what this error represents and should call \ref computeJointControlCmds with
 74   * that error.  The control commands are sent to the hardware with \ref
 75   * writeJointControlCmds.
 76   *
 77   */
 78  class CartesianControllerBase : public controller_interface::ControllerInterface
 79  {
 80  public:
 81    CartesianControllerBase();
 82    virtual ~CartesianControllerBase(){};
 83  
 84    virtual controller_interface::InterfaceConfiguration command_interface_configuration()
 85      const override;
 86  
 87    virtual controller_interface::InterfaceConfiguration state_interface_configuration()
 88      const override;
 89  
 90    virtual LifecycleNodeInterface::CallbackReturn on_init() override;
 91  
 92    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
 93      const rclcpp_lifecycle::State & previous_state) override;
 94  
 95    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
 96      const rclcpp_lifecycle::State & previous_state) override;
 97  
 98    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
 99      const rclcpp_lifecycle::State & previous_state) override;
100  
101    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
102      const rclcpp_lifecycle::State & previous_state) override;
103  
104  protected:
105    /**
106       * @brief Write joint control commands to the real hardware
107       *
108       * Depending on the hardware interface used, this is either joint positions
109       * or velocities.
110       */
111    void writeJointControlCmds();
112  
113    /**
114       * @brief Compute one control step using forward dynamics simulation
115       *
116       * Check \ref ForwardDynamicsSolver for details.
117       *
118       * @param error The error to minimize
119       * @param period The period for this control cycle
120       */
121    void computeJointControlCmds(const ctrl::Vector6D & error, const rclcpp::Duration & period);
122  
123    /**
124       * @brief Display the given vector in the given robot base link
125       *
126       * @param vector The quantity to transform
127       * @param from The reference frame where the quantity was formulated
128       *
129       * @return The quantity in the robot base frame
130       */
131    ctrl::Vector6D displayInBaseLink(const ctrl::Vector6D & vector, const std::string & from);
132  
133    /**
134       * @brief Display the given tensor in the robot base frame
135       *
136       * @param tensor The quantity to transform
137       * @param from The reference frame where the quantity was formulated
138       *
139       * @return The quantity in the robot base frame
140       */
141    ctrl::Matrix6D displayInBaseLink(const ctrl::Matrix6D & tensor, const std::string & from);
142  
143    /**
144       * @brief Display a given vector in a new reference frame
145       *
146       * The vector is assumed to be given in the robot base frame.
147       *
148       * @param vector The quantity to transform
149       * @param to The reference frame in which to formulate the quantity
150       *
151       * @return The quantity in the new frame
152       */
153    ctrl::Vector6D displayInTipLink(const ctrl::Vector6D & vector, const std::string & to);
154  
155    /**
156       * @brief Check if specified links are part of the robot chain
157       *
158       * @param s Link to check for existence
159       *
160       * @return True if existent, false otherwise
161       */
162    bool robotChainContains(const std::string & s)
163    {
164      for (const auto & segment : this->m_robot_chain.segments)
165      {
166        if (segment.getName() == s)
167        {
168          return true;
169        }
170      }
171      return false;
172    }
173  
174    /**
175     * @brief Helper method to check the controller's state during input callbacks
176     *
177     * @return True if the controller is active, false otherwise
178     */
179    bool isActive() const { return m_active; };
180  
181    KDL::Chain m_robot_chain;
182  
183    std::shared_ptr<KDL::TreeFkSolverPos_recursive> m_forward_kinematics_solver;
184  
185    /**
186       * @brief Allow users to choose the IK solver type on startup
187       */
188    std::shared_ptr<pluginlib::ClassLoader<IKSolver>> m_solver_loader;
189    std::shared_ptr<IKSolver> m_ik_solver;
190  
191    // Dynamic parameters
192    std::string m_end_effector_link;
193    std::string m_robot_base_link;
194    int m_iterations;
195  
196    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
197      m_joint_state_pos_handles;
198  
199  protected:
200    /**
201       * @brief Stop joint motion when in velocity control
202       */
203    void stopCurrentMotion()
204    {
205      for (size_t i = 0; i < m_joint_cmd_vel_handles.size(); ++i)
206      {
207        m_joint_cmd_vel_handles[i].get().set_value(0.0);
208      }
209    }
210  
211    /**
212       * @brief Publish the controller's end-effector pose and twist
213       *
214       * The data are w.r.t. the specified robot base link.
215       * If this function is called after `computeJointControlCmds()` has
216       * been called, then the controller's internal state represents the state
217       * right after the error computation, and corresponds to the new target
218       * state that will be send to the actuators in this control cycle.
219       */
220    void publishStateFeedback();
221  
222    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::msg::PoseStamped>
223      m_feedback_pose_publisher;
224    realtime_tools::RealtimePublisherSharedPtr<geometry_msgs::msg::TwistStamped>
225      m_feedback_twist_publisher;
226  
227    std::vector<std::string> m_cmd_interface_types;
228    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
229      m_joint_cmd_pos_handles;
230    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
231      m_joint_cmd_vel_handles;
232  
233    std::vector<std::string> m_joint_names;
234    trajectory_msgs::msg::JointTrajectoryPoint m_simulated_joint_motion;
235    SpatialPDController m_spatial_controller;
236    ctrl::Vector6D m_cartesian_input;
237  
238    // Against multi initialization in multi inheritance scenarios
239    bool m_initialized = {false};
240    bool m_configured = {false};
241    bool m_active = {false};
242  
243    // Dynamic parameters
244    double m_error_scale;
245    std::string m_robot_description;
246  };
247  
248  }  // namespace cartesian_controller_base
249  
250  #endif