From 684f95dc75e5212e70e1969e3345ad715fafa7d3 Mon Sep 17 00:00:00 2001 From: Luca Beber Date: Wed, 12 Jun 2024 10:31:28 +0200 Subject: [PATCH 1/4] chore: Add motion control handle to launch and config files --- lbr_bringup/launch/real.launch.py | 4 + lbr_bringup/launch_mixins/lbr_ros2_control.py | 7 +- lbr_description/gazebo/lbr_gazebo.xacro | 3 + lbr_ros2_control/config/lbr_controllers.yaml | 96 +++++++++++++++++++ 4 files changed, 109 insertions(+), 1 deletion(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 6e7ac7f1..b55f3557 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -41,6 +41,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) + lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( + controller="motion_control_handle" + ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -53,6 +56,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: force_torque_broadcaster, lbr_state_broadcaster, controller, + lbr_motion_control_handle, ], ) ) diff --git a/lbr_bringup/launch_mixins/lbr_ros2_control.py b/lbr_bringup/launch_mixins/lbr_ros2_control.py index fd270768..fe3642aa 100644 --- a/lbr_bringup/launch_mixins/lbr_ros2_control.py +++ b/lbr_bringup/launch_mixins/lbr_ros2_control.py @@ -35,9 +35,11 @@ def arg_ctrl() -> DeclareLaunchArgument: "lbr_joint_position_command_controller", "lbr_torque_command_controller", "lbr_wrench_command_controller", + "cartesian_impedance_controller", + "gravity_compensation", ], ) - + @staticmethod def arg_use_sim_time() -> DeclareLaunchArgument: return DeclareLaunchArgument( @@ -74,6 +76,9 @@ def node_ros2_control( namespace=robot_name, remappings=[ ("~/robot_description", "robot_description"), + ("cartesian_impedance_controller/target_frame", "target_frame"), + ("cartesian_impedance_controller/target_wrench", "target_wrench"), + ("motion_control_handle/target_frame", "target_frame"), ], **kwargs, ) diff --git a/lbr_description/gazebo/lbr_gazebo.xacro b/lbr_description/gazebo/lbr_gazebo.xacro index 49495d90..4950f225 100644 --- a/lbr_description/gazebo/lbr_gazebo.xacro +++ b/lbr_description/gazebo/lbr_gazebo.xacro @@ -10,6 +10,9 @@ /${robot_name} ~/robot_description:=robot_description + cartesian_impedance_controller/target_frame:=target_frame + cartesian_impedance_controller/target_wrench:=target_wrench + motion_control_handle/target_frame:=target_frame diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index a9e70793..5737d1c6 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -32,6 +32,15 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController + cartesian_impedance_controller: + type: cartesian_impedance_controller/CartesianImpedanceController + + gravity_compensation: + type: gravity_compensation/GravityCompensation + + motion_control_handle: + type: cartesian_controller_handles/MotionControlHandle + /**/force_torque_broadcaster: ros__parameters: frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 @@ -66,3 +75,90 @@ - A6 - A7 interface_name: position + +/**/cartesian_impedance_controller: + ros__parameters: + + # This is the tip of the robot tool that you usually use for your task. + # For instance, it could be the drilling bit of a screwdriver or a grinding + # tool. When you specify a target_wrench, i.e. some additional forces that + # your robot should apply to its environment, that target_wrench gets + # applied in this frame. + end_effector_link: "link_ee" + + # This is usually the link directly before the first actuated joint. All + # controllers will build a kinematic chain from this link up to + # end_effector_link. It's also the reference frame for the superposition + # of error components in all controllers. + robot_base_link: "link_0" + + # This is the URDF link of your sensor. Sensor signals are assumed to be + # given in this frame. It's important that this link is located somewhere + # between end_effector_link and robot_base_link. If that's not the case, + # the controllers won't initialize and will emit an error message. + ft_sensor_ref_link: "link_ee" + + # This is the link that the robot feels compliant about. It does not need + # to coincide with the end_effector_link, but for many use cases, this + # configuration is handy. When working with a screwdriver, for instance, + # setting compliance_ref_link == end_effector_link makes it easy to specify + # downward pushing forces without generating unwanted offset moments. + # On the other hand, an application could benefit from yielding a little in + # the robot's wrist while drawing a line on a surface with a pen. + compliance_ref_link: "link_ee" + + command_interfaces: + - effort + state_interfaces: + - position + - velocity + joints: + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 + delta_tau_max: 1.0 # Nm + stiffness: # w.r.t. compliance_ref_link coordinates + trans_x: 200.0 + trans_y: 200.0 + trans_z: 200.0 + rot_x: 20.0 + rot_y: 20.0 + rot_z: 20.0 + kuka: true +/**/motion_control_handle: + ros__parameters: + end_effector_link: "link_ee" + robot_base_link: "link_0" + ft_sensor_ref_link: "link_ee" + joints: + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 + +/**/gravity_compensation: + ros__parameters: + end_effector_link: "link_ee" + robot_base_link: "link_0" + ft_sensor_ref_link: "link_ee" + command_interfaces: + - effort + state_interfaces: + - position + - velocity + joints: + - A1 + - A2 + - A3 + - A4 + - A5 + - A6 + - A7 + kuka: true \ No newline at end of file From d17311c0c35ba9bfc2a1ffc6b4321b0478430fbe Mon Sep 17 00:00:00 2001 From: Luca Beber Date: Wed, 12 Jun 2024 10:36:06 +0200 Subject: [PATCH 2/4] chore: Update launch and config files with motion control handle and robot description --- lbr_bringup/launch/real.launch.py | 12 +++++++----- lbr_bringup/launch/system_interface.launch.py | 4 +++- lbr_bringup/launch_mixins/lbr_ros2_control.py | 2 ++ lbr_ros2_control/config/lbr_controllers.yaml | 2 +- lbr_ros2_control/config/lbr_system_parameters.yaml | 6 +++--- 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index b55f3557..c498fe63 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -28,7 +28,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld.add_action(robot_state_publisher) # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control() + ros2_control_node = LBRROS2ControlMixin.node_ros2_control( + robot_description=robot_description, + ) ld.add_action(ros2_control_node) # joint state broad caster and controller on ros2 control node start @@ -41,9 +43,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( - controller="motion_control_handle" - ) + # lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( + # controller="motion_control_handle" + # ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -56,7 +58,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: force_torque_broadcaster, lbr_state_broadcaster, controller, - lbr_motion_control_handle, + # lbr_motion_control_handle, ], ) ) diff --git a/lbr_bringup/launch/system_interface.launch.py b/lbr_bringup/launch/system_interface.launch.py index efc53910..e3aab2ae 100644 --- a/lbr_bringup/launch/system_interface.launch.py +++ b/lbr_bringup/launch/system_interface.launch.py @@ -24,7 +24,9 @@ def generate_launch_description() -> LaunchDescription: use_sim_time=False, ) ld.add_action(robot_state_publisher) - ros2_control_node = LBRSystemInterface.node_ros2_control() + ros2_control_node = LBRSystemInterface.node_ros2_control( + robot_description=robot_description, + ) ld.add_action(ros2_control_node) joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( controller="joint_state_broadcaster" diff --git a/lbr_bringup/launch_mixins/lbr_ros2_control.py b/lbr_bringup/launch_mixins/lbr_ros2_control.py index fe3642aa..020a5bc7 100644 --- a/lbr_bringup/launch_mixins/lbr_ros2_control.py +++ b/lbr_bringup/launch_mixins/lbr_ros2_control.py @@ -50,6 +50,7 @@ def arg_use_sim_time() -> DeclareLaunchArgument: @staticmethod def node_ros2_control( + robot_description: Dict[str, str], robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), @@ -59,6 +60,7 @@ def node_ros2_control( package="controller_manager", executable="ros2_control_node", parameters=[ + robot_description, {"use_sim_time": False}, PathJoinSubstitution( [ diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 5737d1c6..0d7a0b62 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 100 + update_rate: 500 # ROS 2 control broadcasters joint_state_broadcaster: diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml index 91cbaeb6..46f528e0 100644 --- a/lbr_ros2_control/config/lbr_system_parameters.yaml +++ b/lbr_ros2_control/config/lbr_system_parameters.yaml @@ -2,8 +2,8 @@ hardware: fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro major_version: 1 - minor_version: 15 - client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench] + minor_version: 17 + client_command_mode: torque # the command mode specifies the user-sent commands. Available: [position, torque, wrench] 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 @@ -16,7 +16,7 @@ hardware: command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop] external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz] - open_loop: true # KUKA works the best in open_loop control mode + open_loop: false # KUKA works the best in open_loop control mode estimated_ft_sensor: # estimates the external force-torque from the external joint torque values chain_root: link_0 From 6b63858481d03157581a83d84f6e1b1a4761fb4a Mon Sep 17 00:00:00 2001 From: Luca Beber Date: Wed, 12 Jun 2024 10:40:53 +0200 Subject: [PATCH 3/4] chore: Update repository URLs to use idra-lab organization --- README.md | 2 +- lbr_fri_ros2_stack/repos-fri-1.15.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index b9cbd2d4..a51f0143 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io source /opt/ros/humble/setup.bash export FRI_CLIENT_VERSION=1.15 mkdir -p lbr-stack/src && cd lbr-stack - vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml + vcs import src --input https://raw.githubusercontent.com/idra-lab/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml rosdep install --from-paths src -i -r -y ``` diff --git a/lbr_fri_ros2_stack/repos-fri-1.15.yaml b/lbr_fri_ros2_stack/repos-fri-1.15.yaml index 5e7f40fe..de573d76 100644 --- a/lbr_fri_ros2_stack/repos-fri-1.15.yaml +++ b/lbr_fri_ros2_stack/repos-fri-1.15.yaml @@ -9,5 +9,5 @@ repositories: version: fri-1 lbr_fri_ros2_stack: type: git - url: https://github.com/lbr-stack/lbr_fri_ros2_stack + url: https://github.com/idra-lab/lbr_fri_ros2_stack version: humble From adb7f2d3506949aa4f972f877e15c2c8fb1bfa17 Mon Sep 17 00:00:00 2001 From: Luca Beber Date: Mon, 25 Nov 2024 15:36:17 +0100 Subject: [PATCH 4/4] working branch --- lbr_bringup/config/config.rviz | 14 ++++++++-- lbr_bringup/launch/real.launch.py | 8 +++--- .../lbr_demos_cpp/src/torque_sine_overlay.cpp | 2 +- .../urdf/iiwa14/iiwa14_description.xacro | 11 +++++++- .../src/interfaces/torque_command.cpp | 18 +++++++------ lbr_ros2_control/config/lbr_controllers.yaml | 26 +++++++++---------- .../src/lbr_torque_command_controller.cpp | 8 ++++++ 7 files changed, 58 insertions(+), 29 deletions(-) diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/config.rviz index b3a3df75..c2e77ab4 100644 --- a/lbr_bringup/config/config.rviz +++ b/lbr_bringup/config/config.rviz @@ -9,6 +9,7 @@ Panels: - /RobotModel1 - /RobotModel1/Description Topic1 - /Wrench1 + - /InteractiveMarkers1 Splitter Ratio: 0.40760868787765503 Tree Height: 724 - Class: rviz_common/Selection @@ -143,6 +144,15 @@ Visualization Manager: Torque Arrow Scale: 0.30000001192092896 Torque Color: 170; 85; 255 Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /lbr/motion_control_handle + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true Enabled: true Global Options: Background Color: 21; 21; 26 @@ -225,5 +235,5 @@ Window Geometry: Views: collapsed: false Width: 1850 - X: 1080 - Y: 450 + X: 710 + Y: 382 diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index c498fe63..957e59bc 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -43,9 +43,9 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( controller="lbr_state_broadcaster" ) - # lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( - # controller="motion_control_handle" - # ) + lbr_motion_control_handle = LBRROS2ControlMixin.node_controller_spawner( + controller="motion_control_handle" + ) controller = LBRROS2ControlMixin.node_controller_spawner( controller=LaunchConfiguration("ctrl") ) @@ -57,8 +57,8 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: joint_state_broadcaster, force_torque_broadcaster, lbr_state_broadcaster, - controller, # lbr_motion_control_handle, + controller, ], ) ) diff --git a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp index 9e700ffe..a640affd 100644 --- a/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp +++ b/lbr_demos/lbr_demos_cpp/src/torque_sine_overlay.cpp @@ -14,7 +14,7 @@ #include "lbr_fri_ros2/utils.hpp" class TorqueSineOverlay { - static constexpr double amplitude_ = 15.; // Nm + static constexpr double amplitude_ = 0.5; // Nm static constexpr double frequency_ = 0.25; // Hz public: diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 7e63d77f..075111d9 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -292,5 +292,14 @@ + + + + + + + + + - \ No newline at end of file + diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index 5228a443..86dc6a72 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -31,15 +31,17 @@ 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)), - command_.joint_position); + // 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)), + // command_.joint_position); + command_.torque = command_target_.torque; - + command_.joint_position = command_target_.joint_position; + // RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()), "Command: " << command_.torque.data()[0]<< " " << command_.torque.data()[1]<< " " << command_.torque.data()[2]<< " " << command_.torque.data()[3]<< " " << command_.torque.data()[4]<< " " << command_.torque.data()[5]<< " " << command_.torque.data()[6]); // validate if (!command_guard_->is_valid_command(command_, state)) { std::string err = "Invalid command."; diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 0d7a0b62..8ce52b57 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 500 + update_rate: 1000 # ROS 2 control broadcasters joint_state_broadcaster: @@ -84,7 +84,7 @@ # tool. When you specify a target_wrench, i.e. some additional forces that # your robot should apply to its environment, that target_wrench gets # applied in this frame. - end_effector_link: "link_ee" + end_effector_link: "link_tool" # This is usually the link directly before the first actuated joint. All # controllers will build a kinematic chain from this link up to @@ -100,12 +100,12 @@ # This is the link that the robot feels compliant about. It does not need # to coincide with the end_effector_link, but for many use cases, this - # configuration is handy. When working with a screwdriver, for instance, + # configuration is handy. When working with a screwdriver, for instance, # setting compliance_ref_link == end_effector_link makes it easy to specify # downward pushing forces without generating unwanted offset moments. # On the other hand, an application could benefit from yielding a little in # the robot's wrist while drawing a line on a surface with a pen. - compliance_ref_link: "link_ee" + compliance_ref_link: "link_tool" command_interfaces: - effort @@ -122,16 +122,16 @@ - A7 delta_tau_max: 1.0 # Nm stiffness: # w.r.t. compliance_ref_link coordinates - trans_x: 200.0 - trans_y: 200.0 - trans_z: 200.0 - rot_x: 20.0 - rot_y: 20.0 - rot_z: 20.0 + trans_x: 2800.0 # 1900.0 + trans_y: 2800.0 # 1900.0 + trans_z: 1500.0 # 700.0 + rot_x: 180.0 + rot_y: 180.0 + rot_z: 180.0 kuka: true /**/motion_control_handle: ros__parameters: - end_effector_link: "link_ee" + end_effector_link: "link_tool" robot_base_link: "link_0" ft_sensor_ref_link: "link_ee" joints: @@ -145,7 +145,7 @@ /**/gravity_compensation: ros__parameters: - end_effector_link: "link_ee" + end_effector_link: "link_tool" robot_base_link: "link_0" ft_sensor_ref_link: "link_ee" command_interfaces: @@ -161,4 +161,4 @@ - A5 - A6 - A7 - kuka: true \ No newline at end of file + kuka: true diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/lbr_torque_command_controller.cpp index 2892f8eb..82eef2a3 100644 --- a/lbr_ros2_control/src/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/lbr_torque_command_controller.cpp @@ -80,6 +80,14 @@ bool LBRTorqueCommandController::reference_command_interfaces_() { torque_command_interfaces_.emplace_back(std::ref(command_interface)); } } + // Print command interfaces + RCLCPP_INFO(this->get_node()->get_logger(), "Command interfaces:"); + for (const auto &joint_position_command_interface : joint_position_command_interfaces_) { + RCLCPP_INFO(this->get_node()->get_logger(), " %s", joint_position_command_interface.get().get_name()); + } + for (const auto &torque_command_interface : torque_command_interfaces_) { + RCLCPP_INFO(this->get_node()->get_logger(), " %s", torque_command_interface.get().get_name()); + } if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { RCLCPP_ERROR( this->get_node()->get_logger(),