differential_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__DIFFERENTIAL_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
 
#include <fmt/compile.h>
 
#include <cassert>
#include <string>
#include <vector>
 
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/accessor.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"
 
namespace transmission_interface
{
 
class DifferentialTransmission : public Transmission
{
public:
  DifferentialTransmission(
    const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
    const std::vector<double> & joint_offset = {0.0, 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 2; }
  std::size_t num_joints() const override { return 2; }
 
  const std::vector<double> & get_actuator_reduction() const { return actuator_reduction_; }
  const std::vector<double> & get_joint_reduction() const { return joint_reduction_; }
  const std::vector<double> & get_joint_offset() const { return joint_offset_; }
 
  std::string get_handles_info() const;
 
  std::vector<std::string> get_supported_actuator_interfaces() const override
  {
    return {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
            hardware_interface::HW_IF_EFFORT,   hardware_interface::HW_IF_TORQUE,
            hardware_interface::HW_IF_FORCE,    HW_IF_ABSOLUTE_POSITION};
  }
 
  std::vector<std::string> get_supported_joint_interfaces() const override
  {
    return {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
            hardware_interface::HW_IF_EFFORT,   hardware_interface::HW_IF_TORQUE,
            hardware_interface::HW_IF_FORCE,    HW_IF_ABSOLUTE_POSITION};
  }
 
protected:
  std::vector<double> actuator_reduction_;
  std::vector<double> joint_reduction_;
  std::vector<double> joint_offset_;
 
  std::vector<JointHandle> joint_position_;
  std::vector<JointHandle> joint_velocity_;
  std::vector<JointHandle> joint_effort_;
  std::vector<JointHandle> joint_torque_;
  std::vector<JointHandle> joint_force_;
  std::vector<JointHandle> joint_absolute_position_;
 
  std::vector<ActuatorHandle> actuator_position_;
  std::vector<ActuatorHandle> actuator_velocity_;
  std::vector<ActuatorHandle> actuator_effort_;
  std::vector<ActuatorHandle> actuator_torque_;
  std::vector<ActuatorHandle> actuator_force_;
  std::vector<ActuatorHandle> actuator_absolute_position_;
};
 
inline DifferentialTransmission::DifferentialTransmission(
  const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
  const std::vector<double> & joint_offset)
: actuator_reduction_(actuator_reduction),
  joint_reduction_(joint_reduction),
  joint_offset_(joint_offset)
{
  if (
    num_actuators() != actuator_reduction_.size() || num_joints() != joint_reduction_.size() ||
    num_joints() != joint_offset_.size())
  {
    throw Exception("Reduction and offset vectors must have size 2.");
  }
 
  if (
    0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
    0.0 == joint_reduction_[1])
  {
    throw Exception("Transmission reduction ratios cannot be zero.");
  }
}
 
void DifferentialTransmission::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");
  }
 
