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

Enhance joint limits and add acceleration scaling to MoveToNamedPose service

Browse Source
This commit is contained in:
Даниил Грабарь
2026-05-15 07:20:26 +03:00
parent 99215c0b20
commit 86cf252a87
12 changed files with 40 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_config/config/moveit/joint_limits.yaml
+17 -10
View File
@@ -16,7 +16,8 @@ joint_limits:
max_velocity: 1.71
has_acceleration_limits: true
max_acceleration: 8.5521
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 85.0
joint2:
has_position_limits: true
min_position: -2.09
@@ -25,7 +26,8 @@ joint_limits:
max_velocity: 1.71
has_acceleration_limits: true
max_acceleration: 8.5521
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 85.0
joint3:
has_position_limits: true
min_position: -2.97
@@ -34,7 +36,8 @@ joint_limits:
max_velocity: 1.75
has_acceleration_limits: true
max_acceleration: 8.7266
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 87.0
joint4:
has_position_limits: true
min_position: -2.09
@@ -43,32 +46,36 @@ joint_limits:
max_velocity: 2.27
has_acceleration_limits: true
max_acceleration: 11.3446
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 113.0
joint5:
has_position_limits: true
min_position: -2.97
max_position: 2.97
has_velocity_limits: true
max_velocity: 2.4399999999999999
max_velocity: 2.44
has_acceleration_limits: true
max_acceleration: 12.2173
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 122.0
joint6:
has_position_limits: true
min_position: -2.09
max_position: 2.09
has_velocity_limits: true
max_velocity: 3.1400000000000001
max_velocity: 3.14
has_acceleration_limits: true
max_acceleration: 15.7080
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 157.0
joint7:
has_position_limits: true
min_position: -3.05
max_position: 3.05
has_velocity_limits: true
max_velocity: 3.1400000000000001
max_velocity: 3.14
has_acceleration_limits: true
max_acceleration: 15.7080
has_jerk_limits: false
has_jerk_limits: true
max_jerk: 157.0
src/iiwa_controller_v2/external
+1 -1
View File
@@ -1 +1 @@
/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_controller/external
/home/daniel/dev/ros2_iiwa7/src/iiwa_controller/external
src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp
+5 -16
View File
@@ -36,8 +36,6 @@ protected:
double joint_position_tau{0.04};
bool open_loop{true}; // JTC sees filtered_pos, not measured_pos
int rt_prio{80};
bool external_torque_safety_check{true};
double external_torque_limit{2.0}; // [Nm] per joint, safety threshold
};
// ── Command interface handles ───────────────────────────────────────────────
@@ -151,11 +149,9 @@ public:
SystemInterface() = default;
// ── Lifecycle ───────────────────────────────────────────────────────────────
controller_interface::CallbackReturn on_init(
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareComponentInterfaceParams & params) override;
// Per-joint: external_torque, commanded_torque, ipo_joint_position
// Auxiliary: sample_time, session_state, connection_quality, time_stamp_*
std::vector<hardware_interface::InterfaceDescription>
export_unlisted_state_interface_descriptions() override;
@@ -163,20 +159,16 @@ public:
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces) override;
// on_configure: opens the UDP socket (FRI does not need the robot yet)
controller_interface::CallbackReturn on_configure(
hardware_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state) override;
// on_activate: starts the FRI thread, waits for COMMANDING_WAIT
controller_interface::CallbackReturn on_activate(
hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state) override;
// on_deactivate: stops the FRI thread
controller_interface::CallbackReturn on_deactivate(
hardware_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state) override;
// on_cleanup: closes the UDP socket
controller_interface::CallbackReturn on_cleanup(
hardware_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state) override;
hardware_interface::return_type read(
@@ -192,9 +184,6 @@ protected:
bool exit_commanding_active_(KUKA::FRI::ESessionState previous,
KUKA::FRI::ESessionState current);
// Safety check: abort if any external torque exceeds the configured limit.
bool external_torque_safe_(const IIWAStateSnapshot & snap) const;
void friThreadFunc();
// Compute finite-difference velocity from FRI timestamps.
src/iiwa_controller_v2/src/system_interface.cpp
-20
View File
@@ -288,12 +288,6 @@ hardware_interface::return_type SystemInterface::read(
}
previous_session_state_ = current_state;
// External torque safety check
if (parameters_.external_torque_safety_check && !external_torque_safe_(snap)) {
RCLCPP_ERROR(rclcpp::get_logger(LOG),
"External torque exceeded safety limit (%.1f Nm). Stopping.", parameters_.external_torque_limit);
return hardware_interface::return_type::ERROR;
}
compute_velocity_(snap);
state_if_handles_.push(snap, velocity_);
@@ -343,10 +337,6 @@ bool SystemInterface::parse_parameters_()
parameters_.joint_position_tau = std::stod(getParam(info, "joint_position_tau", "0.04"));
parameters_.open_loop = (getParam(info, "open_loop", "true") == "true");
parameters_.rt_prio = std::stoi(getParam(info, "rt_prio", "80"));
parameters_.external_torque_safety_check =
(getParam(info, "external_torque_safety_check", "true") == "true");
parameters_.external_torque_limit =
std::stod(getParam(info, "external_torque_limit", "2.0"));
if (parameters_.fri_port < 30200 || parameters_.fri_port > 30209) {
RCLCPP_ERROR(rclcpp::get_logger(LOG),
@@ -366,16 +356,6 @@ bool SystemInterface::exit_commanding_active_(
return previous == KUKA::FRI::COMMANDING_ACTIVE && current != KUKA::FRI::COMMANDING_ACTIVE;
}
bool SystemInterface::external_torque_safe_(const IIWAStateSnapshot & snap) const
{
for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
if (std::abs(snap.external_tau[i]) > parameters_.external_torque_limit) {
return false;
}
}
return true;
}
void SystemInterface::compute_velocity_(const IIWAStateSnapshot & snap)
{
const double ts_sec = static_cast<double>(snap.time_stamp_sec);
src/iiwa_description/urdf/iiwa7.urdf.xacro
-4
View File
@@ -11,8 +11,6 @@
<!-- iiwa_controller_v2: новые параметры. Убери эти строки при откате на v1 -->
<xacro:arg name="open_loop" default="true"/>
<xacro:arg name="rt_prio" default="80"/>
<xacro:arg name="external_torque_safety_check" default="true"/>
<xacro:arg name="external_torque_limit" default="2.0"/>
<xacro:property name="initial_positions"
value="${xacro.load_yaml('$(arg initial_positions_file)')['initial_positions']}"/>
@@ -163,8 +161,6 @@
<!-- новые параметры v2: -->
<param name="open_loop">$(arg open_loop)</param>
<param name="rt_prio">$(arg rt_prio)</param>
<param name="external_torque_safety_check">$(arg external_torque_safety_check)</param>
<param name="external_torque_limit">$(arg external_torque_limit)</param>
<!-- ── v1 (закомментировано) ─────────────────────────────
<plugin>iiwa_controller/IIWAHardwareInterface</plugin>
src/iiwa_msgs/srv/MoveToNamedPose.srv
+2
View File
@@ -2,6 +2,8 @@
string name
# Velocity scaling 0.0–1.0
float32 speed
# Acceleration scaling 0.0–1.0 (0.0 = same as speed)
float32 accel_scale
---
bool success
string message
src/iiwa_planning/scripts/move_to_pose_server.py
+15 -6
View File
@@ -235,8 +235,12 @@ class IiwaMotionServer(Node):
def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
name = request.name.strip()
velocity_scale = max(0.01, min(1.0, float(request.speed)))
raw_accel = float(request.accel_scale)
accel_scale = max(0.01, min(1.0, raw_accel)) if raw_accel > 0.0 else velocity_scale
self.get_logger().info(f"[named] name='{name}' speed={velocity_scale:.2f}")
self.get_logger().info(
f"[named] name='{name}' speed={velocity_scale:.2f} accel={accel_scale:.2f}"
)
self._arm.set_start_state_to_current_state()
try:
@@ -248,7 +252,7 @@ class IiwaMotionServer(Node):
return response
plan_params = self._make_plan_params(
"pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale
"pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
)
plan_result = self._arm.plan(single_plan_parameters=plan_params)
if not plan_result:
@@ -257,10 +261,15 @@ class IiwaMotionServer(Node):
self.get_logger().error(response.message)
return response
self._moveit.execute(plan_result.trajectory, controllers=[])
response.success = True
response.message = f"Переместился в '{name}'"
self.get_logger().info(response.message)
exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
if exec_result:
response.success = True
response.message = f"Переместился в '{name}'"
self.get_logger().info(response.message)
else:
response.success = False
response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
self.get_logger().error(response.message)
return response
def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
src/realsense2_camera/AMENT_IGNORE
View File
src/realsense2_camera_msgs/AMENT_IGNORE
View File
src/realsense2_description/AMENT_IGNORE
View File
src/realsense2_rgbd_plugin/AMENT_IGNORE
View File
src/realsense2_ros_mqtt_bridge/AMENT_IGNORE
View File