diff --git a/openarm/package.xml b/openarm/package.xml index 0457deb8..12957414 100644 --- a/openarm/package.xml +++ b/openarm/package.xml @@ -27,6 +27,7 @@ openarm_bringup openarm_description openarm_hardware + openarm_bimanual_moveit_config ament_lint_auto ament_lint_common diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf deleted file mode 100644 index 8c2c6e1d..00000000 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf +++ /dev/null @@ -1,252 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/openarm_bimanual_moveit_config/config/joint_limits.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/joint_limits.yaml similarity index 99% rename from openarm_bimanual_moveit_config/config/joint_limits.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/joint_limits.yaml index 995f9999..858e3172 100644 --- a/openarm_bimanual_moveit_config/config/joint_limits.yaml +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/joint_limits.yaml @@ -101,4 +101,4 @@ joint_limits: has_velocity_limits: true max_velocity: 20.943946 has_acceleration_limits: false - max_acceleration: 0.0 \ No newline at end of file + max_acceleration: 0.0 diff --git a/openarm_bimanual_moveit_config/config/kinematics.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/kinematics.yaml similarity index 100% rename from openarm_bimanual_moveit_config/config/kinematics.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/kinematics.yaml diff --git a/openarm_bimanual_moveit_config/config/moveit.rviz b/openarm_bimanual_moveit_config/config/openarm_v1.0/moveit.rviz similarity index 100% rename from openarm_bimanual_moveit_config/config/moveit.rviz rename to openarm_bimanual_moveit_config/config/openarm_v1.0/moveit.rviz diff --git a/openarm_bimanual_moveit_config/config/moveit_controllers.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/moveit_controllers.yaml similarity index 100% rename from openarm_bimanual_moveit_config/config/moveit_controllers.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/moveit_controllers.yaml diff --git a/openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.srdf new file mode 100644 index 00000000..0cb128be --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.srdf @@ -0,0 +1,143 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.urdf.xacro similarity index 93% rename from openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro rename to openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.urdf.xacro index 6f6f4951..f4f725f5 100644 --- a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/openarm_bimanual.urdf.xacro @@ -17,15 +17,11 @@ - - - - + - diff --git a/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/pilz_cartesian_limits.yaml similarity index 97% rename from openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/pilz_cartesian_limits.yaml index 61b7e410..40ee0f2d 100644 --- a/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/pilz_cartesian_limits.yaml @@ -17,4 +17,4 @@ cartesian_limits: max_trans_vel: 1.0 max_trans_acc: 2.25 max_trans_dec: -5.0 - max_rot_vel: 1.57 \ No newline at end of file + max_rot_vel: 1.57 diff --git a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/ros2_controllers.yaml similarity index 99% rename from openarm_bimanual_moveit_config/config/ros2_controllers.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/ros2_controllers.yaml index de64f007..7c2053be 100644 --- a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/ros2_controllers.yaml @@ -82,4 +82,4 @@ right_gripper_controller: command_interfaces: - position state_interfaces: - - position \ No newline at end of file + - position diff --git a/openarm_bimanual_moveit_config/config/sensors_3d.yaml b/openarm_bimanual_moveit_config/config/openarm_v1.0/sensors_3d.yaml similarity index 60% rename from openarm_bimanual_moveit_config/config/sensors_3d.yaml rename to openarm_bimanual_moveit_config/config/openarm_v1.0/sensors_3d.yaml index 01b9ef2e..1199f4e1 100644 --- a/openarm_bimanual_moveit_config/config/sensors_3d.yaml +++ b/openarm_bimanual_moveit_config/config/openarm_v1.0/sensors_3d.yaml @@ -1,3 +1,17 @@ +# 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. + # This file is used to configure the 3D sensors for the robot. # It is a placeholder for now. Configure the sensors to use the correct topics and parameters. sensors: @@ -24,4 +38,4 @@ kinect_depthimage: padding_scale: 4.0 queue_size: 5 sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - shadow_threshold: 0.2 \ No newline at end of file + shadow_threshold: 0.2 diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/joint_limits.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/joint_limits.yaml new file mode 100644 index 00000000..3f60ae44 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/joint_limits.yaml @@ -0,0 +1,104 @@ +# 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. +# 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. + +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + openarm_left_finger_joint1: + has_velocity_limits: true + max_velocity: 10.0 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint1: + has_velocity_limits: true + max_velocity: 16.754666 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint2: + has_velocity_limits: true + max_velocity: 16.754666 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint3: + has_velocity_limits: true + max_velocity: 5.445426 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint4: + has_velocity_limits: true + max_velocity: 5.445426 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint5: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint6: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_left_joint7: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_finger_joint1: + has_velocity_limits: true + max_velocity: 10.0 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint1: + has_velocity_limits: true + max_velocity: 16.754666 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint2: + has_velocity_limits: true + max_velocity: 16.754666 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint3: + has_velocity_limits: true + max_velocity: 5.445426 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint4: + has_velocity_limits: true + max_velocity: 5.445426 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint5: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint6: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 + openarm_right_joint7: + has_velocity_limits: true + max_velocity: 20.943946 + has_acceleration_limits: false + max_acceleration: 0.0 diff --git a/openarm_bimanual_moveit_config/config/initial_positions.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/kinematics.yaml similarity index 53% rename from openarm_bimanual_moveit_config/config/initial_positions.yaml rename to openarm_bimanual_moveit_config/config/openarm_v2.0/kinematics.yaml index 4c87cabf..c1d6d23f 100644 --- a/openarm_bimanual_moveit_config/config/initial_positions.yaml +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/kinematics.yaml @@ -12,21 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Default initial positions for openarm's ros2_control fake system -initial_positions: - openarm_left_finger_joint1: 0 - openarm_left_joint1: 0 - openarm_left_joint2: 0 - openarm_left_joint3: 0 - openarm_left_joint4: 0 - openarm_left_joint5: 0 - openarm_left_joint6: 0 - openarm_left_joint7: 0 - openarm_right_finger_joint1: 0 - openarm_right_joint1: 0 - openarm_right_joint2: 0 - openarm_right_joint3: 0 - openarm_right_joint4: 0 - openarm_right_joint5: 0 - openarm_right_joint6: 0 - openarm_right_joint7: 0 \ No newline at end of file + +left_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 + +right_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit.rviz b/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit.rviz new file mode 100644 index 00000000..3f2746d9 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit.rviz @@ -0,0 +1,447 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + Splitter Ratio: 0.5 + Tree Height: 254 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + openarm_body_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_left_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_left_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_right_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_right_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 20; 93; 250 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: upper_body + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + openarm_body_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_left_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_left_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_left_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_right_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + openarm_right_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + openarm_right_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.3624029159545898 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.05883333832025528 + Y: 0.2625991404056549 + Z: 0.3334124982357025 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.31999996304512024 + Target Frame: world + Value: Orbit (rviz_default_plugins) + Yaw: 0.16200032830238342 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Width: 1280 + Height: 720 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd00000001000000000000036a000006f0fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007000fffffffb000000100044006900730070006c0061007900730100000069000001ec0000017800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002610000036f0000028e00fffffffb0000000800480065006c0070000000029a0000006e000000cf00fffffffb0000000a0056006900650077007301000005dc0000017d0000012300ffffff0000096a000006f000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + X: 0 + Y: 64 diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit_controllers.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit_controllers.yaml new file mode 100644 index 00000000..90bcdfb7 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/moveit_controllers.yaml @@ -0,0 +1,65 @@ +# 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. +# 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. + + +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - left_joint_trajectory_controller + - right_joint_trajectory_controller + - left_gripper_controller + - right_gripper_controller + + left_joint_trajectory_controller: + type: FollowJointTrajectory + joints: + - openarm_left_joint1 + - openarm_left_joint2 + - openarm_left_joint3 + - openarm_left_joint4 + - openarm_left_joint5 + - openarm_left_joint6 + - openarm_left_joint7 + action_ns: follow_joint_trajectory + default: true + + right_joint_trajectory_controller: + type: FollowJointTrajectory + joints: + - openarm_right_joint1 + - openarm_right_joint2 + - openarm_right_joint3 + - openarm_right_joint4 + - openarm_right_joint5 + - openarm_right_joint6 + - openarm_right_joint7 + action_ns: follow_joint_trajectory + default: true + + left_gripper_controller: + type: GripperCommand + joints: + - openarm_left_finger_joint1 + action_ns: gripper_cmd + default: true + + right_gripper_controller: + type: GripperCommand + joints: + - openarm_right_finger_joint1 + action_ns: gripper_cmd + default: true diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.srdf new file mode 100644 index 00000000..acb5cacd --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.srdf @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.urdf.xacro new file mode 100644 index 00000000..a57888f6 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/openarm_bimanual.urdf.xacro @@ -0,0 +1,27 @@ + + + + + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/pilz_cartesian_limits.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..034e6426 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/pilz_cartesian_limits.yaml @@ -0,0 +1,20 @@ +# 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. +# 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. + +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/ros2_controllers.yaml new file mode 100644 index 00000000..84ad43ef --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/ros2_controllers.yaml @@ -0,0 +1,91 @@ +# 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. + +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + left_gripper_controller: + type: position_controllers/GripperActionController + + right_gripper_controller: + type: position_controllers/GripperActionController + + # left_gripper_controller: + # type: joint_trajectory_controller/JointTrajectoryController + + # right_gripper_controller: + # type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +left_arm_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 + command_interfaces: + - position + state_interfaces: + - position + - velocity + - effort + +right_arm_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 + command_interfaces: + - position + state_interfaces: + - position + - velocity + - effort + +left_gripper_controller: + ros__parameters: + joints: + - openarm_left_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + +right_gripper_controller: + ros__parameters: + joints: + - openarm_right_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position diff --git a/openarm_bimanual_moveit_config/config/openarm_v2.0/sensors_3d.yaml b/openarm_bimanual_moveit_config/config/openarm_v2.0/sensors_3d.yaml new file mode 100644 index 00000000..ee5af9d8 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_v2.0/sensors_3d.yaml @@ -0,0 +1,41 @@ +# 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. +# 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. + +# This file is used to configure the 3D sensors for the robot. +# It is a placeholder for now. Configure the sensors to use the correct topics and parameters. +sensors: + - default_sensor + - kinect_depthimage +default_sensor: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py index 5ed6731c..45a111a7 100644 --- a/openarm_bimanual_moveit_config/launch/demo.launch.py +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -14,51 +14,59 @@ import os import xacro -from ament_index_python.packages import ( - get_package_share_directory, -) +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription, LaunchContext -from launch.actions import ( - DeclareLaunchArgument, - TimerAction, - OpaqueFunction, -) -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, -) +from launch.actions import DeclareLaunchArgument, TimerAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder +# All accepted arm_type values +VALID_ARM_TYPES = { + "v1.0", "v10", "v1_0", "openarm_v1.0", "openarm_v10", "openarm_v1_0", + "v2.0", "v20", "v2_0", "openarm_v2.0", "openarm_v20", "openarm_v2_0", +} + + +def resolve_arm_config(arm_type_str: str) -> tuple[str, str]: + """ + Resolve folder name and xacro file name from arm_type. + Accepts: v1.0, v10, v1_0, openarm_v1.0, openarm_v10, openarm_v1_0 (and v2.0 variants) + Raises ValueError if arm_type is not recognized. + """ + if arm_type_str not in VALID_ARM_TYPES: + raise ValueError( + f"Invalid arm_type: '{arm_type_str}'. " + f"Please specify openarm_v1.0 or openarm_v2.0." + ) + if any(x in arm_type_str for x in ("1.0", "10", "1_0")): + return "openarm_v1.0", "openarm_v10.urdf.xacro" + return "openarm_v2.0", "openarm_v20.urdf.xacro" + def generate_robot_description( context: LaunchContext, description_package, - description_file, arm_type, use_fake_hardware, right_can_interface, left_can_interface, - arm_prefix, ): - """Render Xacro and return XML string.""" description_package_str = context.perform_substitution(description_package) - description_file_str = context.perform_substitution(description_file) arm_type_str = context.perform_substitution(arm_type) use_fake_hardware_str = context.perform_substitution(use_fake_hardware) right_can_interface_str = context.perform_substitution(right_can_interface) left_can_interface_str = context.perform_substitution(left_can_interface) - arm_prefix_str = context.perform_substitution(arm_prefix) + + folder_name, file_name = resolve_arm_config(arm_type_str) xacro_path = os.path.join( get_package_share_directory(description_package_str), - "urdf", - "robot", - description_file_str, + "assets", "robot", folder_name, "urdf", file_name ) - robot_description = xacro.process_file( + return xacro.process_file( xacro_path, mappings={ "arm_type": arm_type_str, @@ -67,17 +75,13 @@ def generate_robot_description( "ros2_control": "true", "left_can_interface": left_can_interface_str, "right_can_interface": right_can_interface_str, - # arm_prefix unused inside xacro but kept for completeness }, ).toprettyxml(indent=" ") - return robot_description - def robot_nodes_spawner( context: LaunchContext, description_package, - description_file, arm_type, use_fake_hardware, controllers_file, @@ -88,12 +92,10 @@ def robot_nodes_spawner( robot_description = generate_robot_description( context, description_package, - description_file, arm_type, use_fake_hardware, right_can_interface, left_can_interface, - arm_prefix, ) controllers_file_str = context.perform_substitution(controllers_file) @@ -138,38 +140,111 @@ def controller_spawner(context: LaunchContext, robot_controller): ] +def moveit_nodes_spawner(context: LaunchContext, arm_type, use_fake_hardware): + arm_type_str = context.perform_substitution(arm_type) + use_fake_hardware_str = context.perform_substitution(use_fake_hardware) + + description_pkg_path = get_package_share_directory("openarm_description") + moveit_pkg_path = get_package_share_directory( + "openarm_bimanual_moveit_config") + + folder_name, file_name = resolve_arm_config(arm_type_str) + + xacro_path = os.path.join( + description_pkg_path, "assets", "robot", folder_name, "urdf", file_name + ) + + if any(x in arm_type_str for x in ("1.0", "10", "1_0")): + config_dir = "openarm_v1.0" + else: + config_dir = "openarm_v2.0" + + moveit_config = ( + MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config") + .robot_description( + file_path=xacro_path, + mappings={ + "arm_type": arm_type_str, + "bimanual": "true", + "use_fake_hardware": use_fake_hardware_str, + "ros2_control": "true", + } + ) + .robot_description_semantic(file_path=f"config/{config_dir}/openarm_bimanual.srdf") + .robot_description_kinematics(file_path=f"config/{config_dir}/kinematics.yaml") + .joint_limits(file_path=f"config/{config_dir}/joint_limits.yaml") + .trajectory_execution(file_path=f"config/{config_dir}/moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl"], + default_planning_pipeline="ompl" + ) + .to_moveit_configs() + ) + + moveit_params = moveit_config.to_dict() + + pilz_cartesian_limits_path = os.path.join( + moveit_pkg_path, "config", config_dir, "pilz_cartesian_limits.yaml" + ) + + if os.path.exists(pilz_cartesian_limits_path): + import yaml + with open(pilz_cartesian_limits_path, 'r') as f: + config_data = yaml.safe_load(f) + if "cartesian_limits" in config_data: + if "robot_description_planning" not in moveit_params: + moveit_params["robot_description_planning"] = {} + moveit_params["robot_description_planning"].update(config_data) + + rviz_cfg = os.path.join(moveit_pkg_path, "config", + config_dir, "moveit.rviz") + + return [ + Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_params], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_cfg], + parameters=[moveit_params], + ), + ] + + def generate_launch_description(): declared_arguments = [ + DeclareLaunchArgument("description_package", + default_value="openarm_description"), DeclareLaunchArgument( - "description_package", - default_value="openarm_description", - ), - DeclareLaunchArgument( - "description_file", - default_value="v10.urdf.xacro", + "arm_type", + default_value="openarm_v2.0", + description="Arm type. Accepts: v1.0, v10, openarm_v1.0, v2.0, v20, openarm_v2.0, etc." ), - DeclareLaunchArgument("arm_type", default_value="v10"), - DeclareLaunchArgument("use_fake_hardware", default_value="false"), + DeclareLaunchArgument("use_fake_hardware", default_value="true"), DeclareLaunchArgument( "robot_controller", default_value="joint_trajectory_controller", choices=["forward_position_controller", "joint_trajectory_controller"], ), - DeclareLaunchArgument( - "runtime_config_package", default_value="openarm_bringup" - ), + DeclareLaunchArgument("runtime_config_package", + default_value="openarm_bringup"), DeclareLaunchArgument("arm_prefix", default_value=""), DeclareLaunchArgument("right_can_interface", default_value="can0"), DeclareLaunchArgument("left_can_interface", default_value="can1"), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", - ), + default_value="openarm_bimanual_moveit_controllers.yaml"), ] description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") arm_type = LaunchConfiguration("arm_type") use_fake_hardware = LaunchConfiguration("use_fake_hardware") robot_controller = LaunchConfiguration("robot_controller") @@ -181,14 +256,13 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( function=robot_nodes_spawner, args=[ description_package, - description_file, arm_type, use_fake_hardware, controllers_file, @@ -198,6 +272,11 @@ def generate_launch_description(): ], ) + moveit_nodes_func = OpaqueFunction( + function=moveit_nodes_spawner, + args=[arm_type, use_fake_hardware] + ) + jsb_spawner = Node( package="controller_manager", executable="spawner", @@ -206,7 +285,8 @@ def generate_launch_description(): ) controller_spawner_func = OpaqueFunction( - function=controller_spawner, args=[robot_controller]) + function=controller_spawner, args=[robot_controller] + ) gripper_spawner = Node( package="controller_manager", @@ -215,46 +295,13 @@ def generate_launch_description(): "right_gripper_controller", "-c", "/controller_manager"], ) - delayed_jsb = TimerAction(period=2.0, actions=[jsb_spawner]) - delayed_arm_ctrl = TimerAction( - period=1.0, actions=[controller_spawner_func]) - delayed_gripper = TimerAction(period=1.0, actions=[gripper_spawner]) - - moveit_config = MoveItConfigsBuilder( - "openarm", package_name="openarm_bimanual_moveit_config" - ).to_moveit_configs() - - moveit_params = moveit_config.to_dict() - - run_move_group_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - parameters=[moveit_params], - ) - - rviz_cfg = os.path.join( - get_package_share_directory( - "openarm_bimanual_moveit_config"), "config", "moveit.rviz" - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_cfg], - parameters=[moveit_params], - ) - return LaunchDescription( declared_arguments + [ robot_nodes_spawner_func, - delayed_jsb, - delayed_arm_ctrl, - delayed_gripper, - run_move_group_node, - rviz_node, + moveit_nodes_func, + TimerAction(period=2.0, actions=[jsb_spawner]), + TimerAction(period=1.0, actions=[controller_spawner_func]), + TimerAction(period=1.0, actions=[gripper_spawner]), ] ) diff --git a/openarm_bimanual_moveit_config/launch/move_group.launch.py b/openarm_bimanual_moveit_config/launch/move_group.launch.py index ec098d29..36bd77c9 100644 --- a/openarm_bimanual_moveit_config/launch/move_group.launch.py +++ b/openarm_bimanual_moveit_config/launch/move_group.launch.py @@ -1,8 +1,40 @@ +# 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. + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_move_group_launch +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + + +def move_group_spawner(context: LaunchContext, arm_type): + arm_type_str = context.perform_substitution(arm_type) + moveit_config = ( + MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config") + .robot_description_semantic(file_path=f"config/{arm_type_str}/openarm_bimanual.srdf") + .joint_limits(file_path=f"config/{arm_type_str}/joint_limits.yaml") + .robot_description_kinematics(file_path=f"config/{arm_type_str}/kinematics.yaml") + .to_moveit_configs() + ) + return generate_move_group_launch(moveit_config).entities def generate_launch_description(): - moveit_config = MoveItConfigsBuilder( - "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() - return generate_move_group_launch(moveit_config) + return LaunchDescription([ + DeclareLaunchArgument("arm_type", default_value="v20"), + OpaqueFunction(function=move_group_spawner, args=[ + LaunchConfiguration("arm_type")]) + ]) diff --git a/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py b/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py index 1bed84f3..f2466d58 100644 --- a/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py +++ b/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py @@ -1,8 +1,40 @@ +# 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. + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_moveit_rviz_launch +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + + +def rviz_spawner(context: LaunchContext, arm_type): + arm_type_str = context.perform_substitution(arm_type) + moveit_config = ( + MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config") + .robot_description_semantic(file_path=f"config/{arm_type_str}/openarm_bimanual.srdf") + .joint_limits(file_path=f"config/{arm_type_str}/joint_limits.yaml") + .robot_description_kinematics(file_path=f"config/{arm_type_str}/kinematics.yaml") + .to_moveit_configs() + ) + return generate_moveit_rviz_launch(moveit_config).entities def generate_launch_description(): - moveit_config = MoveItConfigsBuilder( - "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() - return generate_moveit_rviz_launch(moveit_config) + return LaunchDescription([ + DeclareLaunchArgument("arm_type", default_value="v20"), + OpaqueFunction(function=rviz_spawner, args=[ + LaunchConfiguration("arm_type")]) + ]) diff --git a/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py b/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py index d5ecc3c4..3b363b61 100644 --- a/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py +++ b/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py @@ -1,8 +1,40 @@ +# 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. + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_spawn_controllers_launch +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + + +def spawn_controllers_spawner(context: LaunchContext, arm_type): + arm_type_str = context.perform_substitution(arm_type) + moveit_config = ( + MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config") + .robot_description_semantic(file_path=f"config/{arm_type_str}/openarm_bimanual.srdf") + .joint_limits(file_path=f"config/{arm_type_str}/joint_limits.yaml") + .robot_description_kinematics(file_path=f"config/{arm_type_str}/kinematics.yaml") + .to_moveit_configs() + ) + return generate_spawn_controllers_launch(moveit_config).entities def generate_launch_description(): - moveit_config = MoveItConfigsBuilder( - "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() - return generate_spawn_controllers_launch(moveit_config) + return LaunchDescription([ + DeclareLaunchArgument("arm_type", default_value="v20"), + OpaqueFunction(function=spawn_controllers_spawner, + args=[LaunchConfiguration("arm_type")]) + ]) diff --git a/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py b/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py index 9b82fdc7..f12e6461 100644 --- a/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -1,8 +1,40 @@ +# 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. + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch +from launch import LaunchDescription, LaunchContext +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + + +def static_tfs_spawner(context: LaunchContext, arm_type): + arm_type_str = context.perform_substitution(arm_type) + moveit_config = ( + MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config") + .robot_description_semantic(file_path=f"config/{arm_type_str}/openarm_bimanual.srdf") + .joint_limits(file_path=f"config/{arm_type_str}/joint_limits.yaml") + .robot_description_kinematics(file_path=f"config/{arm_type_str}/kinematics.yaml") + .to_moveit_configs() + ) + return generate_static_virtual_joint_tfs_launch(moveit_config).entities def generate_launch_description(): - moveit_config = MoveItConfigsBuilder( - "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() - return generate_static_virtual_joint_tfs_launch(moveit_config) + return LaunchDescription([ + DeclareLaunchArgument("arm_type", default_value="v20"), + OpaqueFunction(function=static_tfs_spawner, args=[ + LaunchConfiguration("arm_type")]) + ]) diff --git a/openarm_bringup/config/controllers/openarm_bimanual_controllers.yaml b/openarm_bringup/config/controllers/openarm_bimanual_controllers.yaml new file mode 100644 index 00000000..11bc51fb --- /dev/null +++ b/openarm_bringup/config/controllers/openarm_bimanual_controllers.yaml @@ -0,0 +1,208 @@ +# 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: 750 # 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: joint_trajectory_controller/JointTrajectoryController + + 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: joint_trajectory_controller/JointTrajectoryController + +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 + state_interfaces: + - position + + # to achieve high real-time performance, interpolation is disabled and the controller is expected to receive high-frequency commands --- IGNORE --- + interpolation_method: "none" + interpolate_from_desired_state: true + + state_publish_rate: 50.0 + action_monitor_rate: 50.0 + allow_partial_joints_goal: false + + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + decelerate_on_cancel: false + openarm_left_joint1: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint2: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint3: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint4: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint5: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint6: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint7: { trajectory: 0.0, goal: 0.0 } + +left_gripper_controller: + ros__parameters: + joints: + - openarm_left_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + + interpolation_method: "none" + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + +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 + state_interfaces: + - position + + interpolation_method: "none" + interpolate_from_desired_state: true + + state_publish_rate: 50.0 + action_monitor_rate: 50.0 + allow_partial_joints_goal: false + + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + decelerate_on_cancel: false + openarm_right_joint1: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint2: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint3: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint4: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint5: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint6: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint7: { trajectory: 0.0, goal: 0.0 } + +right_gripper_controller: + ros__parameters: + joints: + - openarm_right_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + + interpolation_method: "none" + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 diff --git a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml b/openarm_bringup/config/controllers/openarm_bimanual_moveit_controllers.yaml similarity index 69% rename from openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml rename to openarm_bringup/config/controllers/openarm_bimanual_moveit_controllers.yaml index 8a386eec..84cf8dde 100644 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers.yaml +++ b/openarm_bringup/config/controllers/openarm_bimanual_moveit_controllers.yaml @@ -15,7 +15,7 @@ controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 100 joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -30,7 +30,7 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController left_gripper_controller: - type: position_controllers/GripperActionController + type: joint_trajectory_controller/JointTrajectoryController right_forward_position_controller: type: forward_command_controller/ForwardCommandController @@ -42,8 +42,7 @@ controller_manager: type: joint_trajectory_controller/JointTrajectoryController right_gripper_controller: - type: position_controllers/GripperActionController - + type: joint_trajectory_controller/JointTrajectoryController left_forward_position_controller: ros__parameters: @@ -93,22 +92,37 @@ left_joint_trajectory_controller: state_interfaces: - position - state_publish_rate: 50.0 # Defaults to 50 - action_monitor_rate: 50.0 # Defaults to 20 + interpolation_method: "splines" + interpolate_from_desired_state: false + + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false - allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediately) + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + openarm_left_joint1: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint2: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint3: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint4: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint5: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint6: { trajectory: 0.0, goal: 0.0 } + openarm_left_joint7: { trajectory: 0.0, goal: 0.0 } left_gripper_controller: ros__parameters: - joint: openarm_left_finger_joint1 + joints: + - openarm_left_finger_joint1 command_interfaces: - position state_interfaces: - position + interpolation_method: "splines" + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 right_forward_position_controller: ros__parameters: @@ -158,18 +172,34 @@ right_joint_trajectory_controller: state_interfaces: - position - state_publish_rate: 50.0 # Defaults to 50 - action_monitor_rate: 50.0 # Defaults to 20 + interpolation_method: "splines" + interpolate_from_desired_state: false + + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false - allow_partial_joints_goal: false # Defaults to false constraints: - stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediately) + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + openarm_right_joint1: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint2: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint3: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint4: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint5: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint6: { trajectory: 0.0, goal: 0.0 } + openarm_right_joint7: { trajectory: 0.0, goal: 0.0 } right_gripper_controller: ros__parameters: - joint: openarm_right_finger_joint1 + joints: + - openarm_right_finger_joint1 command_interfaces: - position state_interfaces: - - position \ No newline at end of file + - position + + interpolation_method: "splines" + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 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 deleted file mode 100644 index ca0b9721..00000000 --- a/openarm_bringup/config/v10_controllers/openarm_v10_bimanual_controllers_namespaced.yaml +++ /dev/null @@ -1,138 +0,0 @@ -# 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 - 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.01 - 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 - 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.01 - 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/v10_controllers/openarm_v10_controllers.yaml b/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml deleted file mode 100644 index f2f3a03c..00000000 --- a/openarm_bringup/config/v10_controllers/openarm_v10_controllers.yaml +++ /dev/null @@ -1,99 +0,0 @@ -# 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 - - 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 - state_interfaces: - - position - - 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 - goal_time: 0.0 # Defaults to 0.0 (start immediately) - - -gripper_controller: - ros__parameters: - joint: openarm_finger_joint1 - command_interfaces: - - position - state_interfaces: - - position \ No newline at end of file diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index feac6cc8..2b5fb46f 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -19,15 +19,33 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription, LaunchContext -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction, OpaqueFunction -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, -) +from launch.actions import DeclareLaunchArgument, TimerAction, OpaqueFunction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +# All accepted arm_type values +VALID_ARM_TYPES = { + "v1.0", "v10", "v1_0", "openarm_v1.0", "openarm_v10", "openarm_v1_0", + "v2.0", "v20", "v2_0", "openarm_v2.0", "openarm_v20", "openarm_v2_0", +} + + +def resolve_arm_config(arm_type_str: str) -> tuple[str, str]: + """ + Resolve folder name and xacro file name from arm_type. + Accepts: v1.0, v10, v1_0, openarm_v1.0, openarm_v10, openarm_v1_0 (and v2.0 variants) + Raises ValueError if arm_type is not recognized. + """ + if arm_type_str not in VALID_ARM_TYPES: + raise ValueError( + f"Invalid arm_type: '{arm_type_str}'. " + f"Please specify openarm_v1.0 or openarm_v2.0." + ) + if any(x in arm_type_str for x in ("1.0", "10", "1_0")): + return "openarm_v1.0", "openarm_v10.urdf.xacro" + return "openarm_v2.0", "openarm_v20.urdf.xacro" + def namespace_from_context(context, arm_prefix): arm_prefix_str = context.perform_substitution(arm_prefix) @@ -39,20 +57,19 @@ def namespace_from_context(context, arm_prefix): def generate_robot_description(context: LaunchContext, description_package, description_file, arm_type, use_fake_hardware, right_can_interface, left_can_interface): """Generate robot description using xacro processing.""" - description_package_str = context.perform_substitution(description_package) - description_file_str = context.perform_substitution(description_file) arm_type_str = context.perform_substitution(arm_type) use_fake_hardware_str = context.perform_substitution(use_fake_hardware) right_can_interface_str = context.perform_substitution(right_can_interface) left_can_interface_str = context.perform_substitution(left_can_interface) + folder_name, file_name = resolve_arm_config(arm_type_str) + xacro_path = os.path.join( get_package_share_directory(description_package_str), - "urdf", "robot", description_file_str + "assets", "robot", folder_name, "urdf", file_name ) - # Process xacro with required arguments robot_description = xacro.process_file( xacro_path, mappings={ @@ -69,12 +86,14 @@ def generate_robot_description(context: LaunchContext, description_package, desc def robot_nodes_spawner(context: LaunchContext, description_package, description_file, - arm_type, use_fake_hardware, controllers_file, right_can_interface, left_can_interface, arm_prefix): + arm_type, use_fake_hardware, controllers_file, + right_can_interface, left_can_interface, arm_prefix): """Spawn both robot state publisher and control nodes with shared robot description.""" namespace = namespace_from_context(context, arm_prefix) robot_description = generate_robot_description( - context, description_package, description_file, arm_type, use_fake_hardware, right_can_interface, left_can_interface, + context, description_package, description_file, arm_type, + use_fake_hardware, right_can_interface, left_can_interface, ) controllers_file_str = context.perform_substitution(controllers_file) @@ -82,8 +101,10 @@ 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_bimanual_controllers.yaml", + "openarm_bimanual_controllers_namespaced.yaml" ) + robot_state_pub_node = Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -107,8 +128,9 @@ def robot_nodes_spawner(context: LaunchContext, description_package, description def controller_spawner(context: LaunchContext, robot_controller, arm_prefix): """Spawn controller based on robot_controller argument.""" namespace = namespace_from_context(context, arm_prefix) - - controller_manager_ref = f"/{namespace}/controller_manager" if namespace else "/controller_manager" + controller_manager_ref = ( + f"/{namespace}/controller_manager" if namespace else "/controller_manager" + ) robot_controller_str = context.perform_substitution(robot_controller) @@ -121,21 +143,20 @@ def controller_spawner(context: LaunchContext, robot_controller, arm_prefix): else: raise ValueError(f"Unknown robot_controller: {robot_controller_str}") - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - namespace=namespace, - arguments=[robot_controller_left, - robot_controller_right, "-c", controller_manager_ref], - ) - - return [robot_controller_spawner] + return [ + Node( + package="controller_manager", + executable="spawner", + namespace=namespace, + arguments=[robot_controller_left, robot_controller_right, + "-c", controller_manager_ref], + ) + ] def generate_launch_description(): """Generate launch description for OpenArm bimanual configuration.""" - # Declare launch arguments declared_arguments = [ DeclareLaunchArgument( "description_package", @@ -144,17 +165,17 @@ def generate_launch_description(): ), DeclareLaunchArgument( "description_file", - default_value="v10.urdf.xacro", + default_value="v20.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="openarm_v2.0", + description="Arm type. Accepts: v1.0, v10, openarm_v1.0, v2.0, v20, openarm_v2.0, etc.", ), DeclareLaunchArgument( "use_fake_hardware", - default_value="false", + default_value="true", description="Use fake hardware instead of real hardware.", ), DeclareLaunchArgument( @@ -186,12 +207,11 @@ def generate_launch_description(): ), DeclareLaunchArgument( "controllers_file", - default_value="openarm_v10_bimanual_controllers.yaml", - description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", + default_value="openarm_bimanual_controllers.yaml", + description="Controllers file to use.", ), ] - # Initialize launch configurations description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") arm_type = LaunchConfiguration("arm_type") @@ -205,18 +225,18 @@ def generate_launch_description(): controllers_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] + "controllers", controllers_file] ) robot_nodes_spawner_func = OpaqueFunction( function=robot_nodes_spawner, args=[description_package, description_file, arm_type, - use_fake_hardware, controllers_file, right_can_interface, left_can_interface, arm_prefix] + use_fake_hardware, controllers_file, + right_can_interface, left_can_interface, arm_prefix] ) rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", - "bimanual.rviz"] + [FindPackageShare(description_package), "rviz", "bimanual.rviz"] ) rviz_node = Node( @@ -227,19 +247,21 @@ def generate_launch_description(): arguments=["-d", rviz_config_file], ) - # Joint state broadcaster spawner joint_state_broadcaster_spawner = OpaqueFunction( function=lambda context: [Node( package="controller_manager", executable="spawner", namespace=namespace_from_context(context, arm_prefix), - arguments=["joint_state_broadcaster", - "--controller-manager", - f"/{namespace_from_context(context, arm_prefix)}/controller_manager" if namespace_from_context(context, arm_prefix) else "/controller_manager"], + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + f"/{namespace_from_context(context, arm_prefix)}/controller_manager" + if namespace_from_context(context, arm_prefix) + else "/controller_manager" + ], )] ) - # Controller spawners controller_spawner_func = OpaqueFunction( function=controller_spawner, args=[robot_controller, arm_prefix] @@ -250,36 +272,27 @@ def generate_launch_description(): package="controller_manager", executable="spawner", namespace=namespace_from_context(context, arm_prefix), - arguments=["left_gripper_controller", - "right_gripper_controller", "-c", - f"/{namespace_from_context(context, arm_prefix)}/controller_manager" if namespace_from_context(context, arm_prefix) else "/controller_manager"], + arguments=[ + "left_gripper_controller", "right_gripper_controller", + "-c", + f"/{namespace_from_context(context, arm_prefix)}/controller_manager" + if namespace_from_context(context, arm_prefix) + else "/controller_manager" + ], )] ) - # Timing and sequencing LAUNCH_DELAY_SECONDS = 1.0 - delayed_joint_state_broadcaster = TimerAction( - period=LAUNCH_DELAY_SECONDS, - actions=[joint_state_broadcaster_spawner], - ) - - delayed_robot_controller = TimerAction( - period=LAUNCH_DELAY_SECONDS, - actions=[controller_spawner_func], - ) - delayed_gripper_controller = TimerAction( - period=LAUNCH_DELAY_SECONDS, - actions=[gripper_controller_spawner], - ) return LaunchDescription( declared_arguments + [ robot_nodes_spawner_func, rviz_node, - ] + - [ - delayed_joint_state_broadcaster, - delayed_robot_controller, - delayed_gripper_controller, + TimerAction(period=LAUNCH_DELAY_SECONDS, actions=[ + joint_state_broadcaster_spawner]), + TimerAction(period=LAUNCH_DELAY_SECONDS, + actions=[controller_spawner_func]), + TimerAction(period=LAUNCH_DELAY_SECONDS, actions=[ + gripper_controller_spawner]), ] ) diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py deleted file mode 100644 index 847bbd0d..00000000 --- a/openarm_bringup/launch/openarm.launch.py +++ /dev/null @@ -1,235 +0,0 @@ -# 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. - -import os -import xacro - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription, LaunchContext -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction, OpaqueFunction -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_robot_description(context: LaunchContext, description_package, description_file, - arm_type, use_fake_hardware, can_interface, arm_prefix): - """Generate robot description using xacro processing.""" - - # Substitute launch configuration values - description_package_str = context.perform_substitution(description_package) - description_file_str = context.perform_substitution(description_file) - arm_type_str = context.perform_substitution(arm_type) - use_fake_hardware_str = context.perform_substitution(use_fake_hardware) - can_interface_str = context.perform_substitution(can_interface) - arm_prefix_str = context.perform_substitution(arm_prefix) - - # Build xacro file path - xacro_path = os.path.join( - get_package_share_directory(description_package_str), - "urdf", "robot", description_file_str - ) - - # Process xacro with required arguments - robot_description = xacro.process_file( - xacro_path, - mappings={ - "arm_type": arm_type_str, - "bimanual": "false", - "use_fake_hardware": use_fake_hardware_str, - "ros2_control": "true", - "can_interface": can_interface_str, - "arm_prefix": arm_prefix_str, - } - ).toprettyxml(indent=" ") - - return robot_description - - -def robot_nodes_spawner(context: LaunchContext, description_package, description_file, - arm_type, use_fake_hardware, controllers_file, can_interface, arm_prefix): - """Spawn both robot state publisher and control nodes with shared robot description.""" - - # Generate robot description once - robot_description = generate_robot_description( - context, description_package, description_file, arm_type, use_fake_hardware, can_interface, arm_prefix - ) - - # Get controllers file path - controllers_file_str = context.perform_substitution(controllers_file) - robot_description_param = {"robot_description": robot_description} - - # Robot state publisher node - robot_state_pub_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[robot_description_param], - ) - - # Control node - control_node = Node( - package="controller_manager", - executable="ros2_control_node", - output="both", - parameters=[robot_description_param, controllers_file_str], - ) - - return [robot_state_pub_node, control_node] - - -def generate_launch_description(): - """Generate launch description for OpenArm unimanual configuration.""" - - # Declare launch arguments - declared_arguments = [ - DeclareLaunchArgument( - "description_package", - default_value="openarm_description", - description="Description package with robot URDF/xacro files.", - ), - DeclareLaunchArgument( - "description_file", - default_value="v10.urdf.xacro", - description="URDF/XACRO description file with the robot.", - ), - DeclareLaunchArgument( - "arm_type", - default_value="v10", - description="Type of arm (e.g., v10).", - ), - DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Use fake hardware instead of real hardware.", - ), - DeclareLaunchArgument( - "robot_controller", - default_value="joint_trajectory_controller", - choices=["forward_position_controller", - "joint_trajectory_controller"], - description="Robot controller to start.", - ), - DeclareLaunchArgument( - "runtime_config_package", - default_value="openarm_bringup", - description="Package with the controller's configuration in config folder.", - ), - DeclareLaunchArgument( - "arm_prefix", - default_value="", - description="Prefix for the arm.", - ), - DeclareLaunchArgument( - "can_interface", - default_value="can0", - description="CAN interface to use.", - ), - DeclareLaunchArgument( - "controllers_file", - default_value="openarm_v10_controllers.yaml", - description="Controllers file(s) to use. Can be a single file or comma-separated list of files.", - ), - ] - - # Initialize launch configurations - description_package = LaunchConfiguration("description_package") - description_file = LaunchConfiguration("description_file") - arm_type = LaunchConfiguration("arm_type") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") - robot_controller = LaunchConfiguration("robot_controller") - runtime_config_package = LaunchConfiguration("runtime_config_package") - controllers_file = LaunchConfiguration("controllers_file") - can_interface = LaunchConfiguration("can_interface") - arm_prefix = LaunchConfiguration("arm_prefix") - # Configuration file paths - controllers_file = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", - "v10_controllers", controllers_file] - ) - - # Robot nodes spawner (both state publisher and control) - robot_nodes_spawner_func = OpaqueFunction( - function=robot_nodes_spawner, - args=[description_package, description_file, arm_type, - use_fake_hardware, controllers_file, can_interface, arm_prefix] - ) - # RViz configuration - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", - "arm_only.rviz"] - ) - - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - # Joint state broadcaster spawner - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", - "--controller-manager", "/controller_manager"], - ) - - # Controller spawners - robot_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[robot_controller, "-c", "/controller_manager"], - ) - - gripper_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["gripper_controller", "-c", "/controller_manager"], - ) - - # Timing and sequencing - delayed_joint_state_broadcaster = TimerAction( - period=1.0, - actions=[joint_state_broadcaster_spawner], - ) - - delayed_robot_controller = TimerAction( - period=1.0, - actions=[robot_controller_spawner], - ) - delayed_gripper_controller = TimerAction( - period=1.0, - actions=[gripper_controller_spawner], - ) - - return LaunchDescription( - declared_arguments + [ - robot_nodes_spawner_func, - rviz_node, - ] + - [ - delayed_joint_state_broadcaster, - delayed_robot_controller, - delayed_gripper_controller, - ] - ) diff --git a/openarm_bringup/package.xml b/openarm_bringup/package.xml index 773de5ad..4f9ec12e 100644 --- a/openarm_bringup/package.xml +++ b/openarm_bringup/package.xml @@ -26,6 +26,9 @@ ros2_controllers controller_manager + openarm_bimanual_moveit_config + openarm_description + rviz2 ament_lint_auto ament_lint_common diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index 1ed46920..15a23815 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -30,7 +30,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(OpenArmCAN REQUIRED) add_library(${PROJECT_NAME} SHARED - src/v10_simple_hardware.cpp + src/openarm_simple_hardware.cpp ) target_include_directories( diff --git a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_simple_hardware.hpp similarity index 94% rename from openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp rename to openarm_hardware/include/openarm_hardware/openarm_simple_hardware.hpp index 1130df20..60df58cc 100644 --- a/openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/openarm_simple_hardware.hpp @@ -38,9 +38,9 @@ namespace openarm_hardware { * following the pattern from full_arm.cpp example. Much simpler than * the original implementation. */ -class OpenArm_v10HW : public hardware_interface::SystemInterface { +class OpenArmHW : public hardware_interface::SystemInterface { public: - OpenArm_v10HW(); + OpenArmHW(); TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC hardware_interface::CallbackReturn on_init( @@ -117,6 +117,7 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface { // Configuration std::string can_interface_; std::string arm_prefix_; + std::string ee_type_; bool hand_; bool can_fd_; @@ -134,6 +135,16 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface { std::vector vel_states_; std::vector tau_states_; + static constexpr std::array ZERO_POSITION = { + 0.0, // joint1 + 0.0, // joint2 + 0.0, // joint3 + 0.0, // joint4 + 0.0, // joint5 + 0.0, // joint6 + 0.0, // joint7 + }; + // Helper methods void return_to_zero(); bool parse_config(const hardware_interface::HardwareInfo& info); diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml index fd4fee92..83315567 100644 --- a/openarm_hardware/openarm_hardware.xml +++ b/openarm_hardware/openarm_hardware.xml @@ -15,11 +15,11 @@ --> - - ros2_control hardware interface for OpenArm V10. + ros2_control hardware interface for OpenArm . diff --git a/openarm_hardware/src/v10_simple_hardware.cpp b/openarm_hardware/src/openarm_simple_hardware.cpp similarity index 68% rename from openarm_hardware/src/v10_simple_hardware.cpp rename to openarm_hardware/src/openarm_simple_hardware.cpp index 2ddaeacb..9321c4a8 100644 --- a/openarm_hardware/src/v10_simple_hardware.cpp +++ b/openarm_hardware/src/openarm_simple_hardware.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "openarm_hardware/v10_simple_hardware.hpp" +#include "openarm_hardware/openarm_simple_hardware.hpp" #include #include @@ -26,9 +26,9 @@ namespace openarm_hardware { -OpenArm_v10HW::OpenArm_v10HW() = default; +OpenArmHW::OpenArmHW() = default; -bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { +bool OpenArmHW::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"; @@ -71,6 +71,10 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { kd_[i - 1] = std::stod(it->second); } } + // Parse ee_type (default: parallel_link for v10) + it = info.hardware_parameters.find("ee_type"); + ee_type_ = + (it != info.hardware_parameters.end()) ? it->second : "parallel_link"; if (hand_) { it = info.hardware_parameters.find("kp_hand"); if (it != info.hardware_parameters.end()) { @@ -82,14 +86,14 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) { } } - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s", can_interface_.c_str(), arm_prefix_.c_str(), hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); return true; } -void OpenArm_v10HW::generate_joint_names() { +void OpenArmHW::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 @@ -104,19 +108,19 @@ void OpenArm_v10HW::generate_joint_names() { 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_v10HW"), "Added gripper joint: %s", + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Added gripper joint: %s", gripper_joint_name.c_str()); } else { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Gripper joint NOT added because hand_=false"); } - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Generated %zu joint names for arm prefix '%s'", joint_names_.size(), arm_prefix_.c_str()); } -hardware_interface::CallbackReturn OpenArm_v10HW::on_init( +hardware_interface::CallbackReturn OpenArmHW::on_init( const hardware_interface::HardwareInfo& info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { @@ -133,14 +137,14 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_init( // Validate joint count (7 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_v10HW"), + RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), "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_v10HW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Initializing OpenArm on %s with CAN-FD %s...", can_interface_.c_str(), can_fd_ ? "enabled" : "disabled"); openarm_ = @@ -152,7 +156,7 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_init( // Initialize gripper if enabled if (hand_) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Initializing gripper..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Initializing gripper..."); openarm_->init_gripper_motor(DEFAULT_GRIPPER_MOTOR_TYPE, DEFAULT_GRIPPER_SEND_CAN_ID, DEFAULT_GRIPPER_RECV_CAN_ID); @@ -167,13 +171,13 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_init( vel_states_.resize(total_joints, 0.0); tau_states_.resize(total_joints, 0.0); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "OpenArm V10 Simple HW initialized successfully"); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArm_v10HW::on_configure( +hardware_interface::CallbackReturn OpenArmHW::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { // Set callback mode to ignore during configuration openarm_->refresh_all(); @@ -184,7 +188,7 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_configure( } std::vector -OpenArm_v10HW::export_state_interfaces() { +OpenArmHW::export_state_interfaces() { std::vector state_interfaces; for (size_t i = 0; i < joint_names_.size(); ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface( @@ -199,7 +203,7 @@ OpenArm_v10HW::export_state_interfaces() { } std::vector -OpenArm_v10HW::export_command_interfaces() { +OpenArmHW::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) { @@ -216,9 +220,9 @@ OpenArm_v10HW::export_command_interfaces() { return command_interfaces; } -hardware_interface::CallbackReturn OpenArm_v10HW::on_activate( +hardware_interface::CallbackReturn OpenArmHW::on_activate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Activating OpenArm V10..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Activating OpenArm V10..."); openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); openarm_->enable_all(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -227,25 +231,26 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_activate( // Return to zero position return_to_zero(); - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 activated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "OpenArm V10 activated"); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn OpenArm_v10HW::on_deactivate( +hardware_interface::CallbackReturn OpenArmHW::on_deactivate( const rclcpp_lifecycle::State& /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), - "Deactivating OpenArm V10..."); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Deactivating OpenArm V10..."); // Disable all motors (like full_arm.cpp exit) - openarm_->disable_all(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - openarm_->recv_all(); + for (int i = 0; i < 3; ++i) { + openarm_->disable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm_->recv_all(); + } - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 deactivated"); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "OpenArm V10 deactivated"); return CallbackReturn::SUCCESS; } -hardware_interface::return_type OpenArm_v10HW::read( +hardware_interface::return_type OpenArmHW::read( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Receive all motor states openarm_->refresh_all(); @@ -277,7 +282,7 @@ hardware_interface::return_type OpenArm_v10HW::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type OpenArm_v10HW::write( +hardware_interface::return_type OpenArmHW::write( const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { // Control arm motors with MIT control std::vector arm_params; @@ -293,14 +298,14 @@ hardware_interface::return_type OpenArm_v10HW::write( openarm_->get_gripper().mit_control_all( {{gripper_kp_, gripper_kd_, motor_command, 0.0, 0.0}}); } - openarm_->recv_all(1000); + openarm_->recv_all(100); return hardware_interface::return_type::OK; } -void OpenArm_v10HW::return_to_zero() { - RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), - "Returning to zero position..."); +void OpenArmHW::return_to_zero() { + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Returning to zero position..."); + openarm_->refresh_all(); // Return arm to zero with MIT control std::vector arm_params; for (size_t i = 0; i < ARM_DOF; ++i) { @@ -315,25 +320,95 @@ void OpenArm_v10HW::return_to_zero() { } std::this_thread::sleep_for(std::chrono::microseconds(1000)); openarm_->recv_all(); + const auto& arm_motors = openarm_->get_arm().get_motors(); + + std::vector start_pos(ARM_DOF, 0.0); + for (size_t i = 0; i < ARM_DOF && i < arm_motors.size(); ++i) { + start_pos[i] = arm_motors[i].get_position(); + } + + const int steps = 200; + const int step_ms = 10; + + for (int step = 0; step <= steps; ++step) { + double t = static_cast(step) / steps; // 0.0 → 1.0 + + std::vector arm_params; + for (size_t i = 0; i < ARM_DOF; ++i) { + double target = start_pos[i] + t * (ZERO_POSITION[i] - start_pos[i]); + arm_params.push_back({kp_[i], kd_[i], target, 0.0, 0.0}); + } + openarm_->get_arm().mit_control_all(arm_params); + + if (hand_) { + openarm_->get_gripper().mit_control_all( + {{GRIPPER_KP, GRIPPER_KD, GRIPPER_JOINT_0_POSITION, 0.0, 0.0}}); + } + + openarm_->recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(step_ms)); + } + + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Reached zero position"); } -// Gripper mapping helper functions -double OpenArm_v10HW::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 +// void OpenArmHW::return_to_zero() { +// RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Returning to zero +// position..."); + +// // Return arm to zero with MIT control +// std::vector arm_params; +// for (size_t i = 0; i < ARM_DOF; ++i) { +// arm_params.push_back({kp_[i], kd_[i], 0.0, 0.0, 0.0}); +// } +// openarm_->get_arm().mit_control_all(arm_params); + +// // Return gripper to zero if enabled +// if (hand_) { +// openarm_->get_gripper().mit_control_all( +// {{GRIPPER_KP, GRIPPER_KD, GRIPPER_JOINT_0_POSITION, 0.0, 0.0}}); +// } +// std::this_thread::sleep_for(std::chrono::microseconds(1000)); +// openarm_->recv_all(); +// } + +double OpenArmHW::joint_to_motor_radians(double joint_value) { + if (ee_type_ == "pinch_gripper") { + // revolute: joint 0-1.5708 rad -> motor 0-1.5708 + return joint_value; + } else { + // parallel_link (prismatic): 0-0.044m -> 0 to -1.0472 rad + return (joint_value / GRIPPER_JOINT_0_POSITION) * GRIPPER_MOTOR_1_RADIANS; + } } -double OpenArm_v10HW::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 +double OpenArmHW::motor_radians_to_joint(double motor_radians) { + if (ee_type_ == "pinch_gripper") { + // revolute: + return motor_radians; + } else { + // parallel_link (prismatic) + return GRIPPER_JOINT_0_POSITION * (motor_radians / GRIPPER_MOTOR_1_RADIANS); + } } +// // Gripper mapping helper functions +// double OpenArmHW::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 OpenArmHW::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_v10HW, +PLUGINLIB_EXPORT_CLASS(openarm_hardware::OpenArmHW, hardware_interface::SystemInterface)