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    SpatialPDController.cpp
33   *
34   * \author  Stefan Scherzinger <scherzin@fzi.de>
35   * \date    2017/07/28
36   *
37   */
38  //-----------------------------------------------------------------------------
39  
40  #include <cartesian_controller_base/SpatialPDController.h>
41  
42  #include <string>
43  
44  namespace cartesian_controller_base
45  {
46  SpatialPDController::SpatialPDController() {}
47  
48  ctrl::Vector6D SpatialPDController::operator()(const ctrl::Vector6D & error,
49                                                 const rclcpp::Duration & period)
50  {
51    // Perform pd control separately on each Cartesian dimension
52    for (int i = 0; i < 6; ++i)  // 3 transition, 3 rotation
53    {
54      m_cmd(i) = m_pd_controllers[i](error[i], period);
55    }
56    return m_cmd;
57  }
58  
59  bool SpatialPDController::init(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> handle)
60  {
61    // Create pd controllers for each Cartesian dimension
62    for (int i = 0; i < 6; ++i)  // 3 transition, 3 rotation
63    {
64      m_pd_controllers.push_back(PDController());
65    }
66  
67    // Load default controller gains
68    std::string gains_config = "pd_gains";
69  
70    m_pd_controllers[0].init(gains_config + ".trans_x", handle);
71    m_pd_controllers[1].init(gains_config + ".trans_y", handle);
72    m_pd_controllers[2].init(gains_config + ".trans_z", handle);
73    m_pd_controllers[3].init(gains_config + ".rot_x", handle);
74    m_pd_controllers[4].init(gains_config + ".rot_y", handle);
75    m_pd_controllers[5].init(gains_config + ".rot_z", handle);
76  
77    return true;
78  }
79  
80  }  // namespace cartesian_controller_base