Program Listing for File simple_transmission.hpp
↰ Return to documentation for file (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_