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

Jump to bottom

Ensure that sim_joints_ always has the same number of elements as the… #77

Merged
merged 5 commits into from
May 28, 2021
Merged

Ensure that sim_joints_ always has the same number of elements as the… #77

Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
576cdad
Ensure that sim_joints_ always has the same number of elements as the…
kbogert May 25, 2021
2acce26
Also don't publish un-simulated joints to avoid freaking out MoveGroup
kbogert May 25, 2021
12020d0
Correct indentation
kbogert May 27, 2021
a8fcedb
Remove empty line
kbogert May 28, 2021
199a15e
Remove trailing whitespace
kbogert May 28, 2021
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
113 changes: 65 additions & 48 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,13 +127,13 @@ bool GazeboSystem::initSim(
std::string joint_name = this->dataPtr->joint_names_[j] = hardware_info.joints[j].name;

gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
this->dataPtr->sim_joints_.push_back(simjoint);
if (!simjoint) {
RCLCPP_WARN_STREAM(
this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name <<
"' which is not in the gazebo model.");
continue;
}
this->dataPtr->sim_joints_.push_back(simjoint);

// Accept this joint and continue configuration
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
Expand Down Expand Up @@ -183,6 +183,7 @@ bool GazeboSystem::initSim(
}
}
}

return true;
}

Expand All @@ -201,25 +202,31 @@ GazeboSystem::export_state_interfaces()
std::vector<hardware_interface::StateInterface> state_interfaces;

for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_[i]));
if (this->dataPtr->sim_joints_[i]) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_[i]));
}
}
for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_[i]));
if (this->dataPtr->sim_joints_[i]) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_[i]));
}
}
for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_[i]));
if (this->dataPtr->sim_joints_[i]) {
state_interfaces.emplace_back(
hardware_interface::StateInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_[i]));
}
}
return state_interfaces;
}
Expand All @@ -230,25 +237,31 @@ GazeboSystem::export_command_interfaces()
std::vector<hardware_interface::CommandInterface> command_interfaces;

for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_cmd_[i]));
if (this->dataPtr->sim_joints_[i]) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_cmd_[i]));
}
}
for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_cmd_[i]));
if (this->dataPtr->sim_joints_[i]) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_cmd_[i]));
}
}
for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_cmd_[i]));
if (this->dataPtr->sim_joints_[i]) {
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
this->dataPtr->joint_names_[i],
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_cmd_[i]));
}
}
return command_interfaces;
}
Expand All @@ -268,9 +281,11 @@ hardware_interface::return_type GazeboSystem::stop()
hardware_interface::return_type GazeboSystem::read()
{
for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(0);
this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(0);
this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(0u);
if (this->dataPtr->sim_joints_[j]) {
this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(0);
this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(0);
this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(0u);
}
}
return hardware_interface::return_type::OK;
}
Expand All @@ -283,20 +298,22 @@ hardware_interface::return_type GazeboSystem::write()
rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;

for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
if (this->dataPtr->joint_control_methods_[j] & POSITION) {
this->dataPtr->sim_joints_[j]->SetPosition(
0, this->dataPtr->joint_position_cmd_[j],
true);
}
if (this->dataPtr->joint_control_methods_[j] & VELOCITY) {
this->dataPtr->sim_joints_[j]->SetVelocity(
0,
this->dataPtr->joint_velocity_cmd_[j]);
}
if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
const double effort =
this->dataPtr->joint_effort_cmd_[j];
this->dataPtr->sim_joints_[j]->SetForce(0, effort);
if (this->dataPtr->sim_joints_[j]) {
kbogert marked this conversation as resolved.
Show resolved Hide resolved
if (this->dataPtr->joint_control_methods_[j] & POSITION) {
this->dataPtr->sim_joints_[j]->SetPosition(
0, this->dataPtr->joint_position_cmd_[j],
true);
}
if (this->dataPtr->joint_control_methods_[j] & VELOCITY) {
this->dataPtr->sim_joints_[j]->SetVelocity(
0,
this->dataPtr->joint_velocity_cmd_[j]);
}
if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
const double effort =
this->dataPtr->joint_effort_cmd_[j];
this->dataPtr->sim_joints_[j]->SetForce(0, effort);
}
}
}

Expand Down