Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Transmissions] Make transmission interface work with new variants approach #1845

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

[Transmissions] Make transmission interface work with new variants approach #1845

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
a283fcf
migrate the transmission_interface to the new variants changes
saikishor Nov 4, 2024
0edcd82
Fix the utils test
saikishor Nov 4, 2024
7f7e608
Fix the simple transmission test
saikishor Nov 4, 2024
85af879
fix differential_transmission tests
saikishor Nov 4, 2024
9fa9b18
Fix four base linkage transmission tests
saikishor Nov 4, 2024
a79ef49
Merge branch 'master' into fix/transmissions/variants
saikishor Dec 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions transmission_interface/include/transmission_interface/accessor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ std::vector<std::string> get_names(const std::vector<T> & handles)
std::set<std::string> names;
std::transform(
handles.cbegin(), handles.cend(), std::inserter(names, names.end()),
[](const auto & handle) { return handle.get_prefix_name(); });
[](const auto & handle) { return handle->get_prefix_name(); });
return std::vector<std::string>(names.begin(), names.end());
}

Expand All @@ -63,8 +63,8 @@ std::vector<T> get_ordered_handles(
unordered_handles.cbegin(), unordered_handles.cend(), std::back_inserter(result),
[&](const auto & handle)
{
return (handle.get_prefix_name() == name) &&
(handle.get_interface_name() == interface_type) && handle;
return (handle->get_prefix_name() == name) &&
(handle->get_interface_name() == interface_type) && handle;
});
}
return result;
Expand Down
79 changes: 46 additions & 33 deletions transmission_interface/include/transmission_interface/differential_transmission.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,8 +125,8 @@ class DifferentialTransmission : public Transmission
* \pre Handles are valid and matching in size
*/
void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles) override;
const std::vector<JointHandle::SharedPtr> & joint_handles,
const std::vector<ActuatorHandle::SharedPtr> & actuator_handles) override;

/// Transform variables from actuator to joint space.
/**
Expand Down Expand Up @@ -159,13 +159,13 @@ class DifferentialTransmission : public Transmission
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::SharedPtr> joint_position_;
std::vector<JointHandle::SharedPtr> joint_velocity_;
std::vector<JointHandle::SharedPtr> joint_effort_;

std::vector<ActuatorHandle> actuator_position_;
std::vector<ActuatorHandle> actuator_velocity_;
std::vector<ActuatorHandle> actuator_effort_;
std::vector<ActuatorHandle::SharedPtr> actuator_position_;
std::vector<ActuatorHandle::SharedPtr> actuator_velocity_;
std::vector<ActuatorHandle::SharedPtr> actuator_effort_;
};

inline DifferentialTransmission::DifferentialTransmission(
Expand All @@ -191,8 +191,8 @@ inline DifferentialTransmission::DifferentialTransmission(
}

void DifferentialTransmission::configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles)
const std::vector<JointHandle::SharedPtr> & joint_handles,
const std::vector<ActuatorHandle::SharedPtr> & actuator_handles)
{
if (joint_handles.empty())
{
Expand All @@ -204,6 +204,19 @@ void DifferentialTransmission::configure(
throw Exception("No actuator handles were passed in");
}

if (std::any_of(
joint_handles.cbegin(), joint_handles.cend(), [](const auto & handle) { return !handle; }))
{
throw Exception("Null pointers in joint handles");
}

if (std::any_of(
actuator_handles.cbegin(), actuator_handles.cend(),
[](const auto & handle) { return !handle; }))
{
throw Exception("Null pointers in actuator handles");
}

const auto joint_names = get_names(joint_handles);
if (joint_names.size() != 2)
{
Expand Down Expand Up @@ -264,11 +277,11 @@ inline void DifferentialTransmission::actuator_to_joint()
{
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_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_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]);
}

Expand All @@ -278,10 +291,10 @@ inline void DifferentialTransmission::actuator_to_joint()
{
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]));
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_;
Expand All @@ -290,10 +303,10 @@ inline void DifferentialTransmission::actuator_to_joint()
{
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]));
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]));
}
}

Expand All @@ -309,10 +322,10 @@ inline void DifferentialTransmission::joint_to_actuator()
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(
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(
act_pos[1]->set_value(
(joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1]) * ar[1]);
}

Expand All @@ -322,10 +335,10 @@ inline void DifferentialTransmission::joint_to_actuator()
{
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]);
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_;
Expand All @@ -334,10 +347,10 @@ inline void DifferentialTransmission::joint_to_actuator()
{
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]));
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]));
}
}

Expand Down
65 changes: 39 additions & 26 deletions transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,8 +123,8 @@ class FourBarLinkageTransmission : public Transmission
* \pre Handles are valid and matching in size
*/
void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles) override;
const std::vector<JointHandle::SharedPtr> & joint_handles,
const std::vector<ActuatorHandle::SharedPtr> & actuator_handles) override;

