diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index 99c7b9ea..cdd7ed21 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -5,6 +5,7 @@ 2.0.0 Advanced Python demos for the lbr_ros2_control. mhubii + cmower Apache License 2.0 lbr_description diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst index efe248ca..d1358852 100644 --- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst +++ b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst @@ -12,6 +12,21 @@ MoveIt via RViZ To run MoveIt via RViZ, simply follow: +Simulation +~~~~~~~~~~ +#. Run the robot driver: + + .. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + moveit:=true \ + sim:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. You can now move the robot via MoveIt in RViZ! + +Hardware +~~~~~~~~ #. Client side configurations: #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` @@ -30,13 +45,4 @@ To run MoveIt via RViZ, simply follow: - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Run the robot driver: - - .. code-block:: bash - - ros2 launch lbr_bringup bringup.launch.py \ - moveit:=true \ - sim:=false \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] - -#. You can now move the robot via MoveIt in RViZ! +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index 54965125..bf72faa3 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -126,20 +126,18 @@ class JointPIDArray { using pid_array_t = std::array; public: - JointPIDArray() = default; + JointPIDArray() = delete; + JointPIDArray(const PIDParameters &pid_parameters); void compute(const value_array_t &command_target, const value_array_t &state, const std::chrono::nanoseconds &dt, value_array_t &command); void compute(const value_array_t &command_target, const double *state, const std::chrono::nanoseconds &dt, value_array_t &command); - void initialize(const PIDParameters &pid_parameters, const double &dt); - inline const bool &is_initialized() const { return initialized_; }; - void log_info() const; protected: - bool initialized_{false}; /**< True if initialized.*/ - pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ + PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ + pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; } // end of namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp index d5f61579..93c061dc 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/base_command.hpp @@ -49,7 +49,6 @@ class BaseCommandInterface { protected: std::unique_ptr command_guard_; - PIDParameters pid_parameters_; JointPIDArray joint_position_pid_; idl_command_t command_, command_target_; }; diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index d900876b..f2e98052 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -43,6 +43,17 @@ void JointExponentialFilterArray::initialize(const double &cutoff_frequency, initialized_ = true; } +JointPIDArray::JointPIDArray(const PIDParameters &pid_parameters) + : pid_parameters_(pid_parameters) // keep local copy of parameters since + // controller_toolbox::Pid::getGains is not const correct + // (i.e. can't be called in this->log_info) +{ + std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { + pid.initPid(pid_parameters_.p, pid_parameters_.i, pid_parameters_.d, pid_parameters_.i_max, + pid_parameters_.i_min, pid_parameters_.antiwindup); + }); +} + void JointPIDArray::compute(const value_array_t &command_target, const value_array_t &state, const std::chrono::nanoseconds &dt, value_array_t &command) { std::for_each(command.begin(), command.end(), [&, i = 0](double &command_i) mutable { @@ -59,11 +70,14 @@ void JointPIDArray::compute(const value_array_t &command_target, const double *s }); } -void JointPIDArray::initialize(const PIDParameters &pid_parameters, const double &dt) { - std::for_each(pid_controllers_.begin(), pid_controllers_.end(), [&](auto &pid) { - pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt, - pid_parameters.i_max * dt, pid_parameters.i_min * dt, pid_parameters.antiwindup); - }); - initialized_ = true; -} +void JointPIDArray::log_info() const { + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "*** Parameters:"); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* p: %.1f", pid_parameters_.p); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i: %.1f", pid_parameters_.i); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* d: %.1f", pid_parameters_.d); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_max: %.1f", pid_parameters_.i_max); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* i_min: %.1f", pid_parameters_.i_min); + RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", + pid_parameters_.antiwindup ? "true" : "false"); +}; } // end of namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/base_command.cpp b/lbr_fri_ros2/src/interfaces/base_command.cpp index c4cc5ea9..f53bddeb 100644 --- a/lbr_fri_ros2/src/interfaces/base_command.cpp +++ b/lbr_fri_ros2/src/interfaces/base_command.cpp @@ -5,7 +5,7 @@ namespace lbr_fri_ros2 { BaseCommandInterface::BaseCommandInterface(const PIDParameters &pid_parameters, const CommandGuardParameters &command_guard_parameters, const std::string &command_guard_variant) - : pid_parameters_(pid_parameters) { + : joint_position_pid_(pid_parameters) { command_guard_ = command_guard_factory(command_guard_parameters, command_guard_variant); }; @@ -18,13 +18,6 @@ void BaseCommandInterface::init_command(const_idl_state_t_ref state) { void BaseCommandInterface::log_info() const { command_guard_->log_info(); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "*** Parameters:"); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.p: %.1f", pid_parameters_.p); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i: %.1f", pid_parameters_.i); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.d: %.1f", pid_parameters_.d); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_max: %.1f", pid_parameters_.i_max); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.i_min: %.1f", pid_parameters_.i_min); - RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "* pid.antiwindup: %s", - pid_parameters_.antiwindup ? "true" : "false"); + joint_position_pid_.log_info(); } } // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 0a81bfe6..26287bdf 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -41,9 +41,6 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 5228a443..f731fc9b 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -31,9 +31,6 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index ee34c25b..9ab34524 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -29,9 +29,6 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, } // PID - if (!joint_position_pid_.is_initialized()) { - joint_position_pid_.initialize(pid_parameters_, state.sample_time); - } joint_position_pid_.compute( command_target_.joint_position, state.measured_joint_position, std::chrono::nanoseconds(static_cast(state.sample_time * 1.e9)), diff --git a/lbr_moveit_config/doc/lbr_moveit_config.rst b/lbr_moveit_config/doc/lbr_moveit_config.rst index 24507c21..3484e3a1 100644 --- a/lbr_moveit_config/doc/lbr_moveit_config.rst +++ b/lbr_moveit_config/doc/lbr_moveit_config.rst @@ -12,7 +12,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 sudo apt install ros-$ROS_DISTRO-moveit* -#. Make sure the ``lbr_fri_ros2_stack`` is installed and **sourced**, see :ref:`Installation`. +#. Make sure the ``lbr-stack`` is installed and **sourced**, see :ref:`Installation`. #. Launch the setup assistant @@ -20,7 +20,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 ros2 launch moveit_setup_assistant setup_assistant.launch.py -#. .. dropdown:: ``Load Files``: E.g. ``lbr_fri_ros2_stack_ws/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.xacro`` +#. .. dropdown:: ``Load Files``: E.g. ``lbr-stack/install/lbr_description/share/lbr_description/urdf/iiwa7/iiwa7.xacro`` .. thumbnail:: img/00_start_screen.png @@ -94,7 +94,7 @@ This procedure applies to all LBRs: ``iiwa7``, ``iiwa14``, ``med7``, and ``med14 Update MoveIt Configuration --------------------------- -#. Make sure the ``lbr_fri_ros2_stack`` is installed and sourced, see :ref:`Installation`. +#. Make sure the ``lbr-stack`` is installed and sourced, see :ref:`Installation`. #. Run the setup assistant for the existing configuration. diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index 91cbaeb6..4ae4cbf2 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -7,7 +7,7 @@ hardware: port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups 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 - pid_p: 10.0 # P gain for the joint position (useful for asynchronous control) + pid_p: 0.1 # P gain for the joint position (useful for asynchronous control) pid_i: 0.0 # I gain for the joint position command pid_d: 0.0 # D gain for the joint position command pid_i_max: 0.0 # max integral value for the joint position command