Program Listing for File simple_transmission.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_control/transmission_interface/include/transmission_interface/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
{

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};

  ActuatorHandle actuator_position_ = {"", "", nullptr};
  ActuatorHandle actuator_velocity_ = {"", "", nullptr};
  ActuatorHandle actuator_effort_ = {"", "", 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);

  if (!joint_position_ && !joint_velocity_ && !joint_effort_)
  {
    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);

  if (!actuator_position_ && !actuator_velocity_ && !actuator_effort_)
  {
    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_);
  }
}

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_);
  }
}

}  // namespace transmission_interface

#endif  // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_