/// Transform variables from actuator to joint space.
/**
Expand Down Expand Up @@ -157,13 +157,13 @@ class FourBarLinkageTransmission : public Transmission
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::SharedPtr> joint_position_;
std::vector<JointHandle::SharedPtr> joint_velocity_;
std::vector<JointHandle::SharedPtr> joint_effort_;

std::vector<ActuatorHandle> actuator_position_;
std::vector<ActuatorHandle> actuator_velocity_;
std::vector<ActuatorHandle> actuator_effort_;
std::vector<ActuatorHandle::SharedPtr> actuator_position_;
std::vector<ActuatorHandle::SharedPtr> actuator_velocity_;
std::vector<ActuatorHandle::SharedPtr> actuator_effort_;
};

inline FourBarLinkageTransmission::FourBarLinkageTransmission(
Expand All @@ -188,8 +188,8 @@ inline FourBarLinkageTransmission::FourBarLinkageTransmission(
}

void FourBarLinkageTransmission::configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles)
const std::vector<JointHandle::SharedPtr> & joint_handles,
const std::vector<ActuatorHandle::SharedPtr> & actuator_handles)
{
if (joint_handles.empty())
{
Expand All @@ -201,6 +201,19 @@ void FourBarLinkageTransmission::configure(
throw Exception("No actuator handles were passed in");
}

if (std::any_of(
joint_handles.cbegin(), joint_handles.cend(), [](const auto & handle) { return !handle; }))
{
throw Exception("Null pointers in joint handles");
}

if (std::any_of(
actuator_handles.cbegin(), actuator_handles.cend(),
[](const auto & handle) { return !handle; }))
{
throw Exception("Null pointers in actuator handles");
}

const auto joint_names = get_names(joint_handles);
if (joint_names.size() != 2)
{
Expand Down Expand Up @@ -262,9 +275,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint()
{
assert(act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1]);

joint_pos[0].set_value(act_pos[0].get_value() / (jr[0] * ar[0]) + joint_offset_[0]);
joint_pos[1].set_value(
(act_pos[1].get_value() / ar[1] - act_pos[0].get_value() / (jr[0] * ar[0])) / jr[1] +
joint_pos[0]->set_value(act_pos[0]->get_value() / (jr[0] * ar[0]) + joint_offset_[0]);
joint_pos[1]->set_value(
(act_pos[1]->get_value() / ar[1] - act_pos[0]->get_value() / (jr[0] * ar[0])) / jr[1] +
joint_offset_[1]);
}

Expand All @@ -275,9 +288,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint()
{
assert(act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1]);

joint_vel[0].set_value(act_vel[0].get_value() / (jr[0] * ar[0]));
joint_vel[1].set_value(
(act_vel[1].get_value() / ar[1] - act_vel[0].get_value() / (jr[0] * ar[0])) / jr[1]);
joint_vel[0]->set_value(act_vel[0]->get_value() / (jr[0] * ar[0]));
joint_vel[1]->set_value(
(act_vel[1]->get_value() / ar[1] - act_vel[0]->get_value() / (jr[0] * ar[0])) / jr[1]);
}

// effort
Expand All @@ -287,9 +300,9 @@ inline void FourBarLinkageTransmission::actuator_to_joint()
{
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]);
joint_eff[1].set_value(
jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0]));
joint_eff[0]->set_value(jr[0] * act_eff[0]->get_value() * ar[0]);
joint_eff[1]->set_value(
jr[1] * (act_eff[1]->get_value() * ar[1] - jr[0] * act_eff[0]->get_value() * ar[0]));
}
}

Expand All @@ -306,9 +319,9 @@ inline void FourBarLinkageTransmission::joint_to_actuator()
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] * ar[0]);
act_pos[1].set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]);
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] * ar[0]);
act_pos[1]->set_value((joints_offset_applied[0] + joints_offset_applied[1] * jr[1]) * ar[1]);
}

// velocity
Expand All @@ -318,8 +331,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator()
{
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] * ar[0]);
act_vel[1].set_value((joint_vel[0].get_value() + joint_vel[1].get_value() * jr[1]) * ar[1]);
act_vel[0]->set_value(joint_vel[0]->get_value() * jr[0] * ar[0]);
act_vel[1]->set_value((joint_vel[0]->get_value() + joint_vel[1]->get_value() * jr[1]) * ar[1]);
}

// effort
Expand All @@ -329,8 +342,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator()
{
assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);

act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0]));
act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]);
act_eff[0]->set_value(joint_eff[0]->get_value() / (ar[0] * jr[0]));
act_eff[1]->set_value((joint_eff[0]->get_value() + joint_eff[1]->get_value() / jr[1]) / ar[1]);
}
}

Expand Down
4 changes: 4 additions & 0 deletions transmission_interface/include/transmission_interface/handle.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_
#define TRANSMISSION_INTERFACE__HANDLE_HPP_

#include <memory>

#include "hardware_interface/handle.hpp"

namespace transmission_interface
Expand All @@ -24,13 +26,15 @@ class ActuatorHandle : public hardware_interface::Handle
{
public:
using hardware_interface::Handle::Handle;
using SharedPtr = std::shared_ptr<ActuatorHandle>;
};

/** A handle used to get and set a value on a given joint interface. */
class JointHandle : public hardware_interface::Handle
{
public:
using hardware_interface::Handle::Handle;
using SharedPtr = std::shared_ptr<JointHandle>;
};

} // namespace transmission_interface
Expand Down
Loading
Loading