diff --git a/lbr_description/ros2_control/lbr_system_config.yaml b/lbr_description/ros2_control/lbr_system_config.yaml index 604d1239..98880ad5 100644 --- a/lbr_description/ros2_control/lbr_system_config.yaml +++ b/lbr_description/ros2_control/lbr_system_config.yaml @@ -8,20 +8,21 @@ hardware: remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection rt_prio: 80 # real-time priority for the control loop # exponential moving average time constant for joint position commands [s]: - # Set tau > robot sample time for more smoothing on the commands - # Set tau << robot sample time for no smoothing on the commands + # set tau > robot sample time for more smoothing on the commands + # set tau << robot sample time for no smoothing on the commands joint_position_tau: 0.04 command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] # exponential moving average time constant for external joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the external torque measurements - # Set tau << robot sample time for no smoothing on the external torque measurements + # set tau > robot sample time for more smoothing on the external torque measurements + # set tau << robot sample time for no smoothing on the external torque measurements external_torque_tau: 0.04 # exponential moving average time constant for joint torque measurements [s]: - # Set tau > robot sample time for more smoothing on the raw torque measurements - # Set tau << robot sample time for no smoothing on the raw torque measurements + # set tau > robot sample time for more smoothing on the raw torque measurements + # set tau << robot sample time for no smoothing on the raw torque measurements measured_torque_tau: 0.04 - open_loop: true # KUKA works the best in open_loop control mode - + # for position control, LBR works best in open_loop control mode + # note that open_loop is overridden to false if LBR is in impedance or Cartesian impedance control mode + open_loop: true estimated_ft_sensor: # estimates the external force-torque from the external joint torque values enabled: true # whether to enable the force-torque estimation update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate) diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 7d11414c..0a2f4ea2 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -67,6 +67,23 @@ void AsyncClient::onStateChange(KUKA::FRI::ESessionState old_state, // initialize command state_interface_ptr_->set_state(robotState()); command_interface_ptr_->init_command(state_interface_ptr_->get_state()); + + // if robot is in impedance or Cartesian impedance control mode, override open_loop_ to false + // also refer to https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226 + auto control_mode = robotState().getControlMode(); + if (control_mode == KUKA::FRI::EControlMode::JOINT_IMP_CONTROL_MODE || + control_mode == KUKA::FRI::EControlMode::CART_IMP_CONTROL_MODE) { + if (open_loop_) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME), + "Overriding open loop from '" + << (open_loop_ ? "true" : "false") + << "' to 'false' since LBR is in control mode '" + << EnumMaps::control_mode_map(control_mode) + << "'. Please refer to " + "[https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/226]."); + open_loop_ = false; + } + } } void AsyncClient::monitor() { state_interface_ptr_->set_state(robotState()); }; diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index ebab71e4..21adfe2b 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -399,10 +399,6 @@ bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo & info_.hardware_parameters["open_loop"].begin(), ::tolower); // convert to lower case parameters_.open_loop = info_.hardware_parameters["open_loop"] == "true"; - std::transform(info_.hardware_parameters["pid_antiwindup"].begin(), - info_.hardware_parameters["pid_antiwindup"].end(), - info_.hardware_parameters["pid_antiwindup"].begin(), - ::tolower); // convert to lower case parameters_.joint_position_tau = std::stod(info_.hardware_parameters["joint_position_tau"]); parameters_.command_guard_variant = system_info.hardware_parameters.at("command_guard_variant"); parameters_.external_torque_tau = std::stod(info_.hardware_parameters["external_torque_tau"]);