  const auto joint_names = get_names(joint_handles);
  if (joint_names.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("There should be exactly two unique joint names but was given '{}'."),
        to_string(joint_names)));
  }
  const auto actuator_names = get_names(actuator_handles);
  if (actuator_names.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("There should be exactly two unique actuator names but was given '{}'."),
        to_string(actuator_names)));
  }
 
  joint_position_ =
    get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_POSITION);
  joint_velocity_ =
    get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY);
  joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT);
  joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE);
  joint_force_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_FORCE);
  joint_absolute_position_ =
    get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION);
 
  if (!joint_position_.empty() && joint_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint position handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_velocity_.empty() && joint_velocity_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint velocity handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_effort_.empty() && joint_effort_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint effort handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_torque_.empty() && joint_torque_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint torque handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_force_.empty() && joint_force_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required joint force handles were present. \n{}"),
        get_handles_info()));
  }
  if (!joint_absolute_position_.empty() && joint_absolute_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE(
          "Not enough valid or required joint absolute position handles were present. \n{}"),
        get_handles_info()));
  }
 
  actuator_position_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_POSITION);
  actuator_velocity_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY);
  actuator_effort_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT);
  actuator_torque_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE);
  actuator_force_ =
    get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_FORCE);
  actuator_absolute_position_ =
    get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION);
 
  if (!actuator_position_.empty() && actuator_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator position handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_velocity_.empty() && actuator_velocity_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator velocity handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_effort_.empty() && actuator_effort_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator effort handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_torque_.empty() && actuator_torque_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator torque handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_force_.empty() && actuator_force_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Not enough valid or required actuator force handles were present. \n{}"),
        get_handles_info()));
  }
  if (!actuator_absolute_position_.empty() && actuator_absolute_position_.size() != 2)
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE(
          "Not enough valid or required actuator absolute position handles were "
          "present. \n{}"),
        get_handles_info()));
  }
 
  if (
    !joint_position_.empty() && !actuator_position_.empty() &&
    joint_position_.size() != actuator_position_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on position interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_velocity_.empty() && !actuator_velocity_.empty() &&
    joint_velocity_.size() != actuator_velocity_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_effort_.empty() && !actuator_effort_.empty() &&
    joint_effort_.size() != actuator_effort_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on effort interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_torque_.empty() && !actuator_torque_.empty() &&
    joint_torque_.size() != actuator_torque_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_force_.empty() && !actuator_force_.empty() &&
    joint_force_.size() != actuator_force_.size())
  {
    throw Exception(
      fmt::format(FMT_COMPILE("Pair-wise mismatch on force interfaces. \n{}"), get_handles_info()));
  }
  if (
    !joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
    joint_absolute_position_.size() != actuator_absolute_position_.size())
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"),
        get_handles_info()));
  }
 
  // Check at least one pair-wise interface is available
  if (!((!joint_position_.empty() && !actuator_position_.empty() &&
         joint_position_.size() == actuator_position_.size()) ||
        (!joint_velocity_.empty() && !actuator_velocity_.empty() &&
         joint_velocity_.size() == actuator_velocity_.size()) ||
        (!joint_effort_.empty() && !actuator_effort_.empty() &&
         joint_effort_.size() == actuator_effort_.size()) ||
        (!joint_torque_.empty() && !actuator_torque_.empty() &&
         joint_torque_.size() == actuator_torque_.size()) ||
        (!joint_force_.empty() && !actuator_force_.empty() &&
         joint_force_.size() == actuator_force_.size()) ||
        (!joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
         joint_absolute_position_.size() == actuator_absolute_position_.size())))
  {
    throw Exception(
      fmt::format(
        FMT_COMPILE("No pair-wise interface available between joints and actuators. \n{}"),
        get_handles_info()));
  }
}
 
