Skip to content

For mobile robot with Isaac Sim (bug fixed version) #25

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

Closed
hijimasa wants to merge 7 commits into from
Closed

For mobile robot with Isaac Sim (bug fixed version) #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c602929
[add] check velocity command exists to publish command
hijimasa May 31, 2024
c8c3165
[fix] fixed bug about mimic joint only publish position
hijimasa Jun 10, 2024
82ae31f
[remove] remove unnecessary warining
hijimasa Aug 4, 2024
2b23e6c
allow hardware parameter to allow initialising joint commands with ro…
hijimasa Aug 4, 2024
6dccdc0
[add] add wait_for_reaching_initial_values parameter
hijimasa Aug 5, 2024
ffa421f
[fix] Fixed a bug that caused joints to return to their original posi…
hijimasa Sep 5, 2024
ab71778
Revert "allow hardware parameter to allow initialising joint commands…
hijimasa Oct 8, 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
4 changes: 4 additions & 0 deletions include/topic_based_ros2_control/topic_based_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
rclcpp::Node::SharedPtr node_;
sensor_msgs::msg::JointState latest_joint_state_;
bool sum_wrapped_joint_states_{ false };
bool initial_cmd_reached_{ false };

/// Use standard interfaces for joints because they are relevant for dynamic behavior
std::array<std::string, 4> standard_interfaces_ = { hardware_interface::HW_IF_POSITION,
Expand All @@ -80,6 +81,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface

struct MimicJoint
{
std::string joint_name;
std::string mimicked_joint_name;
std::size_t joint_index;
std::size_t mimicked_joint_index;
double multiplier = 1.0;
Expand All @@ -89,6 +92,7 @@ class TopicBasedSystem : public hardware_interface::SystemInterface
/// The size of this vector is (standard_interfaces_.size() x nr_joints)
std::vector<std::vector<double>> joint_commands_;
std::vector<std::vector<double>> joint_states_;
std::vector<std::vector<double>> initial_joint_commands_;
Copy link
Contributor

@JafarAbdi JafarAbdi Oct 8, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do we need this?

Copy link
Author

@hijimasa hijimasa Oct 8, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is used to store the initial value of each joint; assigning to joint_commands_ will overwrite the value.

Copy link
Author

@hijimasa hijimasa Oct 8, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

When using Isaac Sim and Move It 2, I have a different variable for joint_command because of the overwriting behavior when I set the initial value to joint_command. It seems that joint_command is overwritten with all zeros even when joint_command messages are not exchanged.


// If the difference between the current joint state and joint command is less than this value,
// the joint command will not be published.
Expand Down
176 changes: 142 additions & 34 deletions src/topic_based_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
}

// Initial command values
initial_joint_commands_ = joint_commands_;
for (auto i = 0u; i < info_.joints.size(); i++)
{
const auto& component = info_.joints[i];
Expand All @@ -91,7 +92,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
// Check the initial_value param is used
if (!interface.initial_value.empty())
{
joint_commands_[index][i] = std::stod(interface.initial_value);
initial_joint_commands_[index][i] = std::stod(interface.initial_value);
}
}
}
Expand All @@ -113,6 +114,8 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found");
}
MimicJoint mimic_joint;
mimic_joint.joint_name = joint.name;
mimic_joint.mimicked_joint_name = mimicked_joint_it->name;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index =
static_cast<std::size_t>(std::distance(info_.joints.begin(), mimicked_joint_it));
Expand Down Expand Up @@ -156,6 +159,10 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo&
{
sum_wrapped_joint_states_ = true;
}
if (get_hardware_parameter("wait_for_reaching_initial_values", "false") == "false")
{
initial_cmd_reached_ = true;
}

