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

Update iiwa_controller configuration for improved joint control and velocity clamping

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-18 03:34:59 +03:00
parent 86cf252a87
commit efaec9b442
5 changed files with 45 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_config/config/moveit/iiwa_controller.yaml
+1 -1
View File
@@ -39,7 +39,7 @@ iiwa_arm_controller:
- position
- velocity
interpolate_from_desired_state: false
interpolate_from_desired_state: true
allow_partial_joints_goal: false
allow_nonzero_velocity_at_trajectory_end: false
src/iiwa_config/config/setting.yaml
+1 -1
View File
@@ -4,7 +4,7 @@ robot:
port: 30200
command_mode: "position" # torque, position
fri_cycle_ms: 5 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
joint_position_tau: 0.04 # постоянная времени фильтра позиций [с]: больше → плавнее, медленнее
joint_position_tau: 0.15 # EMA фильтр позиций: 0 = выкл (без лага → без overshoot при торможении)
active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController
description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
src/iiwa_controller/src/IIWAHardwareInterface.cpp
+10 -1
View File
@@ -1,6 +1,7 @@
#include "iiwa_controller/IIWAHardwareInterface.hpp"
#include <chrono>
#include <cmath>
#include <thread>
#include "hardware_interface/hardware_info.hpp"
@@ -238,9 +239,17 @@ hardware_interface::return_type IIWAHardwareInterface::read(
const double dt =
(static_cast<double>(snap.time_stamp_sec) - static_cast<double>(last_ts_sec_)) +
(static_cast<double>(snap.time_stamp_nano_sec) - static_cast<double>(last_ts_nsec_)) * 1e-9;
// iiwa7 physical velocity limits [rad/s], used to clamp impossible spikes
static constexpr std::array<double, N_JOINTS> kMaxVel =
{1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
static constexpr double kVelDeadband = 1e-4;
if (dt > 0.0) {
for (size_t i = 0; i < N_JOINTS; ++i) {
vel_filtered_[i] = (snap.measured_pos[i] - prev_pos_[i]) / dt;
const double raw = (snap.measured_pos[i] - prev_pos_[i]) / dt;
const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
vel_filtered_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
}
}
for (size_t i = 0; i < N_JOINTS; ++i) {
src/iiwa_controller_v2/src/system_interface.cpp
+16 -2
View File
@@ -2,6 +2,7 @@
#include <chrono>
#include <cmath>
#include <cstdint>
#include <thread>
#include "hardware_interface/types/hardware_component_interface_params.hpp"
@@ -375,10 +376,23 @@ void SystemInterface::compute_velocity_(const IIWAStateSnapshot & snap)
return;
}
const double dt = (ts_sec + ts_nsec * 1e-9) - (last_ts_sec_ + last_ts_nsec_ * 1e-9);
// Use integer subtraction to avoid floating-point precision loss with large Unix timestamps
const double dt =
static_cast<double>(static_cast<int64_t>(snap.time_stamp_sec) -
static_cast<int64_t>(static_cast<unsigned int>(last_ts_sec_))) +
(ts_nsec - last_ts_nsec_) * 1e-9;
// iiwa7 max joint velocity [rad/s], used to clamp impossible spikes
static constexpr std::array<double, FRIClient::N_JOINTS> kMaxVel =
{1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14};
static constexpr double kVelDeadband = 1e-4; // zero out near-stop residuals
if (dt > 0.0) {
for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
velocity_[i] = (snap.measured_pos[i] - last_pos_[i]) / dt;
const double raw = (snap.measured_pos[i] - last_pos_[i]) / dt;
// Clamp to physical limit and apply zero deadband
const double clamped = std::clamp(raw, -kMaxVel[i], kMaxVel[i]);
velocity_[i] = (std::abs(clamped) < kVelDeadband) ? 0.0 : clamped;
}
}
src/iiwa_description/urdf/iiwa7.urdf.xacro
+17 -10
View File
@@ -9,8 +9,8 @@
<xacro:arg name="command_mode" default="position"/>
<xacro:arg name="joint_position_tau" default="0.04"/>
<!-- iiwa_controller_v2: новые параметры. Убери эти строки при откате на v1 -->
<xacro:arg name="open_loop" default="true"/>
<xacro:arg name="rt_prio" default="80"/>
<!-- <xacro:arg name="open_loop" default="true"/>
<xacro:arg name="rt_prio" default="80"/> -->
<xacro:property name="initial_positions"
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -152,15 +152,15 @@
════════════════════════════════════════════════════════ -->
<!-- ── v2 (активен) ──────────────────────────────────────── -->
<plugin>iiwa_controller_v2/SystemInterface</plugin>
<param name="robot_ip">$(arg robot_ip)</param>
<param name="fri_port">$(arg fri_port)</param>
<param name="simulate">false</param>
<param name="command_mode">$(arg command_mode)</param>
<param name="joint_position_tau">$(arg joint_position_tau)</param>
<!-- <plugin>iiwa_controller_v2/SystemInterface</plugin> -->
<!-- <param name="robot_ip">$(arg robot_ip)</param> -->
<!-- <param name="fri_port">$(arg fri_port)</param> -->
<!-- <param name="simulate">false</param> -->
<!-- <param name="command_mode">$(arg command_mode)</param> -->
<!-- <param name="joint_position_tau">$(arg joint_position_tau)</param> -->
<!-- новые параметры v2: -->
<param name="open_loop">$(arg open_loop)</param>
<param name="rt_prio">$(arg rt_prio)</param>
<!-- <param name="open_loop">$(arg open_loop)</param> -->
<!-- <param name="rt_prio">$(arg rt_prio)</param> -->
<!-- ── v1 (закомментировано) ─────────────────────────────
<plugin>iiwa_controller/IIWAHardwareInterface</plugin>
@@ -170,6 +170,13 @@
<param name="command_mode">$(arg command_mode)</param>
<param name="joint_position_tau">$(arg joint_position_tau)</param>
─────────────────────────────────────────────────────────── -->
<plugin>iiwa_controller/IIWAHardwareInterface</plugin>
<param name="robot_ip">$(arg robot_ip)</param>
<param name="fri_port">$(arg fri_port)</param>
<param name="simulate">false</param>
<param name="command_mode">$(arg command_mode)</param>
<param name="joint_position_tau">$(arg joint_position_tau)</param>
</hardware>
<joint name="joint1">