hardware_interface_adapter.hpp

// Copyright 2014, SRI International
// Copyright 2013, PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
 
#ifndef GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
#define GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
 
#include <algorithm>
#include <cassert>
#include <memory>
#include <string>
#include <vector>
 
#include "control_toolbox/pid.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
template <const char * HardwareInterface>
class HardwareInterfaceAdapter
{
public:
  bool init(
    std::optional<
      std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* joint_handle */,
    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & /* node */)
  {
    return false;
  }
 
  void starting(const rclcpp::Time & /* time */) {}
  void stopping(const rclcpp::Time & /* time */) {}
 
  double updateCommand(
    double /* desired_position */, double /* desired_velocity */, double /* error_position */,
    double /* error_velocity */, double /* max_allowed_effort */)
  {
    return 0.0;
  }
};
 
template <>
class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
{
public:
  bool init(
    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
    const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
  {
    joint_handle_ = joint_handle;
    return true;
  }
 
  void starting(const rclcpp::Time & /* time */) {}
  void stopping(const rclcpp::Time & /* time */) {}
 
  double updateCommand(
    double desired_position, double /* desired_velocity */, double /* error_position */,
    double /* error_velocity */, double max_allowed_effort)
  {
    // Forward desired position to command
    joint_handle_->get().set_value(desired_position);
    return max_allowed_effort;
  }
 
private:
  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
};
 
template <>
class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
{
public:
  template <typename ParameterT>
  auto auto_declare(
    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const std::string & name,
    const ParameterT & default_value)
  {
    if (!node->has_parameter(name))
    {
      return node->declare_parameter<ParameterT>(name, default_value);
    }
    else
    {
      return node->get_parameter(name).get_value<ParameterT>();
    }
  }
 
  bool init(
    std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
    const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
  {
    joint_handle_ = joint_handle;
    // Init PID gains from ROS parameter server
    const std::string prefix = "gains." + joint_handle_->get().get_prefix_name();
    const auto k_p = auto_declare<double>(node, prefix + ".p", 0.0);
    const auto k_i = auto_declare<double>(node, prefix + ".i", 0.0);
    const auto k_d = auto_declare<double>(node, prefix + ".d", 0.0);
    const auto i_clamp = auto_declare<double>(node, prefix + ".i_clamp", 0.0);
    // Initialize PID
    pid_ = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
    return true;
  }
 
  void starting(const rclcpp::Time & /* time */)
  {
    if (!joint_handle_)
    {
      return;
    }
    // Reset PIDs, zero effort commands
    pid_->reset();
    joint_handle_->get().set_value(0.0);
  }
 
  void stopping(const rclcpp::Time & /* time */) {}
 
  double updateCommand(
    double /* desired_position */, double /* desired_velocity */, double error_position,
    double error_velocity, double max_allowed_effort)
  {
    // Preconditions
    if (!joint_handle_)
    {
      return 0.0;
    }
    // Time since the last call to update
    const auto period = std::chrono::steady_clock::now() - last_update_time_;
    // Update PIDs
    double command =
      pid_->computeCommand(error_position, error_velocity, static_cast<uint64_t>(period.count()));
    command = std::min<double>(
      fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
    joint_handle_->get().set_value(command);
    last_update_time_ = std::chrono::steady_clock::now();
    return command;
  }
 
private:
  using PidPtr = std::shared_ptr<control_toolbox::Pid>;
  PidPtr pid_;
  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
  std::chrono::steady_clock::time_point last_update_time_;
};
 
#endif  // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_