diff --git a/openarm_bimanual_moveit_config/.setup_assistant b/openarm_bimanual_moveit_config/.setup_assistant new file mode 100644 index 0000000..1f98b90 --- /dev/null +++ b/openarm_bimanual_moveit_config/.setup_assistant @@ -0,0 +1,42 @@ +# 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. + +moveit_setup_assistant_config: + urdf: + package: openarm_description + relative_path: urdf/robot/v10.urdf.xacro + xacro_args: bimanual:=true + srdf: + relative_path: config/openarm_bimanual.srdf + package_settings: + author_name: Enactic, Inc. + author_email: openarm_dev@enactic.ai + generated_timestamp: 1753031250 + control_xacro: + command: + - position + - velocity + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + - velocity + state: + - position + - velocity \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/CMakeLists.txt b/openarm_bimanual_moveit_config/CMakeLists.txt new file mode 100644 index 0000000..85c3d7b --- /dev/null +++ b/openarm_bimanual_moveit_config/CMakeLists.txt @@ -0,0 +1,30 @@ +# 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. + +cmake_minimum_required(VERSION 3.22) +project(openarm_bimanual_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +endif() + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/openarm_bimanual_moveit_config/config/initial_positions.yaml b/openarm_bimanual_moveit_config/config/initial_positions.yaml new file mode 100644 index 0000000..4c87cab --- /dev/null +++ b/openarm_bimanual_moveit_config/config/initial_positions.yaml @@ -0,0 +1,32 @@ +# 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. + +# 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 diff --git a/openarm_bimanual_moveit_config/config/joint_limits.yaml b/openarm_bimanual_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..995f999 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/joint_limits.yaml @@ -0,0 +1,104 @@ +# 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. + +# 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 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/kinematics.yaml b/openarm_bimanual_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..c1d6d23 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/kinematics.yaml @@ -0,0 +1,24 @@ +# 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. + + +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/moveit.rviz b/openarm_bimanual_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..3f2746d --- /dev/null +++ b/openarm_bimanual_moveit_config/config/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/moveit_controllers.yaml b/openarm_bimanual_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..8cece8a --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,62 @@ +# 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. + + +# 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_bimanual.srdf b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf new file mode 100644 index 0000000..8c2c6e1 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf @@ -0,0 +1,252 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro new file mode 100644 index 0000000..6f6f495 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + diff --git a/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml b/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..61b7e41 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,20 @@ +# 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. + +# 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 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..de64f00 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml @@ -0,0 +1,85 @@ +# 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 + + 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 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/sensors_3d.yaml b/openarm_bimanual_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..01b9ef2 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,27 @@ +# 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 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/launch/demo.launch.py b/openarm_bimanual_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..5ed6731 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -0,0 +1,260 @@ +# Copyright 2025 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import xacro +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_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +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) + + xacro_path = os.path.join( + get_package_share_directory(description_package_str), + "urdf", + "robot", + description_file_str, + ) + + robot_description = xacro.process_file( + xacro_path, + mappings={ + "arm_type": arm_type_str, + "bimanual": "true", + "use_fake_hardware": use_fake_hardware_str, + "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, + right_can_interface, + left_can_interface, + arm_prefix, +): + 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) + robot_description_param = {"robot_description": robot_description} + + 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 = 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 controller_spawner(context: LaunchContext, robot_controller): + robot_controller_str = context.perform_substitution(robot_controller) + + if robot_controller_str == "forward_position_controller": + left = "left_forward_position_controller" + right = "right_forward_position_controller" + elif robot_controller_str == "joint_trajectory_controller": + left = "left_joint_trajectory_controller" + right = "right_joint_trajectory_controller" + else: + raise ValueError(f"Unknown robot_controller: {robot_controller_str}") + + return [ + Node( + package="controller_manager", + executable="spawner", + arguments=[left, right, "-c", "/controller_manager"], + ) + ] + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "description_package", + default_value="openarm_description", + ), + DeclareLaunchArgument( + "description_file", + default_value="v10.urdf.xacro", + ), + DeclareLaunchArgument("arm_type", default_value="v10"), + DeclareLaunchArgument("use_fake_hardware", default_value="false"), + DeclareLaunchArgument( + "robot_controller", + default_value="joint_trajectory_controller", + choices=["forward_position_controller", + "joint_trajectory_controller"], + ), + 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", + ), + ] + + 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") + right_can_interface = LaunchConfiguration("right_can_interface") + left_can_interface = LaunchConfiguration("left_can_interface") + arm_prefix = LaunchConfiguration("arm_prefix") + + controllers_file = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", + "v10_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, + ], + ) + + jsb_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", + "--controller-manager", "/controller_manager"], + ) + + controller_spawner_func = OpaqueFunction( + function=controller_spawner, args=[robot_controller]) + + gripper_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["left_gripper_controller", + "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, + ] + ) diff --git a/openarm_bimanual_moveit_config/launch/move_group.launch.py b/openarm_bimanual_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..ec098d2 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/move_group.launch.py @@ -0,0 +1,8 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py b/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..1bed84f --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,8 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/openarm_bimanual_moveit_config/launch/setup_assistant.launch.py b/openarm_bimanual_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..fbc2c52 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,8 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py b/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..d5ecc3c --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,8 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + "openarm", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) 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 new file mode 100644 index 0000000..9b82fdc --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,8 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +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) diff --git a/openarm_bimanual_moveit_config/package.xml b/openarm_bimanual_moveit_config/package.xml new file mode 100644 index 0000000..cb49516 --- /dev/null +++ b/openarm_bimanual_moveit_config/package.xml @@ -0,0 +1,58 @@ + + + + + openarm_bimanual_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the openarm with the MoveIt Motion Planning Framework + + Enactic, Inc. + + Apache-2.0 + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Enactic, Inc. + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + controller_manager + moveit_configs_utils + moveit_ros_visualization + moveit_setup_assistant + openarm_description + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + + + + ament_cmake + + diff --git a/openarm_bringup/README.md b/openarm_bringup/README.md index 391f8f7..a8dc1a9 100644 --- a/openarm_bringup/README.md +++ b/openarm_bringup/README.md @@ -7,7 +7,7 @@ This package provides launch files to bring up the OpenArm robot system. Launch the OpenArm with v1.0 configuration and fake hardware: ```bash -ros2 launch openarm_bringup openarm.launch.py arm_type:=v10 use_fake_hardware:=true +ros2 launch openarm_bringup openarm.launch.py arm_type:=v10 hardware_type:=real ``` ## Launch Files @@ -18,7 +18,7 @@ ros2 launch openarm_bringup openarm.launch.py arm_type:=v10 use_fake_hardware:=t ## Key Parameters - `arm_type` - Arm type (default: v10) -- `use_fake_hardware` - Use fake hardware instead of real hardware (default: false) +- `hardware_type` - Use real/mock/mujoco hardware (default: real) - `can_interface` - CAN interface to use (default: can0) - `robot_controller` - Controller type: `joint_trajectory_controller` or `forward_position_controller` diff --git a/openarm_bringup/launch/openarm.bimanual.launch.py b/openarm_bringup/launch/openarm.bimanual.launch.py index 99250a5..b029a5c 100644 --- a/openarm_bringup/launch/openarm.bimanual.launch.py +++ b/openarm_bringup/launch/openarm.bimanual.launch.py @@ -37,13 +37,13 @@ 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): + arm_type, hardware_type, 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) + hardware_type_str = context.perform_substitution(hardware_type) right_can_interface_str = context.perform_substitution(right_can_interface) left_can_interface_str = context.perform_substitution(left_can_interface) @@ -58,7 +58,7 @@ def generate_robot_description(context: LaunchContext, description_package, desc mappings={ "arm_type": arm_type_str, "bimanual": "true", - "use_fake_hardware": use_fake_hardware_str, + "hardware_type": hardware_type_str, "ros2_control": "true", "right_can_interface": right_can_interface_str, "left_can_interface": left_can_interface_str, @@ -69,12 +69,12 @@ 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, hardware_type, 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, hardware_type, right_can_interface, left_can_interface, ) controllers_file_str = context.perform_substitution(controllers_file) @@ -153,9 +153,9 @@ def generate_launch_description(): description="Type of arm (e.g., v10).", ), DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Use fake hardware instead of real hardware.", + "hardware_type", + default_value="real", + description="Use real/mock/mujoco hardware.", ), DeclareLaunchArgument( "robot_controller", @@ -195,7 +195,7 @@ def generate_launch_description(): description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") arm_type = LaunchConfiguration("arm_type") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") + hardware_type = LaunchConfiguration("hardware_type") robot_controller = LaunchConfiguration("robot_controller") runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") @@ -211,7 +211,7 @@ def generate_launch_description(): robot_nodes_spawner_func = OpaqueFunction( function=robot_nodes_spawner, args=[description_package, description_file, arm_type, - use_fake_hardware, controllers_file, rightcan_interface, left_can_interface, arm_prefix] + hardware_type, controllers_file, rightcan_interface, left_can_interface, arm_prefix] ) rviz_config_file = PathJoinSubstitution( diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py index 1c7a8b4..b257b39 100644 --- a/openarm_bringup/launch/openarm.launch.py +++ b/openarm_bringup/launch/openarm.launch.py @@ -30,14 +30,14 @@ 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): + arm_type, hardware_type, 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) + hardware_type_str = context.perform_substitution(hardware_type) can_interface_str = context.perform_substitution(can_interface) arm_prefix_str = context.perform_substitution(arm_prefix) @@ -53,7 +53,7 @@ def generate_robot_description(context: LaunchContext, description_package, desc mappings={ "arm_type": arm_type_str, "bimanual": "false", - "use_fake_hardware": use_fake_hardware_str, + "hardware_type": hardware_type_str, "ros2_control": "true", "can_interface": can_interface_str, "arm_prefix": arm_prefix_str, @@ -64,12 +64,12 @@ 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, can_interface, arm_prefix): + arm_type, hardware_type, 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 + context, description_package, description_file, arm_type, hardware_type, can_interface, arm_prefix ) # Get controllers file path @@ -117,9 +117,9 @@ def generate_launch_description(): description="Type of arm (e.g., v10).", ), DeclareLaunchArgument( - "use_fake_hardware", - default_value="false", - description="Use fake hardware instead of real hardware.", + "hardware_type", + default_value="real", + description="Use real/mock/mujoco hardware.", ), DeclareLaunchArgument( "robot_controller", @@ -154,7 +154,7 @@ def generate_launch_description(): description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") arm_type = LaunchConfiguration("arm_type") - use_fake_hardware = LaunchConfiguration("use_fake_hardware") + hardware_type = LaunchConfiguration("hardware_type") robot_controller = LaunchConfiguration("robot_controller") runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") @@ -170,7 +170,7 @@ def generate_launch_description(): 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] + hardware_type, controllers_file, can_interface, arm_prefix] ) # RViz configuration rviz_config_file = PathJoinSubstitution(