diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 2404464..d642eea 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -7,6 +7,20 @@ repos:
args:
- --maxkb=2048
- id: check-merge-conflict
+ - repo: https://github.com/astral-sh/ruff-pre-commit
+ # Ruff version.
+ rev: v0.12.7
+ hooks:
+ # Run the linter.
+ - id: ruff-check
+ args: [ --fix, --exit-non-zero-on-fix ]
+ # Run the formatter.
+ - id: ruff-format
+ - repo: https://github.com/python-poetry/poetry
+ rev: 2.1.3
+ hooks:
+ - id: poetry-check
+ - id: poetry-lock
- repo: https://github.com/gitleaks/gitleaks
rev: v8.28.0
hooks:
@@ -49,3 +63,31 @@ repos:
exit 1
- --
+ - repo: local
+ hooks:
+ - id: ament_cppcheck
+ name: ament_cppcheck
+ description: Static code analysis of C/C++ files.
+ entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
+ language: system
+ files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+ - repo: local
+ hooks:
+ - id: ament_cpplint
+ name: ament_cpplint
+ description: Static code analysis of C/C++ files.
+ entry: ament_cpplint
+ language: system
+ files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+ args: ["--linelength=100", "--filter=-whitespace/newline,-legal/copyright"]
+
+ # Cmake hooks
+ - repo: local
+ hooks:
+ - id: ament_lint_cmake
+ name: ament_lint_cmake
+ description: Check format of CMakeLists.txt files.
+ entry: ament_lint_cmake
+ language: system
+ files: CMakeLists\.txt$
diff --git a/controller_tuning.xml b/controller_tuning.xml
new file mode 100644
index 0000000..fd96ef5
--- /dev/null
+++ b/controller_tuning.xml
@@ -0,0 +1,208 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp
index 32df937..6f1fbe9 100644
--- a/include/crisp_controllers/cartesian_controller.hpp
+++ b/include/crisp_controllers/cartesian_controller.hpp
@@ -14,10 +14,10 @@
#include
#include
#include
-#include
#include
#include
#include
+#include
#include
diff --git a/script/config/roslog_to_csv.json b/script/config/roslog_to_csv.json
new file mode 100644
index 0000000..18cce6c
--- /dev/null
+++ b/script/config/roslog_to_csv.json
@@ -0,0 +1,22 @@
+{
+ "keys": {
+ "q": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+ "q target": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+ "q ref": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+ "dq": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"],
+ "current eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+ "interpolated eef": ["x", "y", "z", "yaw", "pitch", "roll","qx", "qy", "qz", "qw"],
+ "desired eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+ "target eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"],
+ "error": ["x", "y", "z", "yaw", "pitch", "roll"],
+ "unnormalized new_angular": ["omega_x", "omega_y", "omega_z"],
+ "unnormalized filtered_angular": ["omega_x", "omega_y", "omega_z"],
+ "normalized new_angular": ["omega_x", "omega_y", "omega_z"],
+ "normalized filtered_angular": ["omega_x", "omega_y", "omega_z"],
+ "angle_diff": ["angle_diff"],
+ "inter angular": ["omega_x", "omega_y", "omega_z"],
+ "control input": ["x", "y", "z", "yaw", "pitch", "roll"],
+ "task wrench": ["x", "y", "z", "yaw", "pitch", "roll"],
+ "task tau": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"]
+ }
+}
\ No newline at end of file
diff --git a/script/roslog_to_csv.py b/script/roslog_to_csv.py
new file mode 100755
index 0000000..e87a750
--- /dev/null
+++ b/script/roslog_to_csv.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+# Copyright (c) 2026 Su Lu.
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+import csv
+import json
+import os
+import re
+
+LOG_RE = re.compile(r"^\[\w+\]\s+\[(?P\d+\.\d+)\]\s+\[[^\]]+\]:\s+(?P.*)$")
+
+KEYVAL_RE = re.compile(r"^(?P[^:]+):\s*(?P.*)$")
+SPLIT_RE = re.compile(r"[,\s]+")
+
+
+def sanitize(s: str) -> str:
+ s = s.strip().lower()
+ s = re.sub(r"\s+", "_", s)
+ return re.sub(r"[^a-z0-9_]", "", s)
+
+
+def parse_values(s: str):
+ return [float(v) for v in SPLIT_RE.split(s.strip()) if v]
+
+
+def load_config(config_path: str) -> dict:
+ """Load and parse configuration file."""
+ with open(config_path, encoding="utf-8") as f:
+ return json.load(f)
+
+
+def prepare_columns(keys: dict) -> tuple[list, dict]:
+ """Prepare column headers and key-to-column mappings."""
+ columns = []
+ key_cols = {}
+ for key, fields in keys.items():
+ key_cols[key] = [f"{sanitize(key)}.{sanitize(f)}" for f in fields]
+ columns.extend(key_cols[key])
+ return columns, key_cols
+
+
+def parse_log_file(
+ input_file: str, keys: dict, key_cols: dict, columns: list
+) -> tuple[list, set]:
+ """Parse log file and extract data rows."""
+ rows = []
+ current_row = None
+ found_keys = set()
+
+ def flush():
+ nonlocal current_row
+ if current_row:
+ rows.append(current_row)
+ current_row = None
+
+ with open(input_file, encoding="utf-8") as f:
+ for line in f:
+ m = LOG_RE.match(line)
+ if not m:
+ continue
+
+ body = m.group("body")
+ km = KEYVAL_RE.match(body)
+ if not km:
+ continue
+
+ key = km.group("key").strip()
+ if key not in keys:
+ continue
+
+ found_keys.add(key)
+ values = parse_values(km.group("values"))
+ fields = keys[key]
+
+ if len(values) < len(fields):
+ continue
+
+ # Check if this key's columns are already populated - if so, create new row
+ if current_row and any(current_row[col] for col in key_cols[key]):
+ flush()
+
+ # Initialize row if needed
+ if current_row is None:
+ current_row = {c: "" for c in columns}
+
+ for col, v in zip(key_cols[key], values):
+ current_row[col] = v
+
+ flush()
+ return rows, found_keys
+
+
+def remove_missing_columns(
+ columns: list, key_cols: dict, rows: list, keys: dict, found_keys: set
+) -> list:
+ """Remove columns for keys not found in the log file."""
+ missing_keys = set(keys.keys()) - found_keys
+ if not missing_keys:
+ return columns
+
+ print("Warning: The following data fields were not found in the log file:")
+ for key in sorted(missing_keys):
+ print(f" - {key}")
+
+ columns_to_remove = set()
+ for missing_key in missing_keys:
+ columns_to_remove.update(key_cols[missing_key])
+
+ columns = [col for col in columns if col not in columns_to_remove]
+
+ for row in rows:
+ for col in columns_to_remove:
+ row.pop(col, None)
+
+ return columns
+
+
+def format_float_values(rows: list, columns: list) -> None:
+ """Convert scientific notation to decimal format for better CSV compatibility."""
+ for row in rows:
+ for col in columns:
+ if col in row and isinstance(row[col], float):
+ row[col] = f"{row[col]:.10f}".rstrip("0").rstrip(".")
+
+
+def write_csv(output_file: str, columns: list, rows: list) -> None:
+ """Write data to CSV file."""
+ with open(output_file, "w", newline="", encoding="utf-8") as out:
+ writer = csv.DictWriter(out, fieldnames=columns)
+ writer.writeheader()
+ writer.writerows(rows)
+
+
+def main():
+ ap = argparse.ArgumentParser()
+ ap.add_argument("-i", "--input", required=True)
+ ap.add_argument("-o", "--output", required=False)
+ ap.add_argument("-c", "--config", default="config/roslog_to_csv.json")
+ args = ap.parse_args()
+
+ # If no output name is given, use input filename (without extension) + .csv
+ if args.output is None:
+ base_name = os.path.basename(args.input)
+ name_without_ext = os.path.splitext(base_name)[0]
+ args.output = name_without_ext + ".csv"
+
+ cfg = load_config(args.config)
+ keys = cfg["keys"]
+
+ columns, key_cols = prepare_columns(keys)
+ rows, found_keys = parse_log_file(args.input, keys, key_cols, columns)
+ columns = remove_missing_columns(columns, key_cols, rows, keys, found_keys)
+ format_float_values(rows, columns)
+ write_csv(args.output, columns, rows)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp
index 2799042..85d0bd2 100644
--- a/src/cartesian_controller.cpp
+++ b/src/cartesian_controller.cpp
@@ -65,7 +65,6 @@ CartesianController::state_interface_configuration() const {
controller_interface::return_type
CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) {
-
// Update current state information with EMA filtered values
updateCurrentState();
@@ -214,8 +213,52 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
setStiffnessAndDamping();
}
+ // TODO(placeholder): log_debug_info function has glitches which results in different number of
+ // data points logged same frequence. For instance, q, dq are logged with same frequency,
+ // but the number of data points can be different. Turning off the logging by assigning false
+ // to params_.log.enabled. This can be done in controllers yaml file.
log_debug_info(time);
+ // TODO(placeholder): The following debug output is supposed to be wrapped within
+ // #ifndef NDEBUG
+ // ...
+ // #endif
+ // So that the debug build can enable the output. However, debug build causes significant
+ // performance drop in the controller which reults in "communication_constraint_violation" errors
+ // for Franka robot.
+
+ // static int read_counter = 0;
+ // // Change the number that `read_counter` doing mode operation with to control the output frequency
+ // // 1 is for every cycle, 2 is for every other cycle, etc. Increase the number to reduce output
+ // // frequency, to avoid causing "communication_constraint_violation" errors for Franka robot.
+ // if (read_counter % 1 == 0) {
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q: " << q.transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q target: " << q_target.transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q ref: " << q_ref.transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "dq: " << dq.transpose());
+ // RCLCPP_INFO_STREAM(
+ // get_node()->get_logger(),
+ // "current eef:" << end_effector_pose.translation().transpose() << " "
+ // << end_effector_pose.rotation().eulerAngles(2, 1, 0).transpose() << " "
+ // << Eigen::Quaterniond(end_effector_pose.rotation()).coeffs().transpose());
+ // RCLCPP_INFO_STREAM(
+ // get_node()->get_logger(),
+ // "desired eef:" << desired_position_.transpose() << " "
+ // << desired_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose()
+ // << " " << desired_orientation_.coeffs().transpose());
+
+ // RCLCPP_INFO_STREAM(
+ // get_node()->get_logger(),
+ // "target eef:" << target_position_.transpose() << " "
+ // << target_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose()
+ // << " " << target_orientation_.coeffs().transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "error: " << error.transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "task tau: " << tau_task.transpose());
+ // RCLCPP_INFO_STREAM(get_node()->get_logger(), "nullspace tau: " << tau_nullspace.transpose());
+ // read_counter = 0; // Optional: prevents overflow, but not necessary
+ // }
+ // read_counter++;
+
return controller_interface::return_type::OK;
}
@@ -301,7 +344,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
return CallbackReturn::ERROR;
}
}
-
+
// Preallocate the matrices and vectors that will be used in the control loop
end_effector_frame_id = model_.getFrameId(params_.end_effector_frame);
q = Eigen::VectorXd::Zero(model_.nv);
@@ -402,7 +445,9 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
stiffness_sub_ = get_node()->create_subscription(
params_.variable_stiffness.topic, rclcpp::QoS(1), target_stiffness_callback);
- RCLCPP_INFO(get_node()->get_logger(), "Variable stiffness topic: %s",
+ RCLCPP_INFO(
+ get_node()->get_logger(),
+ "Variable stiffness topic: %s",
params_.variable_stiffness.topic.c_str());
// Initialize all control vectors with appropriate dimensions
@@ -476,15 +521,27 @@ void CartesianController::setStiffnessAndDamping() {
const double max_k_rot = params_.variable_max_stiffness.rotational;
for (int i = 0; i < 3; ++i) {
if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_trans) {
- RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
- "Translational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_trans);
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 100,
+ "Translational stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i,
+ stiffness(i, i),
+ max_k_trans);
stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_trans);
}
}
for (int i = 3; i < 6; ++i) {
if (stiffness(i, i) < 0.0 || stiffness(i, i) > max_k_rot) {
- RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
- "Rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, stiffness(i, i), max_k_rot);
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 100,
+ "Rotational stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i,
+ stiffness(i, i),
+ max_k_rot);
stiffness(i, i) = std::clamp(stiffness(i, i), 0.0, max_k_rot);
}
}
@@ -530,26 +587,21 @@ void CartesianController::updateCurrentState(bool initialize) {
double q_meas = state_interfaces_[i].get_value();
double dq_meas = state_interfaces_[num_joints + i].get_value();
#endif
-
- q_ref[i] = initialize
- ? q_meas
- : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref);
-
- q[i] = initialize
- ? q_meas
- : exponential_moving_average(q[i], q_meas, params_.filter.q);
-
- if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous
- // joint that is SO(2)
+
+ q_ref[i] =
+ initialize ? q_meas : exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref);
+
+ q[i] = initialize ? q_meas : exponential_moving_average(q[i], q_meas, params_.filter.q);
+
+ if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous
+ // joint that is SO(2)
q_pin[joint.idx_q()] = std::cos(q[i]);
q_pin[joint.idx_q() + 1] = std::sin(q[i]);
} else { // simple revolute joint case
q_pin[joint.idx_q()] = q[i];
}
- dq[i] = initialize
- ? dq_meas
- : exponential_moving_average(dq[i], dq_meas, params_.filter.dq);
+ dq[i] = initialize ? dq_meas : exponential_moving_average(dq[i], dq_meas, params_.filter.dq);
q_target[i] = initialize ? q_meas : q_target[i];
}
@@ -557,7 +609,6 @@ void CartesianController::updateCurrentState(bool initialize) {
CallbackReturn
CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
-
// Update the current state with initial measurements (no EMA filtering)
// to avoid large initial errors
updateCurrentState(true);
@@ -626,28 +677,48 @@ void CartesianController::parse_target_stiffness_() {
}
const double max_k_trans = params_.variable_max_stiffness.translational;
const double max_k_rot = params_.variable_max_stiffness.rotational;
- std::array vals = {msg->data[0], msg->data[1], msg->data[2],
- msg->data[3], msg->data[4], msg->data[5]};
+ std::array vals = {
+ msg->data[0], msg->data[1], msg->data[2], msg->data[3], msg->data[4], msg->data[5]};
for (int i = 0; i < 3; ++i) {
if (vals[i] < 0.0 || vals[i] > max_k_trans) {
- RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
- "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_trans);
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 100,
+ "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i,
+ vals[i],
+ max_k_trans);
vals[i] = std::clamp(vals[i], 0.0, max_k_trans);
}
}
for (int i = 3; i < 6; ++i) {
if (vals[i] < 0.0 || vals[i] > max_k_rot) {
- RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
- "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.", i, vals[i], max_k_rot);
+ RCLCPP_WARN_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 100,
+ "Topic stiffness[%d]=%.1f out of [0, %.1f], clamping.",
+ i,
+ vals[i],
+ max_k_rot);
vals[i] = std::clamp(vals[i], 0.0, max_k_rot);
}
}
topic_stiffness_.setZero();
topic_stiffness_.diagonal() << vals[0], vals[1], vals[2], vals[3], vals[4], vals[5];
use_topic_stiffness_ = true;
- RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 100,
+ RCLCPP_INFO_THROTTLE(
+ get_node()->get_logger(),
+ *get_node()->get_clock(),
+ 100,
"Variable stiffness received: [%.1f, %.1f, %.1f, %.1f, %.1f, %.1f]",
- vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]);
+ vals[0],
+ vals[1],
+ vals[2],
+ vals[3],
+ vals[4],
+ vals[5]);
}
void CartesianController::log_debug_info(const rclcpp::Time & time) {
diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp
index c09dc39..41d5435 100644
--- a/src/pose_broadcaster.cpp
+++ b/src/pose_broadcaster.cpp
@@ -50,8 +50,9 @@ PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & peri
#else
q[i] = state_interfaces_[i].get_value();
#endif
- if (continous_joint_types.count(
- joint.shortname())) { // Then we are handling a continous joint that is SO(2)
+ if (
+ continous_joint_types.count(
+ joint.shortname())) { // Then we are handling a continous joint that is SO(2)
q_pin[joint.idx_q()] = std::cos(q[i]);
q_pin[joint.idx_q() + 1] = std::sin(q[i]);
} else {
diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp
index 37c46ce..e987eb4 100644
--- a/src/twist_broadcaster.cpp
+++ b/src/twist_broadcaster.cpp
@@ -55,8 +55,9 @@ TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & per
q_dot[i] = state_interfaces_[i * 2 + 1].get_value();
#endif
- if (continous_joint_types.count(
- joint.shortname())) { // Then we are handling a continous joint that is SO(2)
+ if (
+ continous_joint_types.count(
+ joint.shortname())) { // Then we are handling a continous joint that is SO(2)
q_pin[joint.idx_q()] = std::cos(q[i]);
q_pin[joint.idx_q() + 1] = std::sin(q[i]);
q_dot_pin[joint.idx_v()] = q_dot[i];