inline void DifferentialTransmission::actuator_to_joint()
{
  const auto & ar = actuator_reduction_;
  const auto & jr = joint_reduction_;
 
  auto & act_pos = actuator_position_;
  auto & joint_pos = joint_position_;
  if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints())
  {
    assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]);
 
    joint_pos[0].set_value(
      (act_pos[0].get_value() / ar[0] + act_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
      joint_offset_[0]);
    joint_pos[1].set_value(
      (act_pos[0].get_value() / ar[0] - act_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
      joint_offset_[1]);
  }
 
  auto & act_vel = actuator_velocity_;
  auto & joint_vel = joint_velocity_;
  if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints())
  {
    assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]);
 
    joint_vel[0].set_value(
      (act_vel[0].get_value() / ar[0] + act_vel[1].get_value() / ar[1]) / (2.0 * jr[0]));
    joint_vel[1].set_value(
      (act_vel[0].get_value() / ar[0] - act_vel[1].get_value() / ar[1]) / (2.0 * jr[1]));
  }
 
  auto & act_eff = actuator_effort_;
  auto & joint_eff = joint_effort_;
  if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints())
  {
    assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);
 
    joint_eff[0].set_value(
      jr[0] * (act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1]));
    joint_eff[1].set_value(
      jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1]));
  }
 
  auto & act_tor = actuator_torque_;
  auto & joint_tor = joint_torque_;
  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
  {
    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
 
    joint_tor[0].set_value(
      jr[0] * (act_tor[0].get_value() * ar[0] + act_tor[1].get_value() * ar[1]));
    joint_tor[1].set_value(
      jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1]));
  }
 
  auto & act_for = actuator_force_;
  auto & joint_for = joint_force_;
  if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
  {
    assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);
 
    joint_for[0].set_value(
      jr[0] * (act_for[0].get_value() * ar[0] + act_for[1].get_value() * ar[1]));
    joint_for[1].set_value(
      jr[1] * (act_for[0].get_value() * ar[0] - act_for[1].get_value() * ar[1]));
  }
 
  auto & act_abs_pos = actuator_absolute_position_;
  auto & joint_abs_pos = joint_absolute_position_;
  if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints())
  {
    assert(act_abs_pos[0] && act_abs_pos[1] && joint_abs_pos[0] && joint_abs_pos[1]);
 
    joint_abs_pos[0].set_value(
      (act_abs_pos[0].get_value() / ar[0] + act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
      joint_offset_[0]);
    joint_abs_pos[1].set_value(
      (act_abs_pos[0].get_value() / ar[0] - act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
      joint_offset_[1]);
  }
}
 
inline void DifferentialTransmission::joint_to_actuator()
{
  const auto & ar = actuator_reduction_;
  const auto & jr = joint_reduction_;
 
  auto & act_pos = actuator_position_;
  auto & joint_pos = joint_position_;
  if (act_pos.size() == num_actuators() && joint_pos.size() == num_joints())
  {
    assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]);
 
    double joints_offset_applied[2] = {
      joint_pos[0].get_value() - joint_offset_[0], joint_pos[1].get_value() - joint_offset_[1]};
    act_pos[0].set_value(
      (joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1]) * ar[0]);
    act_pos[1].set_value(
      (joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1]) * ar[1]);
  }
 
  auto & act_vel = actuator_velocity_;
  auto & joint_vel = joint_velocity_;
  if (act_vel.size() == num_actuators() && joint_vel.size() == num_joints())
  {
    assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]);
 
    act_vel[0].set_value(
      (joint_vel[0].get_value() * jr[0] + joint_vel[1].get_value() * jr[1]) * ar[0]);
    act_vel[1].set_value(
      (joint_vel[0].get_value() * jr[0] - joint_vel[1].get_value() * jr[1]) * ar[1]);
  }
 
  auto & act_eff = actuator_effort_;
  auto & joint_eff = joint_effort_;
  if (act_eff.size() == num_actuators() && joint_eff.size() == num_joints())
  {
    assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);
 
    act_eff[0].set_value(
      (joint_eff[0].get_value() / jr[0] + joint_eff[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_eff[1].set_value(
      (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }
 
  auto & act_tor = actuator_torque_;
  auto & joint_tor = joint_torque_;
  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
  {
    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
 
    act_tor[0].set_value(
      (joint_tor[0].get_value() / jr[0] + joint_tor[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_tor[1].set_value(
      (joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }
 
  auto & act_for = actuator_force_;
  auto & joint_for = joint_force_;
  if (act_for.size() == num_actuators() && joint_for.size() == num_joints())
  {
    assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]);
 
    act_for[0].set_value(
      (joint_for[0].get_value() / jr[0] + joint_for[1].get_value() / jr[1]) / (2.0 * ar[0]));
    act_for[1].set_value(
      (joint_for[0].get_value() / jr[0] - joint_for[1].get_value() / jr[1]) / (2.0 * ar[1]));
  }
}
 
std::string DifferentialTransmission::get_handles_info() const
{
  return fmt::format(
    FMT_COMPILE(
      "Got the following handles:\n"
      "Joint position: {}, Actuator position: {}\n"
      "Joint velocity: {}, Actuator velocity: {}\n"
      "Joint effort: {}, Actuator effort: {}\n"
      "Joint torque: {}, Actuator torque: {}\n"
      "Joint force: {}, Actuator force: {}\n"
      "Joint absolute position: {}, Actuator absolute position: {}"),
    to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)),
    to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)),
    to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)),
    to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)),
    to_string(get_names(joint_force_)), to_string(get_names(actuator_force_)),
    to_string(get_names(joint_absolute_position_)),
    to_string(get_names(actuator_absolute_position_)));
}
 
}  // namespace transmission_interface
 
#endif  // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_