diff --git a/.gitignore b/.gitignore index 19b7394a..2f6c655b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,5 @@ .DS_Store .vscode/ __pycache__/ +.github/ +openarm_description/ diff --git "a/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" "b/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" new file mode 100644 index 00000000..94801cde --- /dev/null +++ "b/dev/\344\277\256\346\224\271\350\250\230\351\214\204.md" @@ -0,0 +1,59 @@ +# 註冊新硬件接口 +openarm_hardware.xml +``` +class name="openarm_hardware/OpenArm_OYHW" +``` + + +## Markdown 內容樣式範例 + +### 標題 +```markdown +# 一級標題 +## 二級標題 +### 三級標題 +``` + +### 列表 +```markdown +- 無序列表項目 1 +- 無序列表項目 2 + +1. 有序列表項目 1 +2. 有序列表項目 2 +``` + +### 程式碼區塊 +```markdown +`行內程式碼` + +​```python +def hello(): + print("Hello World") +​``` +``` + +### 連結與圖片 +```markdown +[連結文字](https://example.com) +![圖片描述](image.png) +``` + +### 表格 +```markdown +| 欄位1 | 欄位2 | 欄位3 | +|-------|-------|-------| +| 內容1 | 內容2 | 內容3 | +``` + +### 引用 +```markdown +> 這是一段引用文字 +``` + +### 強調 +```markdown +**粗體文字** +*斜體文字* +~~刪除線~~ +``` \ No newline at end of file diff --git a/openarm.repos b/openarm.repos index b3cce359..e90cbb20 100644 --- a/openarm.repos +++ b/openarm.repos @@ -15,5 +15,5 @@ repositories: openarm_can: type: git - url: https://github.com/enactic/openarm_can.git - version: main \ No newline at end of file + url: git@github.com:openvmi/openarm_can.git + version: oy_openarm_can diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf index 8c2c6e1d..632c1f58 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf @@ -104,8 +104,8 @@ - - + + diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro index 6f6f4951..7a621541 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro @@ -19,8 +19,7 @@ - - + diff --git a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml index de64f007..f878ee15 100644 --- a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml +++ b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml @@ -44,6 +44,7 @@ left_arm_controller: - openarm_left_joint7 command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -61,6 +62,7 @@ right_arm_controller: - openarm_right_joint7 command_interfaces: - position + - velocity state_interfaces: - position - velocity diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py index 5ed6731c..a1ae036a 100644 --- a/openarm_bimanual_moveit_config/launch/demo.launch.py +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -139,6 +139,41 @@ def controller_spawner(context: LaunchContext, robot_controller): def generate_launch_description(): + """Generate launch description for OpenARM bimanual robot with MoveIt2 configuration. + + This function creates a complete launch description that sets up a dual-arm (bimanual) + OpenARM robot system with MoveIt2 motion planning capabilities. + + The launch description includes: + - Robot description and state publisher nodes via OpaqueFunction + - Joint state broadcaster for publishing joint states + - Arm trajectory controllers (forward position or joint trajectory) + - Gripper controllers for both left and right grippers + - MoveIt2 move_group node for motion planning + - RViz2 visualization with MoveIt configuration + + Launch Arguments: + description_package (str): Package containing robot description. Default: "openarm_description" + description_file (str): URDF/xacro file name. Default: "oy.urdf.xacro" + arm_type (str): Type of arm configuration. Default: "oy" + use_fake_hardware (str): Whether to use simulated hardware. Default: "false" + robot_controller (str): Controller type to use. Default: "joint_trajectory_controller" + Choices: "forward_position_controller", "joint_trajectory_controller" + runtime_config_package (str): Package with runtime configs. Default: "openarm_bringup" + arm_prefix (str): Prefix for arm links/joints. Default: "" + right_can_interface (str): CAN interface for right arm. Default: "can0" + left_can_interface (str): CAN interface for left arm. Default: "can1" + controllers_file (str): Controllers YAML config file. Default: "openarm_oy_bimanual_controllers.yaml" + + Note: + OpaqueFunction is used to defer node creation until launch time, allowing + dynamic configuration based on launch arguments that are only resolved + at runtime. This is necessary when node parameters depend on substitutions + that cannot be evaluated during launch file parsing. + + Returns: + LaunchDescription: Complete launch description for the bimanual robot system. + """ declared_arguments = [ DeclareLaunchArgument( "description_package", @@ -146,9 +181,9 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", ), - DeclareLaunchArgument("arm_type", default_value="v10"), + DeclareLaunchArgument("arm_type", default_value="oy"), DeclareLaunchArgument("use_fake_hardware", default_value="false"), DeclareLaunchArgument( "robot_controller", @@ -164,7 +199,7 @@ def generate_launch_description(): DeclareLaunchArgument("left_can_interface", default_value="can1"), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", + default_value="openarm_oy_bimanual_controllers.yaml", ), ] @@ -181,7 +216,7 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml new file mode 100644 index 00000000..ab6c8eaa --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers.yaml @@ -0,0 +1,179 @@ +# Copyright 2025 Enactic, Inc. +# Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + left_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + left_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + left_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + left_gripper_controller: + type: position_controllers/GripperActionController + + right_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + right_forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + right_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_gripper_controller: + type: position_controllers/GripperActionController + + +left_forward_position_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + +left_forward_velocity_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +left_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +left_gripper_controller: + ros__parameters: + joint: openarm_left_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + + +right_forward_position_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + +right_forward_velocity_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +right_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +right_gripper_controller: + ros__parameters: + joint: openarm_right_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position \ No newline at end of file diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml new file mode 100644 index 00000000..3e2ceee7 --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_bimanual_controllers_namespaced.yaml @@ -0,0 +1,140 @@ +# 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. + +/**: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + left_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + left_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + right_forward_position_controller: + type: forward_command_controller/ForwardCommandController + + left_gripper_controller: + type: forward_command_controller/ForwardCommandController + + right_gripper_controller: + type: forward_command_controller/ForwardCommandController + +/**/left_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: false + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.05 + goal_time: 0.6 + openarm_left_joint1: {trajectory: 0.1, goal: 0.1} + openarm_left_joint2: {trajectory: 0.1, goal: 0.1} + openarm_left_joint3: {trajectory: 0.1, goal: 0.1} + openarm_left_joint4: {trajectory: 0.1, goal: 0.1} + openarm_left_joint5: {trajectory: 0.1, goal: 0.1} + openarm_left_joint6: {trajectory: 0.1, goal: 0.1} + openarm_left_joint7: {trajectory: 0.1, goal: 0.1} + +/**/right_joint_trajectory_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: false + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.05 + goal_time: 0.6 + openarm_right_joint1: {trajectory: 0.1, goal: 0.1} + openarm_right_joint2: {trajectory: 0.1, goal: 0.1} + openarm_right_joint3: {trajectory: 0.1, goal: 0.1} + openarm_right_joint4: {trajectory: 0.1, goal: 0.1} + openarm_right_joint5: {trajectory: 0.1, goal: 0.1} + openarm_right_joint6: {trajectory: 0.1, goal: 0.1} + openarm_right_joint7: {trajectory: 0.1, goal: 0.1} + +/**/left_forward_position_controller: + ros__parameters: + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + interface_name: position + +/**/right_forward_position_controller: + ros__parameters: + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + interface_name: position + +/**/left_gripper_controller: + ros__parameters: + joints: + - openarm_left_finger_joint1 + interface_name: position + +/**/right_gripper_controller: + ros__parameters: + joints: + - openarm_right_finger_joint1 + interface_name: position diff --git a/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml new file mode 100644 index 00000000..b696e647 --- /dev/null +++ b/openarm_bringup/config/oy_controllers/openarm_oy_controllers.yaml @@ -0,0 +1,102 @@ +# 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. + +# Controller configuration for OpenArm with OY Motors + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: position_controllers/GripperActionController + +forward_position_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + + +forward_velocity_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + interface_name: velocity + command_interfaces: + - velocity + state_interfaces: + - velocity + +joint_trajectory_controller: + ros__parameters: + joints: + - openarm_joint1 + - openarm_joint2 + - openarm_joint3 + - openarm_joint4 + - openarm_joint5 + - openarm_joint6 + - openarm_joint7 + + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter + goal_time: 0.0 # Defaults to 0.0 (start immediately) + + +gripper_controller: + ros__parameters: + joint: openarm_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml index 8a386eec..ab6c8eaa 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml @@ -90,15 +90,17 @@ left_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) left_gripper_controller: @@ -155,15 +157,17 @@ right_joint_trajectory_controller: interface_name: position command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) right_gripper_controller: diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml index ca0b9721..3e2ceee7 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml @@ -50,6 +50,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -59,7 +60,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_left_joint1: {trajectory: 0.1, goal: 0.1} openarm_left_joint2: {trajectory: 0.1, goal: 0.1} @@ -82,6 +83,7 @@ interface_name: position command_interfaces: - position + - velocity state_interfaces: - position - velocity @@ -91,7 +93,7 @@ open_loop_control: false allow_integration_in_goal_trajectories: true constraints: - stopped_velocity_tolerance: 0.01 + stopped_velocity_tolerance: 0.05 goal_time: 0.6 openarm_right_joint1: {trajectory: 0.1, goal: 0.1} openarm_right_joint2: {trajectory: 0.1, goal: 0.1} diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml index f2f3a03c..89646639 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml +++ b/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml @@ -78,15 +78,17 @@ joint_trajectory_controller: command_interfaces: - position + - velocity state_interfaces: - position + - velocity state_publish_rate: 50.0 # Defaults to 50 action_monitor_rate: 50.0 # Defaults to 20 allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + stopped_velocity_tolerance: 0.05 # Relaxed for real motor velocity jitter goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index 99250a5c..221f68a8 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -64,6 +64,8 @@ def generate_robot_description(context: LaunchContext, description_package, desc "left_can_interface": left_can_interface_str, } ).toprettyxml(indent=" ") + # 輸出robot_description + # print(robot_description) return robot_description @@ -82,7 +84,7 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description if namespace: controllers_file_str = controllers_file_str.replace( - "openarm_v10_bimanual_controllers.yaml", "openarm_v10_bimanual_controllers_namespaced.yaml" + "openarm_oy_bimanual_controllers.yaml", "openarm_oy_bimanual_controllers_namespaced.yaml" ) robot_state_pub_node = Node( package="robot_state_publisher", @@ -144,13 +146,13 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", description="URDF/XACRO description file with the robot.", ), DeclareLaunchArgument( "arm_type", - default_value="v10", - description="Type of arm (e.g., v10).", + default_value="oy", + description="Type of arm (e.g., oy, v10).", ), DeclareLaunchArgument( "use_fake_hardware", @@ -186,7 +188,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", + default_value="openarm_oy_bimanual_controllers.yaml", description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", ), ] @@ -205,7 +207,7 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py index 847bbd0d..c9df16eb 100644 --- a/openarm_bringup/launch/openarm.launch.py +++ b/openarm_bringup/launch/openarm.launch.py @@ -90,7 +90,8 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description package="controller_manager", executable="ros2_control_node", output="both", - parameters=[robot_description_param, controllers_file_str], + parameters=[controllers_file_str], + remappings=[("~/robot_description", "/robot_description")], ) return [robot_state_pub_node, control_node] @@ -108,13 +109,13 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="oy.urdf.xacro", description="URDF/XACRO description file with the robot.", ), DeclareLaunchArgument( "arm_type", - default_value="v10", - description="Type of arm (e.g., v10).", + default_value="oy", + description="Type of arm (e.g., oy, v10).", ), DeclareLaunchArgument( "use_fake_hardware", @@ -145,7 +146,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_controllers.yaml", + default_value="openarm_oy_controllers.yaml", description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", ), ] @@ -163,7 +164,7 @@ def generate_launch_description(): # Configuration file paths controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "oy_controllers", controllers_file] ) # Robot nodes spawner (both state publisher and control) @@ -198,13 +199,13 @@ def generate_launch_description(): robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[robot_controller, "-c", "/controller_manager"], + arguments=[robot_controller, "--controller-manager", "/controller_manager"], ) gripper_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=["gripper_controller", "-c", "/controller_manager"], + arguments=["gripper_controller", "--controller-manager", "/controller_manager"], ) # Timing and sequencing @@ -213,13 +214,17 @@ def generate_launch_description(): actions=[joint_state_broadcaster_spawner], ) - delayed_robot_controller = TimerAction( - period=1.0, - actions=[robot_controller_spawner], + delayed_robot_controller = RegisterEventHandler( + OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[robot_controller_spawner], + ) ) - delayed_gripper_controller = TimerAction( - period=1.0, - actions=[gripper_controller_spawner], + delayed_gripper_controller = RegisterEventHandler( + OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[gripper_controller_spawner], + ) ) return LaunchDescription( diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index 1ed46920..e063c9db 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -30,7 +30,8 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(OpenArmCAN REQUIRED) add_library(${PROJECT_NAME} SHARED - src/v10_simple_hardware.cpp + # src/v10_simple_hardware.cpp + src/oy_hardware.cpp ) target_include_directories( diff --git a/openarm_hardware/include/openarm_hardware/oy_hardware.hpp b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp new file mode 100644 index 00000000..342a35f1 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/oy_hardware.hpp @@ -0,0 +1,155 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "openarm_hardware/visibility_control.h" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace openarm_hardware { + +/** + * @brief OpenArm Hardware Interface for OY Motors + * + * This hardware interface uses the OpenArm CAN API with OY motors, + * following the pattern from full_arm.cpp example. + * Configurable for different arm configurations via hardware parameters. + */ +class OpenArm_oyHW : public hardware_interface::SystemInterface { + public: + OpenArm_oyHW(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo& info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() + override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read(const rclcpp::Time& time, + const rclcpp::Duration& period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time& time, const rclcpp::Duration& period) override; + + private: + // default configuration + static constexpr size_t ARM_DOF = 7; + static constexpr bool ENABLE_GRIPPER = true; + + // Default OY motor configuration for V10 + // Motor types: GIM8115_9p for large joints, GIM4310_40/GIM4315_8 for small joints + const std::vector DEFAULT_MOTOR_TYPES = { + openarm::oy_motor::MotorType::GIM8115_9p, // Joint 1 + openarm::oy_motor::MotorType::GIM8115_9p, // Joint 2 + openarm::oy_motor::MotorType::GIM4310_40, // Joint 3 + openarm::oy_motor::MotorType::GIM4310_40, // Joint 4 + openarm::oy_motor::MotorType::GIM4315_8, // Joint 5 + openarm::oy_motor::MotorType::GIM4315_8, // Joint 6 + openarm::oy_motor::MotorType::GIM4315_8 // Joint 7 + }; + + const std::vector DEFAULT_SEND_CAN_IDS = {0x101, 0x102, 0x103, 0x104, + 0x105, 0x106, 0x107}; + const std::vector DEFAULT_RECV_CAN_IDS = {0x01, 0x02, 0x03, 0x04, + 0x05, 0x06, 0x07}; + + const openarm::oy_motor::MotorType DEFAULT_GRIPPER_MOTOR_TYPE = + openarm::oy_motor::MotorType::GIM4315_8; + const uint32_t DEFAULT_GRIPPER_SEND_CAN_ID = 0x108; + const uint32_t DEFAULT_GRIPPER_RECV_CAN_ID = 0x08; + + // Default gains for OY motors (may need tuning) + // Order: Joint 1-7 + Gripper + // 帶載機械臂優化：降低 KP 避免過衝，適當提高 KD 增加阻尼抑制震盪 + const std::vector DEFAULT_KP = {40.0, 40.0, 15.0, 15.0, + 2.0, 2.0, 2.0, 2.0}; + const std::vector DEFAULT_KD = {2.0, 2.0, 1.5, 1.2, + 0.2, 0.2, 0.2, 0.15}; + + // Default gains +// const std::vector DEFAULT_KP = {20.0, 20.0, 20.0, 20.0, +// 5.0, 5.0, 5.0, 0.5}; +// const std::vector DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, +// 0.7, 0.6, 0.5, 0.1}; + + const double GRIPPER_JOINT_0_POSITION = 0.044; + const double GRIPPER_JOINT_1_POSITION = 0.0; + const double GRIPPER_MOTOR_0_RADIANS = 0.0; + const double GRIPPER_MOTOR_1_RADIANS = -1.0472; + const double GRIPPER_DEFAULT_KP = 5.0; + const double GRIPPER_DEFAULT_KD = 0.1; + + // Configuration + std::string can_interface_; + std::string arm_prefix_; + bool hand_; + bool can_fd_; + + // OpenArm instance + std::unique_ptr openarm_; + + // Generated joint names for this arm instance + std::vector joint_names_; + + // ROS2 control state and command vectors + std::vector pos_commands_; + std::vector vel_commands_; + std::vector tau_commands_; + std::vector pos_states_; + std::vector vel_states_; + std::vector tau_states_; + + // Helper methods + void return_to_zero(); + bool parse_config(const hardware_interface::HardwareInfo& info); + void generate_joint_names(); + + // Gripper mapping functions + double joint_to_motor_radians(double joint_value); + double motor_radians_to_joint(double motor_radians); +}; + +} // namespace openarm_hardware diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index fd4fee92..8e4e3f24 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -15,11 +15,19 @@ --> - - ros2_control hardware interface for OpenArm V10. + ros2_control hardware interface for OpenArm V10 (Damiao Motors). + + --> + + + ros2_control hardware interface for OpenArm with OY Motors. + Configurable arm DOF via hardware parameters. diff --git a/openarm_hardware/src/oy_hardware.cpp b/openarm_hardware/src/oy_hardware.cpp new file mode 100644 index 00000000..1efab5d9 --- /dev/null +++ b/openarm_hardware/src/oy_hardware.cpp @@ -0,0 +1,351 @@ +// 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. + +#include "openarm_hardware/oy_hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace openarm_hardware { + +OpenArm_oyHW::OpenArm_oyHW() = default; + +bool OpenArm_oyHW::parse_config(const hardware_interface::HardwareInfo& info) { + // Parse CAN interface (default: can0) + auto it = info.hardware_parameters.find("can_interface"); + can_interface_ = (it != info.hardware_parameters.end()) ? it->second : "can0"; + + // Parse arm prefix (default: empty for single arm, "left_" or "right_" for + // bimanual) + it = info.hardware_parameters.find("arm_prefix"); + arm_prefix_ = (it != info.hardware_parameters.end()) ? it->second : ""; + + // Parse gripper enable (default: true) + it = info.hardware_parameters.find("hand"); + if (it == info.hardware_parameters.end()) { + hand_ = true; // Default to true + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + hand_ = (value == "true"); + } + + // Parse CAN-FD enable (default: true) + it = info.hardware_parameters.find("can_fd"); + if (it == info.hardware_parameters.end()) { + can_fd_ = true; // Default to true + } else { + // Handle both "true"/"True" and "false"/"False" + std::string value = it->second; + std::transform(value.begin(), value.end(), value.begin(), ::tolower); + can_fd_ = (value == "true"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Configuration: CAN=%s, arm_prefix=%s, arm_dof=%zu, hand=%s, can_fd=%s", + can_interface_.c_str(), arm_prefix_.c_str(), ARM_DOF, + hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); + return true; +} + +void OpenArm_oyHW::generate_joint_names() { + joint_names_.clear(); + // TODO: read from urdf properly and sort in the future. + // Currently, the joint names are hardcoded for order consistency to align + // with hardware. Generate arm joint names: openarm_{arm_prefix}joint{N} + for (size_t i = 1; i <= ARM_DOF; ++i) { + std::string joint_name = + "openarm_" + arm_prefix_ + "joint" + std::to_string(i); + joint_names_.push_back(joint_name); + } + + // Generate gripper joint name if enabled + if (hand_) { + std::string gripper_joint_name = "openarm_" + arm_prefix_ + "finger_joint1"; + joint_names_.push_back(gripper_joint_name); + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Added gripper joint: %s", + gripper_joint_name.c_str()); + } else { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Gripper joint NOT added because hand_=false"); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Generated %zu joint names for arm prefix '%s'", + joint_names_.size(), arm_prefix_.c_str()); +} + +hardware_interface::CallbackReturn OpenArm_oyHW::on_init( + const hardware_interface::HardwareInfo& info) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + // Parse configuration + if (!parse_config(info)) { + return CallbackReturn::ERROR; + } + + // Generate joint names based on arm prefix + generate_joint_names(); + + // Validate joint count (arm joints + optional gripper) + size_t expected_joints = ARM_DOF + (hand_ ? 1 : 0); + if (joint_names_.size() != expected_joints) { + RCLCPP_ERROR(rclcpp::get_logger("OpenArm_oyHW"), + "Generated %zu joint names, expected %zu", joint_names_.size(), + expected_joints); + return CallbackReturn::ERROR; + } + + // Initialize OpenArm with configurable CAN-FD setting + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Initializing OpenArm (OY Motor) on %s with CAN-FD %s...", + can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); + openarm_ = + std::make_unique(can_interface_, can_fd_); + + // Initialize arm motors with defaults (OY motors) + // Use only the required number of motors based on ARM_DOF + std::vector motor_types( + DEFAULT_MOTOR_TYPES.begin(), + DEFAULT_MOTOR_TYPES.begin() + std::min(ARM_DOF, DEFAULT_MOTOR_TYPES.size())); + std::vector send_ids( + DEFAULT_SEND_CAN_IDS.begin(), + DEFAULT_SEND_CAN_IDS.begin() + std::min(ARM_DOF, DEFAULT_SEND_CAN_IDS.size())); + std::vector recv_ids( + DEFAULT_RECV_CAN_IDS.begin(), + DEFAULT_RECV_CAN_IDS.begin() + std::min(ARM_DOF, DEFAULT_RECV_CAN_IDS.size())); + + openarm_->init_arm_motors(motor_types, send_ids, recv_ids); + + // Initialize gripper if enabled + if (hand_) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Initializing gripper..."); + openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, + DEFAULT_GRIPPER_SEND_CAN_ID, + DEFAULT_GRIPPER_RECV_CAN_ID); + } + + // Initialize state and command vectors based on generated joint count + const size_t total_joints = joint_names_.size(); + pos_commands_.resize(total_joints, 0.0); + vel_commands_.resize(total_joints, 0.0); + tau_commands_.resize(total_joints, 0.0); + pos_states_.resize(total_joints, 0.0); + vel_states_.resize(total_joints, 0.0); + tau_states_.resize(total_joints, 0.0); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "OpenArm OY Motor HW initialized successfully with %zu DOF", + ARM_DOF + (hand_ ? 1 : 0)); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArm_oyHW::on_configure( + const rclcpp_lifecycle::State& /*previous_state*/) { + // Refresh motor states during configuration + openarm_->refresh_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + return CallbackReturn::SUCCESS; +} + +std::vector +OpenArm_oyHW::export_state_interfaces() { + std::vector state_interfaces; + for (size_t i = 0; i < joint_names_.size(); ++i) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, &pos_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, &vel_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_states_[i])); + } + + return state_interfaces; +} + +std::vector +OpenArm_oyHW::export_command_interfaces() { + std::vector command_interfaces; + // TODO: consider exposing only needed interfaces to avoid undefined behavior. + for (size_t i = 0; i < joint_names_.size(); ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_POSITION, + &pos_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_VELOCITY, + &vel_commands_[i])); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &tau_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn OpenArm_oyHW::on_activate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Activating OpenArm OY Motor..."); + + // Enable all motors + openarm_->enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + // Return to zero position + return_to_zero(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "OpenArm OY Motor activated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArm_oyHW::on_deactivate( + const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Deactivating OpenArm OY Motor..."); + + // Disable all motors + openarm_->disable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "OpenArm OY Motor deactivated"); + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type OpenArm_oyHW::read( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Refresh and receive all motor states + openarm_->refresh_all(); + openarm_->recv_all(); + + // Read arm joint states + const auto& arm_motors = openarm_->get_arm().get_motors(); + for (size_t i = 0; i < ARM_DOF && i < arm_motors.size(); ++i) { + pos_states_[i] = arm_motors[i].get_position(); + vel_states_[i] = arm_motors[i].get_velocity(); + tau_states_[i] = arm_motors[i].get_torque(); + //print joint name and states for debugging + // RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), "Joint %s: pos=%.3f, vel=%.3f, tau=%.3f", + // joint_names_[i].c_str(), + // pos_states_[i], + // vel_states_[i], + // tau_states_[i]); + } + + // Read gripper state if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + const auto& gripper_motors = openarm_->get_gripper().get_motors(); + if (!gripper_motors.empty()) { + // TODO the mappings are approximates + // Convert motor position (radians) to joint value (0-0.044m) + double motor_pos = gripper_motors[0].get_position(); + pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); + + //Velocity and torque mapping + vel_states_[ARM_DOF] = gripper_motors[0].get_velocity(); + tau_states_[ARM_DOF] = gripper_motors[0].get_torque(); + } + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type OpenArm_oyHW::write( + const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { + // Control arm motors with MIT control + // OY motor MITParam order: {q, dq, kp, kd, tau} + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + double kp = (i < DEFAULT_KP.size()) ? DEFAULT_KP[i] : DEFAULT_KP.back(); + double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); + //print kp kd pos vel tau for debugging + // if( i == 0 && pos_commands_[i] != 0.0) { // Only print for the first joint if there's a command + // RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + // "Commanding joint %zu: pos_cmd=%.4f, vel_cmd=%.4f, kp=%.2f, kd=%.2f, tau_cmd=%.4f", + // i, pos_commands_[i], vel_commands_[i], kp, kd, tau_commands_[i]); + // } + arm_params.push_back({pos_commands_[i], vel_commands_[i], + kp, kd, tau_commands_[i]}); + } + + openarm_->get_arm().oy_mit_control_all(arm_params); + + // Control gripper if enabled + if (hand_ && joint_names_.size() > ARM_DOF) { + // TODO the true mappings are unimplemented. + double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); + // OY motor MITParam order: {q, dq, kp, kd, tau} + openarm_->get_gripper().oy_mit_control_all( + {{motor_command, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); + } + openarm_->recv_all(1000); + return hardware_interface::return_type::OK; +} + +void OpenArm_oyHW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArm_oyHW"), + "Returning to zero position..."); + + // Return arm to zero with MIT control + // OY motor MITParam order: {q, dq, kp, kd, tau} + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + double kp = (i < DEFAULT_KP.size()) ? DEFAULT_KP[i] : DEFAULT_KP.back(); + double kd = (i < DEFAULT_KD.size()) ? DEFAULT_KD[i] : DEFAULT_KD.back(); + arm_params.push_back({0.0, 0.0, kp, kd, 0.0}); + } + openarm_->get_arm().oy_mit_control_all(arm_params); + + // Return gripper to zero if enabled + if (hand_) { + // OY motor MITParam order: {q, dq, kp, kd, tau} + openarm_->get_gripper().oy_mit_control_all( + {{GRIPPER_JOINT_0_POSITION, 0.0, GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, 0.0}}); + } + std::this_thread::sleep_for(std::chrono::microseconds(1000)); + openarm_->recv_all(); +} + +// Gripper mapping helper functions +double OpenArm_oyHW::joint_to_motor_radians(double joint_value) { + // Joint 0=closed -> motor 0 rad, Joint 0.044=open -> motor -1.0472 rad + return (joint_value / GRIPPER_JOINT_0_POSITION) * + GRIPPER_MOTOR_1_RADIANS; // Scale from 0-0.044 to 0 to -1.0472 +} + +double OpenArm_oyHW::motor_radians_to_joint(double motor_radians) { + // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 0.044 + return GRIPPER_JOINT_0_POSITION * + (motor_radians / + GRIPPER_MOTOR_1_RADIANS); // Scale from 0 to -1.0472 to 0-0.044 +} + +} // namespace openarm_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArm_oyHW, + hardware_interface::SystemInterface)