Skip to content

fix: validate end_effector_frame to prevent OOB access on invalid frame name #53

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

Open
Nabil-Miri wants to merge 1 commit into
base: main
Choose a base branch
from
+78 −0
Open

fix: validate end_effector_frame to prevent OOB access on invalid frame name #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
30 changes: 30 additions & 0 deletions src/cartesian_admittance_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,26 @@ CartesianAdmittanceController::update(const rclcpp::Time & time, const rclcpp::D
pinocchio::updateFramePlacements(model_, data_);
end_effector_pose = data_.oMf[end_effector_frame_id];

if (!end_effector_pose.translation().allFinite() ||
!end_effector_pose.rotation().allFinite()) {
RCLCPP_ERROR_THROTTLE(
get_node()->get_logger(),
*get_node()->get_clock(),
1000,
"end_effector_pose contains NaN/Inf. "
"Holding previous torque command this cycle.");
if (!params_.stop_commands) {
for (size_t i = 0; i < params_.joints.size(); ++i) {
#if ROS2_VERSION_ABOVE_HUMBLE
(void)command_interfaces_[i].set_value(tau_previous[i]);
#else
command_interfaces_[i].set_value(tau_previous[i]);
#endif
}
}
return controller_interface::return_type::OK;
}

// 6. Initialize admittance state on first cycle
if (!admittance_initialized_) {
inner_SE3_ = end_effector_pose;
Expand Down Expand Up @@ -367,6 +387,16 @@ CartesianAdmittanceController::on_configure(const rclcpp_lifecycle::State & /*pr
}

// Preallocate the matrices and vectors that will be used in the control loop
if (!model_.existFrame(params_.end_effector_frame)) {
RCLCPP_ERROR_STREAM(
get_node()->get_logger(),
"end_effector_frame '" << params_.end_effector_frame
<< "' is not present in the robot model. Refusing to configure: "
"activating with an invalid frame results in undefined behavior "
"(out-of-bounds access into pinocchio::Data, manifesting as a "
"segfault or NaN/Inf in computed torques).");
return CallbackReturn::ERROR;
}
end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
if (params_.ft_sensor.frame.empty()) {
ft_sensor_frame_id = end_effector_frame_id;
Expand Down
30 changes: 30 additions & 0 deletions src/cartesian_controller.cpp
View file
Open in desktop
Copy link
Copy Markdown
Collaborator

@danielsanjosepro danielsanjosepro May 12, 2026

Choose a reason for hiding this comment

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

are the checks in the update and on_activate even needed if the on_configure already fails?

Copy link
Copy Markdown
Author

@Nabil-Miri Nabil-Miri May 12, 2026

Choose a reason for hiding this comment

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

Thanks for the review!
You are right. The on_activate check is redundant. I will drop it.
The update() checks for any NaN/Inf invalid value appearing at runtime. I think realistically it should rarely happen. Should I keep it or drop it too?

Comment thread
Nabil-Miri marked this conversation as resolved.
Original file line number Diff line number Diff line change
Expand Up @@ -100,6 +100,26 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
* target_position_);*/
end_effector_pose = data_.oMf[end_effector_frame_id];

if (!end_effector_pose.translation().allFinite() ||
!end_effector_pose.rotation().allFinite()) {
RCLCPP_ERROR_THROTTLE(
get_node()->get_logger(),
*get_node()->get_clock(),
1000,
"end_effector_pose contains NaN/Inf. "
"Holding previous torque command this cycle.");
if (!params_.stop_commands) {
for (size_t i = 0; i < params_.joints.size(); ++i) {
#if ROS2_VERSION_ABOVE_HUMBLE
(void)command_interfaces_[i].set_value(tau_previous[i]);
#else
command_interfaces_[i].set_value(tau_previous[i]);
#endif
}
}
return controller_interface::return_type::OK;
}

// We consider translation and rotation separately to avoid unatural screw
// motions
if (params_.use_local_jacobian) {
Expand Down Expand Up @@ -303,6 +323,16 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
}

// Preallocate the matrices and vectors that will be used in the control loop
if (!model_.existFrame(params_.end_effector_frame)) {
RCLCPP_ERROR_STREAM(
get_node()->get_logger(),
"end_effector_frame '" << params_.end_effector_frame
<< "' is not present in the robot model. Refusing to configure: "
"activating with an invalid frame results in undefined behavior "
"(out-of-bounds access into pinocchio::Data, manifesting as a "
"segfault or NaN/Inf in computed torques).");
return CallbackReturn::ERROR;
}
end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
q = Eigen::VectorXd::Zero(model_.nv);
q_pin = Eigen::VectorXd::Zero(model_.nq);
Expand Down
9 changes: 9 additions & 0 deletions src/pose_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,15 @@ CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*p
}
}

if (!model_.existFrame(params_.end_effector_frame)) {
RCLCPP_ERROR_STREAM(
get_node()->get_logger(),
"end_effector_frame '" << params_.end_effector_frame
<< "' is not present in the robot model. Refusing to configure: "
"activating with an invalid frame results in undefined behavior "
"(out-of-bounds access into pinocchio::Data).");
return CallbackReturn::ERROR;
}
end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
q = Eigen::VectorXd::Zero(model_.nv);

Expand Down
9 changes: 9 additions & 0 deletions src/twist_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,15 @@ CallbackReturn TwistBroadcaster::on_configure(const rclcpp_lifecycle::State & /*
}
}

if (!model_.existFrame(params_.end_effector_frame)) {
RCLCPP_ERROR_STREAM(
get_node()->get_logger(),
"end_effector_frame '" << params_.end_effector_frame
<< "' is not present in the robot model. Refusing to configure: "
"activating with an invalid frame results in undefined behavior "
"(out-of-bounds access into pinocchio::Data).");
return CallbackReturn::ERROR;
}
end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
q = Eigen::VectorXd::Zero(model_.nv);
q_dot = Eigen::VectorXd::Zero(model_.nv);
Expand Down