diff --git a/src/iiwa_config/config/moveit/joint_limits.yaml b/src/iiwa_config/config/moveit/joint_limits.yaml index 0546fcf..f90f72c 100644 --- a/src/iiwa_config/config/moveit/joint_limits.yaml +++ b/src/iiwa_config/config/moveit/joint_limits.yaml @@ -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 \ No newline at end of file diff --git a/src/iiwa_controller_v2/external b/src/iiwa_controller_v2/external index 0f37e91..d642ea3 120000 --- a/src/iiwa_controller_v2/external +++ b/src/iiwa_controller_v2/external @@ -1 +1 @@ -/home/daniel/dev/kuka_iiwa7_ros2/src/iiwa_controller/external \ No newline at end of file +/home/daniel/dev/ros2_iiwa7/src/iiwa_controller/external \ No newline at end of file diff --git a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp index bd78a0e..eca7b01 100644 --- a/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp +++ b/src/iiwa_controller_v2/include/iiwa_controller_v2/system_interface.hpp @@ -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 export_unlisted_state_interface_descriptions() override; @@ -163,20 +159,16 @@ public: const std::vector & start_interfaces, const std::vector & 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. diff --git a/src/iiwa_controller_v2/src/system_interface.cpp b/src/iiwa_controller_v2/src/system_interface.cpp index f8382c0..0773761 100644 --- a/src/iiwa_controller_v2/src/system_interface.cpp +++ b/src/iiwa_controller_v2/src/system_interface.cpp @@ -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(snap.time_stamp_sec); diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index e5a69ad..f5a9175 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -11,8 +11,6 @@ - - @@ -163,8 +161,6 @@ $(arg open_loop) $(arg rt_prio) - $(arg external_torque_safety_check) - $(arg external_torque_limit)