For mobile robot with Isaac Sim (bug fixed version) #25
+146
−34
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -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]; | ||
|
|
@@ -91,7 +92,7 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& | |
| // Check the initial_value param is used | ||
| if (!interface.initial_value.empty()) | ||
| { | ||
| initial_joint_commands_[index][i] = std::stod(interface.initial_value); | ||
|
There was a problem hiding this comment. Any reason to have a different variable for the initial ones? We should use There was a problem hiding this comment. The following file appears to have initial values for state_interfaces. So I'm using state_interfaces. |
||
| } | ||
| } | ||
| } | ||
|
|
@@ -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)); | ||
|
|
@@ -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; | ||
| } | ||
|
|
@@ -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(), | ||
| 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>{}); | ||
|
|
||
| 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; | ||
| } | ||
|
|
||
| exist_velocity_command_old = exist_velocity_command; | ||
|
|
||
| // For Position 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) | ||
| { | ||
| 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]); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| for (const auto& mimic_joint : mimic_joints_) | ||
| { | ||
| for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) | ||
| { | ||
| 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]); | ||
| } | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| 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) | ||
| { | ||
| 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]); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| for (const auto& mimic_joint : mimic_joints_) | ||
| { | ||
| for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) | ||
| { | ||
| 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 Effort 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) | ||
| { | ||
| 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]); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| for (const auto& mimic_joint : mimic_joints_) | ||
| { | ||
| for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) | ||
| { | ||
| 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() && joint_state.name.size() != 0) | ||
| { | ||
| topic_based_joint_commands_publisher_->publish(joint_state); | ||
| } | ||
| } | ||
|
|
||
| return hardware_interface::return_type::OK; | ||
|
|
||
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.