From 8b012c1714b3a33b39c52a29da72c4303484cbd0 Mon Sep 17 00:00:00 2001 From: Guilherme de Campos Affonso Date: Wed, 24 Jun 2026 04:56:57 +0000 Subject: [PATCH 1/6] Add package.xml and install binaries --- CMakeLists.txt | 9 +++++++++ package.xml | 40 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 49 insertions(+) create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a5d25f..8791f15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,7 @@ endif() # ----------------------------- # Find external packages # ----------------------------- +find_package(ament_cmake REQUIRED) find_package(OpenArmCAN REQUIRED) # find_package(urdf REQUIRED) find_package(orocos_kdl REQUIRED) @@ -73,3 +74,11 @@ target_link_libraries(gravity_comp PRIVATE openarm_teleop_lib) target_link_libraries(comm_test PRIVATE openarm_teleop_lib) target_link_libraries(unilateral_control PRIVATE openarm_teleop_lib) target_link_libraries(bilateral_control PRIVATE openarm_teleop_lib) + +# ----------------------------- +# Install +# ----------------------------- +install(TARGETS gravity_comp comm_test unilateral_control bilateral_control + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..1c95f73 --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + + openarm_teleop + 1.0.0 + Teleoperation and control executables for OpenArm + Enactic, Inc. + Apache-2.0 + + ament_cmake + + eigen + orocos_kdl_vendor + kdl_parser + urdfdom + urdfdom_headers + yaml-cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 8c0bd6a8bdf2cb0e146e4d8c49595eeaf96e73bc Mon Sep 17 00:00:00 2001 From: Guilherme de Campos Affonso Date: Wed, 24 Jun 2026 05:14:41 +0000 Subject: [PATCH 2/6] Add launch file to start both arms at once (bilateral) --- CMakeLists.txt | 7 ++ launch/bilateral.launch.py | 141 +++++++++++++++++++++++++++++++++++++ package.xml | 4 ++ 3 files changed, 152 insertions(+) create mode 100644 launch/bilateral.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 8791f15..dba676a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,11 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# Keep the directories of linked dependencies in the installed binaries' RPATH +# so shared libs (e.g. libkdl_parser.so from the ROS env) resolve at runtime +# when run from the install tree, e.g. via `ros2 run` / `ros2 launch`. +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + # ----------------------------- # Find external packages # ----------------------------- @@ -81,4 +86,6 @@ target_link_libraries(bilateral_control PRIVATE openarm_teleop_lib) install(TARGETS gravity_comp comm_test unilateral_control bilateral_control DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/launch/bilateral.launch.py b/launch/bilateral.launch.py new file mode 100644 index 0000000..d38bc11 --- /dev/null +++ b/launch/bilateral.launch.py @@ -0,0 +1,141 @@ +# Copyright 2025 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import os +import tempfile + +import xacro +from ament_index_python.packages import ( + get_package_prefix, + get_package_share_directory, +) +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.substitutions import LaunchConfiguration + +# arm_type -> (folder under assets/robot, xacro file name) +ARM_TYPE_MAP = { + "v10": ("openarm_v1.0", "openarm_v10.urdf.xacro"), + "v20": ("openarm_v2.0", "openarm_v20.urdf.xacro"), +} + + +def resolve_xacro_path(arm_type: str) -> str: + if arm_type not in ARM_TYPE_MAP: + raise RuntimeError( + f"Invalid arm_type: '{arm_type}'. Valid values: {sorted(ARM_TYPE_MAP)}" + ) + folder, file_name = ARM_TYPE_MAP[arm_type] + desc_share = get_package_share_directory("openarm_description") + return os.path.join(desc_share, "assets", "robot", folder, "urdf", file_name) + + +def launch_setup(context, *args, **kwargs): + arm_side = LaunchConfiguration("arm_side").perform(context) + arm_type = LaunchConfiguration("arm_type").perform(context) + + if arm_side not in ("right_arm", "left_arm", "both"): + raise RuntimeError( + f"Invalid arm_side: '{arm_side}'. Must be right_arm, left_arm, or both." + ) + + # Generate the URDF once. The URDF does not depend on arm_side (bimanual + # generates both arms; arm_side only selects the leaf link at runtime), and + # leader/follower share the same description, so a single file serves all + # bilateral_control processes. + xacro_path = resolve_xacro_path(arm_type) + urdf_xml = xacro.process_file( + xacro_path, mappings={"bimanual": "true"} + ).toprettyxml(indent=" ") + + out_dir = os.path.join(tempfile.gettempdir(), "openarm_urdf_gen") + os.makedirs(out_dir, exist_ok=True) + urdf_path = os.path.join(out_dir, f"openarm_{arm_type}.urdf") + with open(urdf_path, "w") as f: + f.write(urdf_xml) + + # Installed (not build/) binary. cwd is the share dir so the binary's + # relative "config/leader.yaml" / "config/follower.yaml" paths resolve. + bin_path = os.path.join( + get_package_prefix("openarm_teleop"), + "lib", + "openarm_teleop", + "bilateral_control", + ) + cwd = get_package_share_directory("openarm_teleop") + + arms = [] + if arm_side in ("right_arm", "both"): + arms.append( + ( + "right_arm", + LaunchConfiguration("leader_right_can").perform(context), + LaunchConfiguration("follower_right_can").perform(context), + ) + ) + if arm_side in ("left_arm", "both"): + arms.append( + ( + "left_arm", + LaunchConfiguration("leader_left_can").perform(context), + LaunchConfiguration("follower_left_can").perform(context), + ) + ) + + return [ + ExecuteProcess( + cmd=[bin_path, urdf_path, urdf_path, side, leader_can, follower_can], + cwd=cwd, + name=f"bilateral_control_{side}", + output="screen", + ) + for side, leader_can, follower_can in arms + ] + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "arm_side", + default_value="both", + description="Which arm(s) to control: right_arm, left_arm, or both.", + ), + DeclareLaunchArgument( + "arm_type", + default_value="v10", + description="Arm type (v10 or v20).", + ), + DeclareLaunchArgument( + "leader_right_can", + default_value="can0", + description="CAN interface for the right-arm leader.", + ), + DeclareLaunchArgument( + "leader_left_can", + default_value="can1", + description="CAN interface for the left-arm leader.", + ), + DeclareLaunchArgument( + "follower_right_can", + default_value="can2", + description="CAN interface for the right-arm follower.", + ), + DeclareLaunchArgument( + "follower_left_can", + default_value="can3", + description="CAN interface for the left-arm follower.", + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/package.xml b/package.xml index 1c95f73..c294ab7 100644 --- a/package.xml +++ b/package.xml @@ -31,6 +31,10 @@ urdfdom_headers yaml-cpp + xacro + openarm_description + ros2launch + ament_lint_auto ament_lint_common From cd7237d77942314f880280b975c4a142afa46f82 Mon Sep 17 00:00:00 2001 From: Guilherme de Campos Affonso Date: Tue, 30 Jun 2026 04:56:33 +0000 Subject: [PATCH 3/6] Add roslaunch support for unilateral_control --- .../{bilateral.launch.py => teleop.launch.py} | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) rename launch/{bilateral.launch.py => teleop.launch.py} (87%) diff --git a/launch/bilateral.launch.py b/launch/teleop.launch.py similarity index 87% rename from launch/bilateral.launch.py rename to launch/teleop.launch.py index d38bc11..4d7b20e 100644 --- a/launch/bilateral.launch.py +++ b/launch/teleop.launch.py @@ -29,6 +29,12 @@ "v20": ("openarm_v2.0", "openarm_v20.urdf.xacro"), } +# control_type -> installed binary name +CONTROL_TYPE_MAP = { + "unilateral": "unilateral_control", + "bilateral": "bilateral_control", +} + def resolve_xacro_path(arm_type: str) -> str: if arm_type not in ARM_TYPE_MAP: @@ -41,9 +47,17 @@ def resolve_xacro_path(arm_type: str) -> str: def launch_setup(context, *args, **kwargs): + control_type = LaunchConfiguration("control_type").perform(context) arm_side = LaunchConfiguration("arm_side").perform(context) arm_type = LaunchConfiguration("arm_type").perform(context) + if control_type not in CONTROL_TYPE_MAP: + raise RuntimeError( + f"Invalid control_type: '{control_type}'. " + f"Valid values: {sorted(CONTROL_TYPE_MAP)}" + ) + binary_name = CONTROL_TYPE_MAP[control_type] + if arm_side not in ("right_arm", "left_arm", "both"): raise RuntimeError( f"Invalid arm_side: '{arm_side}'. Must be right_arm, left_arm, or both." @@ -52,7 +66,7 @@ def launch_setup(context, *args, **kwargs): # Generate the URDF once. The URDF does not depend on arm_side (bimanual # generates both arms; arm_side only selects the leaf link at runtime), and # leader/follower share the same description, so a single file serves all - # bilateral_control processes. + # control processes. xacro_path = resolve_xacro_path(arm_type) urdf_xml = xacro.process_file( xacro_path, mappings={"bimanual": "true"} @@ -70,7 +84,7 @@ def launch_setup(context, *args, **kwargs): get_package_prefix("openarm_teleop"), "lib", "openarm_teleop", - "bilateral_control", + binary_name, ) cwd = get_package_share_directory("openarm_teleop") @@ -96,7 +110,7 @@ def launch_setup(context, *args, **kwargs): ExecuteProcess( cmd=[bin_path, urdf_path, urdf_path, side, leader_can, follower_can], cwd=cwd, - name=f"bilateral_control_{side}", + name=f"{binary_name}_{side}", output="screen", ) for side, leader_can, follower_can in arms @@ -106,6 +120,10 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( [ + DeclareLaunchArgument( + "control_type", + description="Control mode to launch: unilateral or bilateral.", + ), DeclareLaunchArgument( "arm_side", default_value="both", From ea0974e5cfce67ef1895e61e5cd97ad99a2ee8db Mon Sep 17 00:00:00 2001 From: Guilherme de Campos Affonso Date: Tue, 30 Jun 2026 05:01:12 +0000 Subject: [PATCH 4/6] Drop v20 from the launch file, since it is not currently supported --- launch/teleop.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/launch/teleop.launch.py b/launch/teleop.launch.py index 4d7b20e..9848340 100644 --- a/launch/teleop.launch.py +++ b/launch/teleop.launch.py @@ -24,9 +24,10 @@ from launch.substitutions import LaunchConfiguration # arm_type -> (folder under assets/robot, xacro file name) +# NOTE: currently only supports version 1.0 ARM_TYPE_MAP = { "v10": ("openarm_v1.0", "openarm_v10.urdf.xacro"), - "v20": ("openarm_v2.0", "openarm_v20.urdf.xacro"), + # "v20": ("openarm_v2.0", "openarm_v20.urdf.xacro"), } # control_type -> installed binary name @@ -132,7 +133,7 @@ def generate_launch_description(): DeclareLaunchArgument( "arm_type", default_value="v10", - description="Arm type (v10 or v20).", + description="Arm type (hardware version).", ), DeclareLaunchArgument( "leader_right_can", From 8ddae56bf6c128ee9e1b863a551ff9e6a8d09a91 Mon Sep 17 00:00:00 2001 From: "Affonso, Guilherme" Date: Thu, 2 Jul 2026 22:31:21 +0900 Subject: [PATCH 5/6] Update copyright Co-authored-by: Sutou Kouhei --- launch/teleop.launch.py | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/teleop.launch.py b/launch/teleop.launch.py index 9848340..904c710 100644 --- a/launch/teleop.launch.py +++ b/launch/teleop.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 Enactic, Inc. +# Copyright 2026 Enactic, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/package.xml b/package.xml index c294ab7..6219477 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@