simple_transmission.hpp

// Copyright 2020 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 TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
 
#include <algorithm>
#include <cassert>
#include <string>
#include <vector>
 
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"
 
namespace transmission_interface
{
 
constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position";
 
class SimpleTransmission : public Transmission
{
public:
  explicit SimpleTransmission(
    const double joint_to_actuator_reduction, const double joint_offset = 0.0);
 
 
  void configure(
    const std::vector<JointHandle> & joint_handles,
    const std::vector<ActuatorHandle> & actuator_handles) override;
 
 
  void actuator_to_joint() override;
 
 
  void joint_to_actuator() override;
 
  std::size_t num_actuators() const override { return 1; }
  std::size_t num_joints() const override { return 1; }
 
  double get_actuator_reduction() const { return reduction_; }
  double get_joint_offset() const { return jnt_offset_; }
 
protected:
  double reduction_;
  double jnt_offset_;
 
  JointHandle joint_position_ = {"", "", nullptr};
  JointHandle joint_velocity_ = {"", "", nullptr};
  JointHandle joint_effort_ = {"", "", nullptr};
  JointHandle joint_torque_ = {"", "", nullptr};
  JointHandle joint_absolute_position_ = {"", "", nullptr};
 
  ActuatorHandle actuator_position_ = {"", "", nullptr};
  ActuatorHandle actuator_velocity_ = {"", "", nullptr};
  ActuatorHandle actuator_effort_ = {"", "", nullptr};
  ActuatorHandle actuator_torque_ = {"", "", nullptr};
  ActuatorHandle actuator_absolute_position_ = {"", "", nullptr};
};
 
inline SimpleTransmission::SimpleTransmission(
  const double joint_to_actuator_reduction, const double joint_offset)
: reduction_(joint_to_actuator_reduction), jnt_offset_(joint_offset)
{
  if (reduction_ == 0.0)
  {
    throw Exception("Transmission reduction ratio cannot be zero.");
  }
}
 
template <class HandleType>
HandleType get_by_interface(
  const std::vector<HandleType> & handles, const std::string & interface_name)
{
  const auto result = std::find_if(
    handles.cbegin(), handles.cend(),
    [&interface_name](const auto handle) { return handle.get_interface_name() == interface_name; });
  if (result == handles.cend())
  {
    return HandleType(handles.cbegin()->get_prefix_name(), interface_name, nullptr);
  }
  return *result;
}
 
template <class T>
bool are_names_identical(const std::vector<T> & handles)
{
  std::vector<std::string> names;
  std::transform(
    handles.cbegin(), handles.cend(), std::back_inserter(names),
    [](const auto & handle) { return handle.get_prefix_name(); });
  return std::equal(names.cbegin() + 1, names.cend(), names.cbegin());
}
 
inline void SimpleTransmission::configure(
  const std::vector<JointHandle> & joint_handles,
  const std::vector<ActuatorHandle> & actuator_handles)
{
  if (joint_handles.empty())
  {
    throw Exception("No joint handles were passed in");
  }
 
  if (actuator_handles.empty())
  {
    throw Exception("No actuator handles were passed in");
  }
 
  if (!are_names_identical(joint_handles))
  {
    throw Exception("Joint names given to transmissions should be identical");
  }
 
  if (!are_names_identical(actuator_handles))
  {
    throw Exception("Actuator names given to transmissions should be identical");
  }
 
  joint_position_ = get_by_interface(joint_handles, hardware_interface::HW_IF_POSITION);
  joint_velocity_ = get_by_interface(joint_handles, hardware_interface::HW_IF_VELOCITY);
  joint_effort_ = get_by_interface(joint_handles, hardware_interface::HW_IF_EFFORT);
  joint_torque_ = get_by_interface(joint_handles, hardware_interface::HW_IF_TORQUE);
  joint_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION);
 
  if (
    !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ &&
    !joint_absolute_position_)
  {
    throw Exception("None of the provided joint handles are valid or from the required interfaces");
  }
 
  actuator_position_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_POSITION);
  actuator_velocity_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_VELOCITY);
  actuator_effort_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_EFFORT);
  actuator_torque_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_TORQUE);
  actuator_absolute_position_ = get_by_interface(actuator_handles, HW_IF_ABSOLUTE_POSITION);
 
  if (
    !actuator_position_ && !actuator_velocity_ && !actuator_effort_ && !actuator_torque_ &&
    !actuator_absolute_position_)
  {
    throw Exception("None of the provided joint handles are valid or from the required interfaces");
  }
}
 
inline void SimpleTransmission::actuator_to_joint()
{
  if (joint_effort_ && actuator_effort_)
  {
    joint_effort_.set_value(actuator_effort_.get_value() * reduction_);
  }
 
  if (joint_velocity_ && actuator_velocity_)
  {
    joint_velocity_.set_value(actuator_velocity_.get_value() / reduction_);
  }
 
  if (joint_position_ && actuator_position_)
  {
    joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_);
  }
 
  if (joint_torque_ && actuator_torque_)
  {
    joint_torque_.set_value(actuator_torque_.get_value() * reduction_);
  }
 
  if (joint_absolute_position_ && actuator_absolute_position_)
  {
    joint_absolute_position_.set_value(
      actuator_absolute_position_.get_value() / reduction_ + jnt_offset_);
  }
}
 
inline void SimpleTransmission::joint_to_actuator()
{
  if (joint_effort_ && actuator_effort_)
  {
    actuator_effort_.set_value(joint_effort_.get_value() / reduction_);
  }
 
  if (joint_velocity_ && actuator_velocity_)
  {
    actuator_velocity_.set_value(joint_velocity_.get_value() * reduction_);
  }
 
  if (joint_position_ && actuator_position_)
  {
    actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_);
  }
 
  if (joint_torque_ && actuator_torque_)
  {
    actuator_torque_.set_value(joint_torque_.get_value() / reduction_);
  }
}
 
}  // namespace transmission_interface
 
#endif  // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_