From a126a3e0c78cc0dfde8d1e0272d29c552c1ba5d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Mon, 8 Sep 2025 13:27:47 +0200 Subject: [PATCH 1/2] feat: Added twist broadcasting to demos --- .gitignore | 1 + .../config/fr3/controllers.yaml | 17 +++ .../config/fr3/left_controllers.yaml | 17 +++ .../config/fr3/right_controllers.yaml | 17 +++ .../config/iiwa/controllers.yaml | 17 +++ .../config/kinova_gen3/controllers.yaml | 17 +++ .../__pycache__/__init__.cpython-310.pyc | Bin 188 -> 188 bytes ...isp_py_franka_hand_adapter.cpython-310.pyc | Bin 7146 -> 7146 bytes .../launch/franka.launch.py | 6 + .../launch/iiwa.launch.py | 127 ++++++++++-------- .../launch/kinova_gen3.launch.py | 6 + docker/Dockerfile.robots | 4 +- 12 files changed, 172 insertions(+), 57 deletions(-) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2e892ba --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +crisp_controllers diff --git a/crisp_controllers_robot_demos/config/fr3/controllers.yaml b/crisp_controllers_robot_demos/config/fr3/controllers.yaml index cd0052a..b7c1629 100644 --- a/crisp_controllers_robot_demos/config/fr3/controllers.yaml +++ b/crisp_controllers_robot_demos/config/fr3/controllers.yaml @@ -9,6 +9,9 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + twist_broadcaster: + type: crisp_controllers/TwistBroadcaster + pose_broadcaster: type: crisp_controllers/PoseBroadcaster @@ -49,6 +52,20 @@ ros__parameters: use_local_topics: False + twist_broadcaster: + ros__parameters: + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 + + end_effector_frame: fr3_hand_tcp + base_frame: base + pose_broadcaster: ros__parameters: joints: diff --git a/crisp_controllers_robot_demos/config/fr3/left_controllers.yaml b/crisp_controllers_robot_demos/config/fr3/left_controllers.yaml index f146ad1..2dd7c80 100644 --- a/crisp_controllers_robot_demos/config/fr3/left_controllers.yaml +++ b/crisp_controllers_robot_demos/config/fr3/left_controllers.yaml @@ -9,6 +9,9 @@ left: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + twist_broadcaster: + type: crisp_controllers/TwistBroadcaster + pose_broadcaster: type: crisp_controllers/PoseBroadcaster @@ -55,6 +58,20 @@ left: ros__parameters: use_local_topics: False + twist_broadcaster: + ros__parameters: + joints: + - left_fr3_joint1 + - left_fr3_joint2 + - left_fr3_joint3 + - left_fr3_joint4 + - left_fr3_joint5 + - left_fr3_joint6 + - left_fr3_joint7 + + end_effector_frame: left_fr3_hand_tcp + base_frame: left_base + pose_broadcaster: ros__parameters: joints: diff --git a/crisp_controllers_robot_demos/config/fr3/right_controllers.yaml b/crisp_controllers_robot_demos/config/fr3/right_controllers.yaml index 5cec236..71d720a 100644 --- a/crisp_controllers_robot_demos/config/fr3/right_controllers.yaml +++ b/crisp_controllers_robot_demos/config/fr3/right_controllers.yaml @@ -9,6 +9,9 @@ right: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + twist_broadcaster: + type: crisp_controllers/TwistBroadcaster + pose_broadcaster: type: crisp_controllers/PoseBroadcaster @@ -55,6 +58,20 @@ right: ros__parameters: use_local_topics: False + twist_broadcaster: + ros__parameters: + joints: + - right_fr3_joint1 + - right_fr3_joint2 + - right_fr3_joint3 + - right_fr3_joint4 + - right_fr3_joint5 + - right_fr3_joint6 + - right_fr3_joint7 + + end_effector_frame: right_fr3_hand_tcp + base_frame: right_base + pose_broadcaster: ros__parameters: joints: diff --git a/crisp_controllers_robot_demos/config/iiwa/controllers.yaml b/crisp_controllers_robot_demos/config/iiwa/controllers.yaml index b462b6c..0b13d00 100644 --- a/crisp_controllers_robot_demos/config/iiwa/controllers.yaml +++ b/crisp_controllers_robot_demos/config/iiwa/controllers.yaml @@ -9,6 +9,9 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + twist_broadcaster: + type: crisp_controllers/TwistBroadcaster + pose_broadcaster: type: crisp_controllers/PoseBroadcaster @@ -43,6 +46,20 @@ ros__parameters: use_local_topics: False + twist_broadcaster: + ros__parameters: + joints: + - joint_a1 + - joint_a2 + - joint_a3 + - joint_a4 + - joint_a5 + - joint_a6 + - joint_a7 + + end_effector_frame: tool0 + base_frame: iiwa_base + pose_broadcaster: ros__parameters: joints: diff --git a/crisp_controllers_robot_demos/config/kinova_gen3/controllers.yaml b/crisp_controllers_robot_demos/config/kinova_gen3/controllers.yaml index 6221b66..bf1e8d1 100644 --- a/crisp_controllers_robot_demos/config/kinova_gen3/controllers.yaml +++ b/crisp_controllers_robot_demos/config/kinova_gen3/controllers.yaml @@ -9,6 +9,9 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + twist_broadcaster: + type: crisp_controllers/TwistBroadcaster + gravity_compensation: type: crisp_controllers/CartesianController @@ -50,6 +53,20 @@ ros__parameters: use_local_topics: False + twist_broadcaster: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 + + end_effector_frame: end_effector_link + base_frame: base_link + pose_broadcaster: ros__parameters: joints: diff --git a/crisp_controllers_robot_demos/crisp_controllers_robot_demos/__pycache__/__init__.cpython-310.pyc b/crisp_controllers_robot_demos/crisp_controllers_robot_demos/__pycache__/__init__.cpython-310.pyc index ec8a13a424a6acddd8a96acda7458bbbcce11695..a3e39145df17e77b731b8d4b27bab5f83ee1ea42 100644 GIT binary patch delta 19 ZcmdnPxQCHDpO=@50SKZdyG`WY2mmXb1epK; delta 19 ZcmdnPxQCHDpO=@50SH1;Bqnli1OO@31U3Kw diff --git a/crisp_controllers_robot_demos/crisp_controllers_robot_demos/__pycache__/crisp_py_franka_hand_adapter.cpython-310.pyc b/crisp_controllers_robot_demos/crisp_controllers_robot_demos/__pycache__/crisp_py_franka_hand_adapter.cpython-310.pyc index 9a9297cc1434887f6b39880521151466a9cece25..e3956aa249c6b0f73775065eed3a4e2c0ff9e72a 100644 GIT binary patch delta 20 acmaE5{>q#?pO=@50SKPg&)mp;Um5^Jng*5t delta 20 acmaE5{>q#?pO=@50SLZWMsDQ3FAV@f5e6Lq diff --git a/crisp_controllers_robot_demos/launch/franka.launch.py b/crisp_controllers_robot_demos/launch/franka.launch.py index f6165df..5658192 100644 --- a/crisp_controllers_robot_demos/launch/franka.launch.py +++ b/crisp_controllers_robot_demos/launch/franka.launch.py @@ -229,6 +229,12 @@ def generate_launch_description(): arguments=["joint_trajectory_controller"], output="screen", ), + Node( + package="controller_manager", + executable="spawner", + arguments=["twist_broadcaster"], + output="screen", + ), Node( package="controller_manager", executable="spawner", diff --git a/crisp_controllers_robot_demos/launch/iiwa.launch.py b/crisp_controllers_robot_demos/launch/iiwa.launch.py index f960b58..c202c48 100644 --- a/crisp_controllers_robot_demos/launch/iiwa.launch.py +++ b/crisp_controllers_robot_demos/launch/iiwa.launch.py @@ -180,70 +180,78 @@ def generate_launch_description(): namespace = LaunchConfiguration("namespace") # Get URDF via xacro - robot_description_content = Command([ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution([ - FindPackageShare("crisp_controllers_robot_demos"), - "config", - "iiwa", - "iiwa.urdf.xacro", - ]), - " ", - "prefix:=", - prefix, - " ", - "use_fake_hardware:=", - use_fake_hardware, - " ", - "robot_ip:=", - robot_ip, - " ", - "robot_port:=", - robot_port, - " ", - "initial_positions_file:=", - initial_positions_file, - " ", - "command_interface:=", - command_interface, - " ", - "base_frame_file:=", - base_frame_file, - " ", - "description_package:=", - description_package, - " ", - "runtime_config_package:=", - runtime_config_package, - " ", - "controllers_file:=", - controllers_file, - " ", - "namespace:=", - namespace, - ]) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("crisp_controllers_robot_demos"), + "config", + "iiwa", + "iiwa.urdf.xacro", + ] + ), + " ", + "prefix:=", + prefix, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "robot_ip:=", + robot_ip, + " ", + "robot_port:=", + robot_port, + " ", + "initial_positions_file:=", + initial_positions_file, + " ", + "command_interface:=", + command_interface, + " ", + "base_frame_file:=", + base_frame_file, + " ", + "description_package:=", + description_package, + " ", + "runtime_config_package:=", + runtime_config_package, + " ", + "controllers_file:=", + controllers_file, + " ", + "namespace:=", + namespace, + ] + ) robot_description = {"robot_description": robot_description_content} # Use our controllers - robot_controllers = PathJoinSubstitution([ - FindPackageShare("crisp_controllers_robot_demos"), - "config", - "iiwa", - "controllers.yaml", - ]) + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("crisp_controllers_robot_demos"), + "config", + "iiwa", + "controllers.yaml", + ] + ) # robot_controllers = PathJoinSubstitution([ # FindPackageShare(runtime_config_package), # "config", # controllers_file, # ]) - rviz_config_file = PathJoinSubstitution([ - FindPackageShare(description_package), - "rviz", - "iiwa.rviz", - ]) + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare(description_package), + "rviz", + "iiwa.rviz", + ] + ) control_node = Node( package="controller_manager", @@ -282,7 +290,6 @@ def generate_launch_description(): ], ) - robot_controller_spawner = Node( package="controller_manager", executable="spawner", @@ -314,6 +321,15 @@ def generate_launch_description(): ], ) + twist_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "twist_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) nodes = [ control_node, @@ -323,6 +339,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner, cartesian_impedance_controller_spawner, pose_broadcaster_spawner, + twist_broadcaster_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/crisp_controllers_robot_demos/launch/kinova_gen3.launch.py b/crisp_controllers_robot_demos/launch/kinova_gen3.launch.py index 66991f1..9ef70c5 100644 --- a/crisp_controllers_robot_demos/launch/kinova_gen3.launch.py +++ b/crisp_controllers_robot_demos/launch/kinova_gen3.launch.py @@ -108,6 +108,12 @@ def generate_launch_description(): arguments=["pose_broadcaster"], output="screen", ), + Node( + package="controller_manager", + executable="spawner", + arguments=["twist_broadcaster"], + output="screen", + ), Node( package="controller_manager", executable="spawner", diff --git a/docker/Dockerfile.robots b/docker/Dockerfile.robots index 8326e49..99097dd 100644 --- a/docker/Dockerfile.robots +++ b/docker/Dockerfile.robots @@ -1,5 +1,5 @@ ARG ROS_DISTRO=humble -ARG CRISP_CONTROLLERS_VERSION=1.0.0 +ARG CRISP_CONTROLLERS_VERSION=1.1.0 FROM osrf/ros:${ROS_DISTRO}-desktop AS base @@ -157,7 +157,7 @@ RUN source /opt/ros/$ROS_DISTRO/setup.bash \ FROM iiwa AS iiwa-overlay -ARG CRISP_CONTROLLERS_VERSION=1.0.0 +ARG CRISP_CONTROLLERS_VERSION=1.1.0 COPY . src/crisp_controllers_demos From 09a5b4575bbb3669f1c8bc4d5cd0ef799d260ca8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20San=20Jos=C3=A9=20Pro?= <42489409+danielsanjosepro@users.noreply.github.com> Date: Thu, 9 Oct 2025 08:50:38 +0200 Subject: [PATCH 2/2] i --- docker/Dockerfile.robots | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/docker/Dockerfile.robots b/docker/Dockerfile.robots index 99097dd..4dd2beb 100644 --- a/docker/Dockerfile.robots +++ b/docker/Dockerfile.robots @@ -1,5 +1,4 @@ ARG ROS_DISTRO=humble -ARG CRISP_CONTROLLERS_VERSION=1.1.0 FROM osrf/ros:${ROS_DISTRO}-desktop AS base @@ -122,7 +121,7 @@ RUN git clone https://github.com/ICube-Robotics/iiwa_ros2.git src/iiwa_ros2 \ FROM franka AS franka-overlay -ARG CRISP_CONTROLLERS_VERSION=1.0.0 +ARG CRISP_CONTROLLERS_VERSION=1.1.0 COPY . src/crisp_controllers_demos @@ -140,7 +139,7 @@ RUN source /opt/ros/$ROS_DISTRO/setup.bash \ FROM kinova AS kinova-overlay -ARG CRISP_CONTROLLERS_VERSION=1.0.0 +ARG CRISP_CONTROLLERS_VERSION=1.1.0 COPY . src/crisp_controllers_demos