Skip to content

Commit

Permalink
Fix admittance controller interface read/write logic (backport #1232) (
Browse files Browse the repository at this point in the history
  • Loading branch information
mergify[bot] authored Aug 1, 2024
1 parent 1cdaacb commit 18234ac
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 7 deletions.
12 changes: 6 additions & 6 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -480,13 +480,13 @@ void AdmittanceController::read_state_from_hardware(
state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
nan_position |= std::isnan(state_current.positions[joint_ind]);
}
else if (has_velocity_state_interface_)
if (has_velocity_state_interface_)
{
state_current.velocities[joint_ind] =
state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
}
else if (has_acceleration_state_interface_)
if (has_acceleration_state_interface_)
{
state_current.accelerations[joint_ind] =
state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
Expand Down Expand Up @@ -532,15 +532,15 @@ void AdmittanceController::write_state_to_hardware(
command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
state_commanded.positions[joint_ind]);
}
else if (has_velocity_command_interface_)
if (has_velocity_command_interface_)
{
command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
state_commanded.positions[joint_ind]);
state_commanded.velocities[joint_ind]);
}
else if (has_acceleration_command_interface_)
if (has_acceleration_command_interface_)
{
command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
state_commanded.positions[joint_ind]);
state_commanded.accelerations[joint_ind]);
}
}
last_commanded_ = state_commanded;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -211,7 +211,6 @@ class TestableJointTrajectoryController
point.accelerations.resize(size, 0.0);
}
}

};

class TrajectoryControllerTest : public ::testing::Test
Expand Down

0 comments on commit 18234ac

Please sign in to comment.