grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

Update iiwa_controller and MoveIt configuration: Adjust joint velocity filter and parameters for improved performance

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-18 11:33:35 +10:00
parent ace4d02b8a
commit 04b5c99ec8
6 changed files with 54 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_config/config/moveit/iiwa_controller.yaml
+11 -11
View File
@@ -45,19 +45,19 @@ iiwa_arm_controller:
allow_partial_joints_goal: false
allow_nonzero_velocity_at_trajectory_end: false
state_publish_rate: 100.0
state_publish_rate: 50.0 # половина от update_rate, как в референсе lbr_fri_ros2_stack
action_monitor_rate: 20.0
# constraints:
# stopped_velocity_tolerance: 0.01
# goal_time: 1.0
# joint1: { trajectory: 0, goal: 0.01 }
# joint2: { trajectory: 0, goal: 0.01 }
# joint3: { trajectory: 0, goal: 0.01 }
# joint4: { trajectory: 0, goal: 0.01 }
# joint5: { trajectory: 0, goal: 0.01 }
# joint6: { trajectory: 0, goal: 0.01 }
# joint7: { trajectory: 0, goal: 0.01 }
constraints:
stopped_velocity_tolerance: 0.01 # [рад/с] — допуск скорости в конечной точке
goal_time: 2.0 # [с] — допуск на время достижения цели
joint1: { trajectory: 0.1, goal: 0.01 }
joint2: { trajectory: 0.1, goal: 0.01 }
joint3: { trajectory: 0.1, goal: 0.01 }
joint4: { trajectory: 0.1, goal: 0.01 }
joint5: { trajectory: 0.1, goal: 0.01 }
joint6: { trajectory: 0.1, goal: 0.01 }
joint7: { trajectory: 0.1, goal: 0.01 }
forward_position_controller:
ros__parameters:
src/iiwa_config/config/moveit/moveit_cpp.yaml
+9 -9
View File
@@ -16,14 +16,14 @@ plan_request_params:
max_velocity_scaling_factor: 0.5
max_acceleration_scaling_factor: 1.0
ompl_rrtc: # Namespace for individual plan request
plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem
planning_pipeline: ompl # Name of the pipeline that is being used
planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline
max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
ompl_rrtc:
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
planner_id: "RRTConnectkConfigDefault"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 1.0
pilz_ptp:
plan_request_params:
@@ -49,4 +49,4 @@ chomp_planner:
planning_pipeline: chomp
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 1.5
planning_time: 1.5
src/iiwa_config/config/setting.yaml
+2 -1
View File
@@ -4,7 +4,8 @@ robot:
port: 30200
command_mode: "position" # torque, position
fri_cycle_ms: 5 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
joint_position_tau: 0.04 # EMA фильтр позиций: 0 = выкл (без лага → без overshoot при торможении)
joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI
joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей
active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp
+6 -2
View File
@@ -61,6 +61,9 @@ private:
bool simulate_{false};
std::string cmd_mode_str_{"position"};
double joint_position_tau_{0.04};
// EMA-фильтр скорости: сглаживает одиночные выбросы конечных разностей.
// joint_velocity_tau = 0 отключает фильтр (raw finite difference).
double joint_velocity_tau_{0.01};
// Объекты FRI SDK
std::unique_ptr<FRIClient> fri_client_;
@@ -83,9 +86,10 @@ private:
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_pos_;
std::array<hardware_interface::CommandInterface::SharedPtr, N_JOINTS> h_cmd_eff_;
// Вычисление скорости конечными разностями
// Вычисление скорости: конечные разности + EMA-фильтр
std::array<double, N_JOINTS> prev_pos_{};
std::array<double, N_JOINTS> velocity_{};
std::array<double, N_JOINTS> velocity_{}; // отфильтрованная скорость, публикуется в JTC
std::array<double, N_JOINTS> velocity_raw_{}; // сырая скорость до фильтра
unsigned int last_ts_sec_{0};
unsigned int last_ts_nsec_{0};
bool velocity_initialized_{false};
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+24 -9
View File
@@ -43,18 +43,20 @@ CallbackReturn IIWAHardwareInterface::on_init(
const auto & info = params.hardware_info;
robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
simulate_ = (getParam(info, "simulate", "false") == "true");
cmd_mode_str_ = getParam(info, "command_mode", "position");
joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
robot_ip_ = getParam(info, "robot_ip", "192.170.10.2");
fri_port_ = std::stoi(getParam(info, "fri_port", "30200"));
simulate_ = (getParam(info, "simulate", "false") == "true");
cmd_mode_str_ = getParam(info, "command_mode", "position");
joint_position_tau_ = std::stod(getParam(info, "joint_position_tau", "0.04"));
joint_velocity_tau_ = std::stod(getParam(info, "joint_velocity_tau", "0.01"));
RCLCPP_INFO(rclcpp::get_logger(LOG),
"on_init: ip=%s port=%d simulate=%s mode=%s tau=%.3f",
"on_init: ip=%s port=%d simulate=%s mode=%s pos_tau=%.3f vel_tau=%.3f",
robot_ip_.c_str(), fri_port_,
simulate_ ? "true" : "false",
cmd_mode_str_.c_str(),
joint_position_tau_);
joint_position_tau_,
joint_velocity_tau_);
if (info.joints.size() != N_JOINTS) {
RCLCPP_FATAL(rclcpp::get_logger(LOG),
@@ -64,6 +66,7 @@ CallbackReturn IIWAHardwareInterface::on_init(
prev_pos_.fill(0.0);
velocity_.fill(0.0);
velocity_raw_.fill(0.0);
return CallbackReturn::SUCCESS;
}
@@ -231,7 +234,10 @@ void IIWAHardwareInterface::friThreadFunc()
}
// ── compute_velocity_ ──────────────────────────────────────────────────────────
// Конечные разности с int64-вычитанием для точности при больших Unix-timestamp'ах.
// Конечные разности + EMA-фильтр.
// int64-вычитание timestamp'ов предотвращает потерю точности при больших Unix-значениях.
// EMA-фильтр (joint_velocity_tau) убирает одиночные выбросы, которые видит JTC как
// скачки состояния и компенсирует агрессивными командами → хруст двигателей.
void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
{
@@ -240,6 +246,7 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
last_ts_sec_ = snap.time_stamp_sec;
last_ts_nsec_ = snap.time_stamp_nano_sec;
velocity_.fill(0.0);
velocity_raw_.fill(0.0);
velocity_initialized_ = true;
return;
}
@@ -260,10 +267,18 @@ void IIWAHardwareInterface::compute_velocity_(const IIWAStateSnapshot & snap)
static constexpr double kVelDeadband = 1e-4;
if (dt > 0.0) {
// EMA alpha для фильтра скорости: tau=0 → alpha=1 (без фильтра)
const double vel_alpha = (joint_velocity_tau_ > 0.0)
? dt / (joint_velocity_tau_ + dt)
: 1.0;
for (size_t i = 0; i < N_JOINTS; ++i) {
const double raw = (snap.measured_pos[i] - prev_pos_[i]) / dt;
const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
velocity_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
velocity_raw_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
// EMA: velocity_[i] = alpha * raw + (1 - alpha) * prev_filtered
velocity_[i] = vel_alpha * velocity_raw_[i] + (1.0 - vel_alpha) * velocity_[i];
}
}
src/iiwa_description/urdf/iiwa7.urdf.xacro
+2
View File
@@ -8,6 +8,7 @@
<xacro:arg name="fri_port" default="30200"/>
<xacro:arg name="command_mode" default="position"/>
<xacro:arg name="joint_position_tau" default="0.04"/>
<xacro:arg name="joint_velocity_tau" default="0.01"/>
<xacro:property name="initial_positions"
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -148,6 +149,7 @@
<param name="simulate">false</param>
<param name="command_mode">$(arg command_mode)</param>
<param name="joint_position_tau">$(arg joint_position_tau)</param>
<param name="joint_velocity_tau">$(arg joint_velocity_tau)</param>
</hardware>
<joint name="joint1">