return CallbackReturn::SUCCESS;
}
Expand Down Expand Up @@ -264,70 +271,171 @@ bool TopicBasedSystem::getInterface(const std::string& name, const std::string&

hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
std::vector<std::vector<double>> target_joint_commands;
if (initial_cmd_reached_ == false)
{
double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(),
joint_commands_[POSITION_INTERFACE_INDEX].end(), 0,
[](double sum, double val) { return sum + std::abs(val); });
if (abs_sum >= trigger_joint_command_threshold_)
{
initial_cmd_reached_ = true;
}
}
if (initial_cmd_reached_ == false)
{
target_joint_commands = initial_joint_commands_;
}
else
{
target_joint_commands = joint_commands_;
}
// To avoid spamming TopicBased's joint command topic we check the difference between the joint states and
// the current joint commands, if it's smaller than a threshold we don't publish it.
const auto diff = std::transform_reduce(
joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(),
joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
target_joint_commands[POSITION_INTERFACE_INDEX].cbegin(), 0.0,
[](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
if (diff <= trigger_joint_command_threshold_)

bool exist_velocity_command = false;
static bool exist_velocity_command_old = false; // Use old state to publish zero velocities
for (std::size_t i = 0; i < info_.joints.size(); ++i)
{
if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
{
exist_velocity_command = true;
}
}

if (diff <= trigger_joint_command_threshold_ &&
(exist_velocity_command == false && exist_velocity_command_old == false))
{
return hardware_interface::return_type::OK;
}

sensor_msgs::msg::JointState joint_state;
for (std::size_t i = 0; i < info_.joints.size(); ++i)
exist_velocity_command_old = exist_velocity_command;

// For Position Joint
{
joint_state.name.push_back(info_.joints[i].name);
sensor_msgs::msg::JointState joint_state;
joint_state.header.stamp = node_->now();
// only send commands to the interfaces that are defined for this joint
for (const auto& interface : info_.joints[i].command_interfaces)
for (std::size_t i = 0; i < info_.joints.size(); ++i)
{
if (interface.name == hardware_interface::HW_IF_POSITION)
// only send commands to the interfaces that are defined for this joint
for (const auto& interface : info_.joints[i].command_interfaces)
{
joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]);
if (interface.name == hardware_interface::HW_IF_POSITION)
{
joint_state.name.push_back(info_.joints[i].name);
joint_state.position.push_back(target_joint_commands[POSITION_INTERFACE_INDEX][i]);
}
}
else if (interface.name == hardware_interface::HW_IF_VELOCITY)
}

for (const auto& mimic_joint : mimic_joints_)
{
for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
{
joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]);
if (interface.name == hardware_interface::HW_IF_POSITION)
{
for (size_t index = 0; index < joint_state.name.size(); index++)
{
if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
{
joint_state.name.push_back(mimic_joint.joint_name);
joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]);
}
}
}
}
else if (interface.name == hardware_interface::HW_IF_EFFORT)
}

if (rclcpp::ok() && joint_state.name.size() != 0)
{
topic_based_joint_commands_publisher_->publish(joint_state);
}
}

// For Velocity Joint
{
sensor_msgs::msg::JointState joint_state;
joint_state.header.stamp = node_->now();
for (std::size_t i = 0; i < info_.joints.size(); ++i)
{
// only send commands to the interfaces that are defined for this joint
for (const auto& interface : info_.joints[i].command_interfaces)
{
joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]);
if (interface.name == hardware_interface::HW_IF_VELOCITY)
{
joint_state.name.push_back(info_.joints[i].name);
joint_state.velocity.push_back(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]);
}
}
else
}

for (const auto& mimic_joint : mimic_joints_)
{
for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
{
RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.",
info_.joints[i].name.c_str(), interface.name.c_str());
if (interface.name == hardware_interface::HW_IF_VELOCITY)
{
for (size_t index = 0; index < joint_state.name.size(); index++)
{
if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
{
joint_state.name.push_back(mimic_joint.joint_name);
joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]);
}
}
}
}
}

if (rclcpp::ok() && joint_state.name.size() != 0)
{
topic_based_joint_commands_publisher_->publish(joint_state);
}
}

for (const auto& mimic_joint : mimic_joints_)
// For Effort Joint
{
for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
sensor_msgs::msg::JointState joint_state;
joint_state.header.stamp = node_->now();
for (std::size_t i = 0; i < info_.joints.size(); ++i)
{
if (interface.name == hardware_interface::HW_IF_POSITION)
{
joint_state.position[mimic_joint.joint_index] =
mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index];
}
else if (interface.name == hardware_interface::HW_IF_VELOCITY)
// only send commands to the interfaces that are defined for this joint
for (const auto& interface : info_.joints[i].command_interfaces)
{
joint_state.velocity[mimic_joint.joint_index] =
mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index];
if (interface.name == hardware_interface::HW_IF_EFFORT)
{
joint_state.name.push_back(info_.joints[i].name);
joint_state.effort.push_back(target_joint_commands[EFFORT_INTERFACE_INDEX][i]);
}
}
else if (interface.name == hardware_interface::HW_IF_EFFORT)
}

for (const auto& mimic_joint : mimic_joints_)
{
for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces)
{
joint_state.effort[mimic_joint.joint_index] =
mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index];
if (interface.name == hardware_interface::HW_IF_EFFORT)
{
for (size_t index = 0; index < joint_state.name.size(); index++)
{
if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
{
joint_state.name.push_back(mimic_joint.joint_name);
joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]);
}
}
}
}
}
}

if (rclcpp::ok())
{
topic_based_joint_commands_publisher_->publish(joint_state);
if (rclcpp::ok() && joint_state.name.size() != 0)
{
topic_based_joint_commands_publisher_->publish(joint_state);
}
}

return hardware_interface::return_type::OK;
Expand Down