From 596c4985988037777e0a5b8a9c0f99c5c0869dd5 Mon Sep 17 00:00:00 2001 From: thomason <84239247+thomasonzhou@users.noreply.github.com> Date: Thu, 10 Apr 2025 18:31:15 +0900 Subject: [PATCH] Add openarm_bimanual_moveit_config and deploy to physical openarm (#3) See https://github.com/reazon-research/openarm_ros2/pull/3 for video --- .gitignore | 2 + .../launch/description.launch.py | 2 +- .../launch/display.launch.py | 2 +- openarm_bimanual_description/package.xml | 1 + .../urdf/openarm_bimanual.urdf | 824 ++++++++++++------ .../urdf/openarm_bimanual.urdf.xacro | 22 +- .../urdf/openarm_bimanual_sensors.xacro | 5 +- .../urdf/openarm_bimanual_wrapper.urdf.xacro | 5 - .../.setup_assistant | 10 + openarm_bimanual_moveit_config/CMakeLists.txt | 16 + .../config/initial_positions.yaml | 17 + .../config/joint_limits.yaml | 90 ++ .../config/kinematics.yaml | 4 + .../config/moveit.rviz | 51 ++ .../config/moveit_controllers.yaml | 33 + .../config/openarm_bimanual.srdf | 223 +++++ .../config/openarm_bimanual.urdf.xacro | 8 + .../config/pilz_cartesian_limits.yaml | 6 + .../config/ros2_controllers.yaml | 48 + .../config/sensors_3d.yaml | 25 + .../launch/demo.launch.py | 7 + .../launch/move_group.launch.py | 7 + .../launch/moveit_rviz.launch.py | 7 + .../launch/rsp.launch.py | 7 + .../launch/setup_assistant.launch.py | 7 + .../launch/spawn_controllers.launch.py | 7 + .../launch/static_virtual_joint_tfs.launch.py | 7 + .../launch/warehouse_db.launch.py | 7 + openarm_bimanual_moveit_config/package.xml | 49 ++ openarm_bimanual_teleop/CMakeLists.txt | 30 + openarm_bimanual_teleop/LICENSE | 25 + .../config/controllers.yaml | 50 ++ .../launch/depth_camera.launch.py | 15 + .../launch/start_teleop.launch.py | 83 ++ openarm_bimanual_teleop/package.xml | 18 + openarm_bringup/launch/openarm.launch.py | 2 +- openarm_bringup/utils/init_can.sh | 15 + openarm_description/CMakeLists.txt | 2 +- .../{urdf => config}/initial_positions.yaml | 0 .../launch/description.launch.py | 42 +- .../urdf/openarm.ros2_control.xacro | 44 +- openarm_description/urdf/openarm.urdf | 22 +- openarm_description/urdf/openarm.urdf.xacro | 4 +- openarm_description/urdf/openarm.xacro | 38 +- openarm_hardware/CMakeLists.txt | 42 +- .../openarm_hardware/openarm_hardware.hpp | 30 +- openarm_hardware/src/canbus.cpp | 2 +- openarm_hardware/src/motor.cpp | 2 +- openarm_hardware/src/motor_control.cpp | 4 +- openarm_hardware/src/openarm_hardware.cpp | 140 ++- openarm_moveit_config/.setup_assistant | 23 +- .../config/initial_positions.yaml | 3 +- .../config/joint_limits.yaml | 5 - openarm_moveit_config/config/moveit.rviz | 4 +- .../config/moveit_controllers.yaml | 2 + openarm_moveit_config/config/openarm.srdf | 2 +- .../config/openarm.urdf.xacro | 3 +- .../config/ros2_controllers.yaml | 5 +- openarm_moveit_config/config/sensors_3d.yaml | 25 + openarm_moveit_config/package.xml | 7 +- 60 files changed, 1750 insertions(+), 438 deletions(-) delete mode 100644 openarm_bimanual_description/urdf/openarm_bimanual_wrapper.urdf.xacro create mode 100644 openarm_bimanual_moveit_config/.setup_assistant create mode 100644 openarm_bimanual_moveit_config/CMakeLists.txt create mode 100644 openarm_bimanual_moveit_config/config/initial_positions.yaml create mode 100644 openarm_bimanual_moveit_config/config/joint_limits.yaml create mode 100644 openarm_bimanual_moveit_config/config/kinematics.yaml create mode 100644 openarm_bimanual_moveit_config/config/moveit.rviz create mode 100644 openarm_bimanual_moveit_config/config/moveit_controllers.yaml create mode 100644 openarm_bimanual_moveit_config/config/openarm_bimanual.srdf create mode 100644 openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro create mode 100644 openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 openarm_bimanual_moveit_config/config/ros2_controllers.yaml create mode 100644 openarm_bimanual_moveit_config/config/sensors_3d.yaml create mode 100644 openarm_bimanual_moveit_config/launch/demo.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/move_group.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/rsp.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/setup_assistant.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py create mode 100644 openarm_bimanual_moveit_config/launch/warehouse_db.launch.py create mode 100644 openarm_bimanual_moveit_config/package.xml create mode 100644 openarm_bimanual_teleop/CMakeLists.txt create mode 100644 openarm_bimanual_teleop/LICENSE create mode 100644 openarm_bimanual_teleop/config/controllers.yaml create mode 100644 openarm_bimanual_teleop/launch/depth_camera.launch.py create mode 100644 openarm_bimanual_teleop/launch/start_teleop.launch.py create mode 100644 openarm_bimanual_teleop/package.xml create mode 100755 openarm_bringup/utils/init_can.sh rename openarm_description/{urdf => config}/initial_positions.yaml (100%) create mode 100644 openarm_moveit_config/config/sensors_3d.yaml diff --git a/.gitignore b/.gitignore index e43b0f9..508562d 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ .DS_Store +.vscode/ +__pycache__/ \ No newline at end of file diff --git a/openarm_bimanual_description/launch/description.launch.py b/openarm_bimanual_description/launch/description.launch.py index 9fc9dcd..337a4ab 100644 --- a/openarm_bimanual_description/launch/description.launch.py +++ b/openarm_bimanual_description/launch/description.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): "openarm_bimanual_description" ) ) - default_model_path = pkg_share / "urdf/openarm_bimanual_wrapper.urdf.xacro" + default_model_path = pkg_share / "urdf/openarm_bimanual.urdf.xacro" use_sim_time = LaunchConfiguration("use_sim_time") use_sim_time_launch_arg = DeclareLaunchArgument("use_sim_time", default_value="true") diff --git a/openarm_bimanual_description/launch/display.launch.py b/openarm_bimanual_description/launch/display.launch.py index 705ed50..49fefab 100644 --- a/openarm_bimanual_description/launch/display.launch.py +++ b/openarm_bimanual_description/launch/display.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): "openarm_bimanual_description" ) ) - default_model_path = pkg_share / "urdf/openarm_bimanual_wrapper.urdf.xacro" + default_model_path = pkg_share / "urdf/openarm_bimanual.urdf.xacro" default_rviz_config_path = pkg_share / "rviz/robot_description.rviz" use_sim_time = LaunchConfiguration("use_sim_time") diff --git a/openarm_bimanual_description/package.xml b/openarm_bimanual_description/package.xml index e781837..7cf14cd 100644 --- a/openarm_bimanual_description/package.xml +++ b/openarm_bimanual_description/package.xml @@ -11,6 +11,7 @@ joint_state_publisher_gui ros_gz + realsense2_description ament_lint_auto ament_lint_common diff --git a/openarm_bimanual_description/urdf/openarm_bimanual.urdf b/openarm_bimanual_description/urdf/openarm_bimanual.urdf index 2c11513..9378dbb 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual.urdf +++ b/openarm_bimanual_description/urdf/openarm_bimanual.urdf @@ -4,10 +4,12 @@ - + + + @@ -30,276 +32,210 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - - - - + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + + + - - - - - - + + + + + + - - - - - - + + + + + - - - - - - + + + + + + - - - - - - + + + + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + openarm_hardware/OpenArmHW + left_ + can1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -532,7 +468,7 @@ - + @@ -546,7 +482,7 @@ - + @@ -554,29 +490,371 @@ - + - + - - - - - - - - - - + + + + + + + + + + + openarm_hardware/OpenArmHW + right_ + can0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro b/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro index 0d7050f..536cafd 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro +++ b/openarm_bimanual_description/urdf/openarm_bimanual.urdf.xacro @@ -1,25 +1,29 @@ - - - + + - - - - - + + - + + + + + + + + + diff --git a/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro b/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro index 063dc54..24d718e 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro +++ b/openarm_bimanual_description/urdf/openarm_bimanual_sensors.xacro @@ -1,5 +1,8 @@ - + + + + diff --git a/openarm_bimanual_description/urdf/openarm_bimanual_wrapper.urdf.xacro b/openarm_bimanual_description/urdf/openarm_bimanual_wrapper.urdf.xacro deleted file mode 100644 index e7a860c..0000000 --- a/openarm_bimanual_description/urdf/openarm_bimanual_wrapper.urdf.xacro +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/openarm_bimanual_moveit_config/.setup_assistant b/openarm_bimanual_moveit_config/.setup_assistant new file mode 100644 index 0000000..2cfa29b --- /dev/null +++ b/openarm_bimanual_moveit_config/.setup_assistant @@ -0,0 +1,10 @@ +moveit_setup_assistant_config: + urdf: + package: openarm_bimanual_description + relative_path: urdf/openarm_bimanual.urdf.xacro + srdf: + relative_path: config/openarm_bimanual.srdf + package_settings: + author_name: Thomason Zhou + author_email: t95zhou@uwaterloo.ca + generated_timestamp: 1744266716 \ 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..b2d1fc9 --- /dev/null +++ b/openarm_bimanual_moveit_config/CMakeLists.txt @@ -0,0 +1,16 @@ +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..953e855 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/initial_positions.yaml @@ -0,0 +1,17 @@ +# Default initial positions for openarm_bimanual's ros2_control fake system + +initial_positions: + left_rev1: 0 + left_rev2: 0 + left_rev3: 0 + left_rev4: 0 + left_rev5: 0 + left_rev6: 0 + left_rev7: 0 + right_rev1: 0 + right_rev2: 0 + right_rev3: 0 + right_rev4: 0 + right_rev5: 0 + right_rev6: 0 + right_rev7: 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..972282b --- /dev/null +++ b/openarm_bimanual_moveit_config/config/joint_limits.yaml @@ -0,0 +1,90 @@ +# 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: + left_left_pris1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev5: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev6: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_rev7: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + left_right_pris2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev5: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev6: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + right_rev7: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 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..6f91597 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +left_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/openarm_bimanual_moveit_config/config/moveit.rviz b/openarm_bimanual_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..f31651e --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 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..3584579 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,33 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - left_arm_controller + - right_arm_controller + + left_arm_controller: + type: FollowJointTrajectory + joints: + - left_rev1 + - left_rev2 + - left_rev3 + - left_rev4 + - left_rev5 + - left_rev6 + - left_rev7 + action_ns: follow_joint_trajectory + default: true + right_arm_controller: + type: FollowJointTrajectory + joints: + - right_rev1 + - right_rev2 + - right_rev3 + - right_rev4 + - right_rev5 + - right_rev6 + - right_rev7 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file 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..32cec20 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.srdf @@ -0,0 +1,223 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..0fbc43d --- /dev/null +++ b/openarm_bimanual_moveit_config/config/openarm_bimanual.urdf.xacro @@ -0,0 +1,8 @@ + + + + + + + + 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..b2997ca --- /dev/null +++ b/openarm_bimanual_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# 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/ros2_controllers.yaml b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..0461344 --- /dev/null +++ b/openarm_bimanual_moveit_config/config/ros2_controllers.yaml @@ -0,0 +1,48 @@ +# 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 + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +left_arm_controller: + ros__parameters: + joints: + - left_rev1 + - left_rev2 + - left_rev3 + - left_rev4 + - left_rev5 + - left_rev6 + - left_rev7 + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity +right_arm_controller: + ros__parameters: + joints: + - right_rev1 + - right_rev2 + - right_rev3 + - right_rev4 + - right_rev5 + - right_rev6 + - right_rev7 + command_interfaces: + - position + - velocity + state_interfaces: + - position + - velocity \ 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..99dcaeb --- /dev/null +++ b/openarm_bimanual_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,25 @@ +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..dd1e01f --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) 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..b9d4183 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", 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..27933bd --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/openarm_bimanual_moveit_config/launch/rsp.launch.py b/openarm_bimanual_moveit_config/launch/rsp.launch.py new file mode 100644 index 0000000..d80af03 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_rsp_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..88b6d5b --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", 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..2723c17 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", 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..556a736 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +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_bimanual", 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/launch/warehouse_db.launch.py b/openarm_bimanual_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..e392922 --- /dev/null +++ b/openarm_bimanual_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("openarm_bimanual", package_name="openarm_bimanual_moveit_config").to_moveit_configs() + return generate_warehouse_db_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..f317b88 --- /dev/null +++ b/openarm_bimanual_moveit_config/package.xml @@ -0,0 +1,49 @@ + + + + openarm_bimanual_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the openarm_bimanual with the MoveIt Motion Planning Framework + + Thomason Zhou + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Thomason Zhou + + 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_move_group + moveit_ros_visualization + moveit_setup_assistant + openarm_bimanual_description + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + + + + ament_cmake + + diff --git a/openarm_bimanual_teleop/CMakeLists.txt b/openarm_bimanual_teleop/CMakeLists.txt new file mode 100644 index 0000000..66eddf0 --- /dev/null +++ b/openarm_bimanual_teleop/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(openarm_bimanual_teleop) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/openarm_bimanual_teleop/LICENSE b/openarm_bimanual_teleop/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/openarm_bimanual_teleop/LICENSE @@ -0,0 +1,25 @@ +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/openarm_bimanual_teleop/config/controllers.yaml b/openarm_bimanual_teleop/config/controllers.yaml new file mode 100644 index 0000000..4d85899 --- /dev/null +++ b/openarm_bimanual_teleop/config/controllers.yaml @@ -0,0 +1,50 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broad: + type: joint_state_broadcaster/JointStateBroadcaster + +left_arm_controller: + ros__parameters: + joints: + - left_rev1 + - left_rev2 + - left_rev3 + - left_rev4 + - left_rev5 + - left_rev6 + - left_rev7 + command_interfaces: + - position + - velocity + - effort + state_interfaces: + - position + - velocity + - effort +right_arm_controller: + ros__parameters: + joints: + - right_rev1 + - right_rev2 + - right_rev3 + - right_rev4 + - right_rev5 + - right_rev6 + - right_rev7 + command_interfaces: + - position + - velocity + - effort + state_interfaces: + - position + - velocity + - effort \ No newline at end of file diff --git a/openarm_bimanual_teleop/launch/depth_camera.launch.py b/openarm_bimanual_teleop/launch/depth_camera.launch.py new file mode 100644 index 0000000..9c9016d --- /dev/null +++ b/openarm_bimanual_teleop/launch/depth_camera.launch.py @@ -0,0 +1,15 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + realsense_share_dir = FindPackageShare("realsense2_camera") + rs_launch_path = [realsense_share_dir, "/launch/rs_launch.py"] + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(rs_launch_path), + launch_arguments={"pointcloud.enable": "true"}.items() + ) + ]) diff --git a/openarm_bimanual_teleop/launch/start_teleop.launch.py b/openarm_bimanual_teleop/launch/start_teleop.launch.py new file mode 100644 index 0000000..643c1d3 --- /dev/null +++ b/openarm_bimanual_teleop/launch/start_teleop.launch.py @@ -0,0 +1,83 @@ +import launch +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch_ros.parameter_descriptions import ParameterValue +from launch.launch_description_sources import PythonLaunchDescriptionSource +import pathlib + +def generate_launch_description(): + + pkg_share = FindPackageShare(package="openarm_bimanual_description") + + + xacro_path = pathlib.Path(pkg_share.find( + "openarm_bimanual_description" + )) / "urdf/openarm_bimanual.urdf.xacro" + + use_sim_time = LaunchConfiguration("use_sim_time") + use_sim_time_launch_arg = DeclareLaunchArgument(name="use_sim_time", default_value="false") + + robot_state_publisher_node = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + pkg_share, + "launch", + "description.launch.py", + ] + ), + ] + ), + launch_arguments=dict(use_sim_time=use_sim_time).items(), + ) + + + robot_description_content = Command([ + 'xacro ', LaunchConfiguration("model") + ]) + + robot_description_param = {'robot_description': ParameterValue(robot_description_content, value_type=str)} + + controller_params = PathJoinSubstitution([ + FindPackageShare(package='openarm_bimanual_teleop'), + 'config', + 'controllers.yaml' + ]) + + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[controller_params, robot_description_param],) + + delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager]) + + joint_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broad'], + ) + + delayed_joint_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=controller_manager, + on_start=[joint_broadcaster_spawner], + ) + ) + + return launch.LaunchDescription( + [ + DeclareLaunchArgument( + name="model", + default_value=str(xacro_path), + description="Absolute path to the robot URDF or xacro file" + ), + use_sim_time_launch_arg, + robot_state_publisher_node, + delayed_controller_manager, + delayed_joint_broadcaster_spawner + ] + ) diff --git a/openarm_bimanual_teleop/package.xml b/openarm_bimanual_teleop/package.xml new file mode 100644 index 0000000..ee305d6 --- /dev/null +++ b/openarm_bimanual_teleop/package.xml @@ -0,0 +1,18 @@ + + + + openarm_bimanual_teleop + 0.0.0 + Teleoperation setup for bimanual openarm + Thomason Zhou + BSD-3-Clause + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py index 039d9e6..555ffbb 100644 --- a/openarm_bringup/launch/openarm.launch.py +++ b/openarm_bringup/launch/openarm.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "use_mock_hardware", - default_value="true", + default_value="false", description="Start robot with mock hardware mirroring command to its states.", ) ) diff --git a/openarm_bringup/utils/init_can.sh b/openarm_bringup/utils/init_can.sh new file mode 100755 index 0000000..522bc80 --- /dev/null +++ b/openarm_bringup/utils/init_can.sh @@ -0,0 +1,15 @@ +#!/bin/bash +# This script initializes the CAN interface on a Linux system. + +echo Using CAN interface: $1 with $2 + +if [ ! -e $1 ]; then + echo "Device $1 does not exist." + exit 1 +fi + +bitrate=1000000 +sudo slcand -o -c -s8 $1 +sudo ip link set $2 type can bitrate $bitrate +sudo ip link set $2 up +sudo ip link show $2 \ No newline at end of file diff --git a/openarm_description/CMakeLists.txt b/openarm_description/CMakeLists.txt index cb927f5..fdc67b9 100644 --- a/openarm_description/CMakeLists.txt +++ b/openarm_description/CMakeLists.txt @@ -15,7 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(DIRECTORY launch meshes rviz urdf worlds +install(DIRECTORY launch meshes rviz urdf worlds config DESTINATION share/${PROJECT_NAME} ) diff --git a/openarm_description/urdf/initial_positions.yaml b/openarm_description/config/initial_positions.yaml similarity index 100% rename from openarm_description/urdf/initial_positions.yaml rename to openarm_description/config/initial_positions.yaml diff --git a/openarm_description/launch/description.launch.py b/openarm_description/launch/description.launch.py index e7a543f..69b9c33 100644 --- a/openarm_description/launch/description.launch.py +++ b/openarm_description/launch/description.launch.py @@ -17,9 +17,39 @@ def generate_launch_description(): ) default_model_path = pkg_share / "urdf/openarm.urdf.xacro" + model_arg = launch.actions.DeclareLaunchArgument( + name="model", + default_value=str(default_model_path), + description="Absolute path to the robot's URDF file", + ) + side_arg = DeclareLaunchArgument( + name="side", default_value="right", # Use "left" to test left arm. + description="Select arm side: 'left' or 'right'" + ) + zero_pos_arg = DeclareLaunchArgument( + name="zero_pos", default_value="up", # Use "arm" to test alternative configuration. + description="Specify zero position: 'up' or 'arm'" + ) + prefix_arg = DeclareLaunchArgument( + name="prefix", default_value="", + description="Prefix for link and joint names (e.g., left_, right_)" + ) + can_device_arg = DeclareLaunchArgument( + name="can_device", default_value="can0", + description="CAN device identifier to use" + ) + use_sim_time = LaunchConfiguration("use_sim_time") use_sim_time_launch_arg = DeclareLaunchArgument("use_sim_time", default_value="true") + robot_description_command = Command([ + "xacro ", LaunchConfiguration("model"), + " side:=", LaunchConfiguration("side"), + " zero_pos:=", LaunchConfiguration("zero_pos"), + " prefix:=", LaunchConfiguration("prefix"), + " can_device:=", LaunchConfiguration("can_device") + ]) + robot_state_publisher_node = launch_ros.actions.Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -27,7 +57,7 @@ def generate_launch_description(): { # ParameterValue is required to avoid being interpreted as YAML. "robot_description": ParameterValue( - Command(["xacro ", LaunchConfiguration("model")]), value_type=str + robot_description_command, value_type=str ), "use_sim_time": use_sim_time, }, @@ -36,11 +66,11 @@ def generate_launch_description(): return launch.LaunchDescription( [ - launch.actions.DeclareLaunchArgument( - name="model", - default_value=str(default_model_path), - description="Absolute path to the robot's URDF file", - ), + model_arg, + side_arg, + zero_pos_arg, + prefix_arg, + can_device_arg, use_sim_time_launch_arg, robot_state_publisher_node, ] diff --git a/openarm_description/urdf/openarm.ros2_control.xacro b/openarm_description/urdf/openarm.ros2_control.xacro index c269488..2f7d915 100644 --- a/openarm_description/urdf/openarm.ros2_control.xacro +++ b/openarm_description/urdf/openarm.ros2_control.xacro @@ -1,21 +1,21 @@ - + - mock_components/GenericSystem + openarm_hardware/OpenArmHW + ${prefix} + ${can_device} - - ${initial_positions['rev1']} - + @@ -23,9 +23,7 @@ - - ${initial_positions['rev2']} - + @@ -33,9 +31,7 @@ - - ${initial_positions['rev3']} - + @@ -43,9 +39,7 @@ - - ${initial_positions['rev4']} - + @@ -53,9 +47,7 @@ - - ${initial_positions['rev5']} - + @@ -63,9 +55,7 @@ - - ${initial_positions['rev6']} - + @@ -73,25 +63,21 @@ - - ${initial_positions['rev7']} - + - - ${initial_positions['left_pris1']} - + - + diff --git a/openarm_description/urdf/openarm.urdf b/openarm_description/urdf/openarm.urdf index 06a7b76..0e7ddfd 100644 --- a/openarm_description/urdf/openarm.urdf +++ b/openarm_description/urdf/openarm.urdf @@ -7,14 +7,19 @@ + + + - mock_components/GenericSystem + openarm_hardware/OpenArmHW + + can0 @@ -93,12 +98,17 @@ - - - 0 - - + + + + + + diff --git a/openarm_description/urdf/openarm.urdf.xacro b/openarm_description/urdf/openarm.urdf.xacro index 4a0a9cf..f9ab46f 100644 --- a/openarm_description/urdf/openarm.urdf.xacro +++ b/openarm_description/urdf/openarm.urdf.xacro @@ -2,7 +2,5 @@ - - - + diff --git a/openarm_description/urdf/openarm.xacro b/openarm_description/urdf/openarm.xacro index fc8eb37..e75179f 100644 --- a/openarm_description/urdf/openarm.xacro +++ b/openarm_description/urdf/openarm.xacro @@ -4,8 +4,11 @@ + + + - + @@ -14,7 +17,16 @@ - + + + + + + + + + + @@ -220,21 +232,21 @@ - + - + - - - + + + - + @@ -248,21 +260,21 @@ - + - + - + @@ -270,14 +282,14 @@ - + - + diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt index bcdd949..190e46c 100644 --- a/openarm_hardware/CMakeLists.txt +++ b/openarm_hardware/CMakeLists.txt @@ -16,12 +16,15 @@ find_package(rclcpp_lifecycle REQUIRED) add_library(${PROJECT_NAME} SHARED src/openarm_hardware.cpp + src/canbus.cpp + src/motor.cpp + src/motor_control.cpp ) -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ +target_include_directories( + ${PROJECT_NAME} + PRIVATE + include ) ament_target_dependencies(${PROJECT_NAME} @@ -30,24 +33,23 @@ ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_lifecycle ) -ament_export_libraries(${PROJECT_NAME}) + pluginlib_export_plugin_description_file(hardware_interface openarm_hardware.xml) +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib + ) + install(DIRECTORY include/ - DESTINATION include +DESTINATION include ) -install(TARGETS ${PROJECT_NAME} - DESTINATION bin -) -install(FILES - openarm_hardware.xml - DESTINATION share/${PROJECT_NAME} -) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(hardware_interface REQUIRED) ament_add_gmock(test_openarm_hardware test/test_openarm_hardware.cpp @@ -64,4 +66,18 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() + +ament_export_include_directories( + include +) + +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp +) + ament_package() diff --git a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp index 164f0e3..75e6313 100644 --- a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp +++ b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp @@ -35,13 +35,22 @@ namespace openarm_hardware { -const std::vector MOTORS_TYPES = {DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310}; -const std::vector CAN_DEVICE_IDS = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; -const std::vector CAN_MASTER_IDS = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; -const std::vector MOTOR_WITH_TORQUE = {true,true,true,true,true,true,true}; -const Control_Type CONTROL_MODE = Control_Type::MIT; -const double DEFAULT_KP = 1.0; -const double DEFAULT_KD = 0.0; +std::vector motor_types{DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4340, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310, DM_Motor_Type::DM4310}; +std::vector can_device_ids{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; +std::vector can_master_ids{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; +static const Control_Type CONTROL_MODE = Control_Type::MIT; +static const std::size_t ARM_DOF = 7; +static const std::size_t GRIPPER_DOF = 1; +static const std::size_t TOTAL_DOF = ARM_DOF + GRIPPER_DOF; +static const std::array KP = {80.0, 80.0, 20.0, 55.0, 5.0, 5.0, 5.0, 0.5}; +static const std::array KD = {1.25, 0.17, 0.015, 0.07, 0.07, 0.05, 0.05, 0.01}; +static const std::array SLOW_KP = {10.0, 10.0, 10.0, 7.5, 5.0, 5.0, 5.0, 0.5}; +static const double START_POS_TOLERANCE_RAD = 0.1; +static const double POS_JUMP_TOLERANCE_RAD = 3.1415 / 2.0; + +static const bool USING_GRIPPER = true; +static const double GRIPPER_REFERENCE_GEAR_RADIUS_M = 0.00853; +static const int GRIPPER_INDEX = TOTAL_DOF - 1; class OpenArmHW : public hardware_interface::SystemInterface { @@ -78,9 +87,11 @@ public: hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; + std::size_t curr_dof = ARM_DOF; // minus gripper private: + std::string prefix_; std::unique_ptr canbus_; - MotorControl motor_control_; + std::unique_ptr motor_control_; std::vector pos_commands_; std::vector pos_states_; std::vector vel_commands_; @@ -88,6 +99,9 @@ private: std::vector tau_ff_commands_; std::vector tau_states_; std::vector> motors_; + + void refresh_motors(); + bool disable_torque_; }; } // namespace openarm_hardware diff --git a/openarm_hardware/src/canbus.cpp b/openarm_hardware/src/canbus.cpp index 8b34423..36327b7 100644 --- a/openarm_hardware/src/canbus.cpp +++ b/openarm_hardware/src/canbus.cpp @@ -1,4 +1,4 @@ -#include "canbus.hpp" +#include "openarm_hardware/canbus.hpp" #include CANBus::CANBus(const std::string& interface) { diff --git a/openarm_hardware/src/motor.cpp b/openarm_hardware/src/motor.cpp index 1b23c32..0d915ef 100644 --- a/openarm_hardware/src/motor.cpp +++ b/openarm_hardware/src/motor.cpp @@ -1,4 +1,4 @@ -#include "motor.hpp" +#include "openarm_hardware/motor.hpp" Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID) : MotorType(motorType), SlaveID(slaveID), MasterID(masterID), diff --git a/openarm_hardware/src/motor_control.cpp b/openarm_hardware/src/motor_control.cpp index 9bafe67..02eedf5 100644 --- a/openarm_hardware/src/motor_control.cpp +++ b/openarm_hardware/src/motor_control.cpp @@ -1,5 +1,5 @@ -#include "motor_control.hpp" -#include "motor.hpp" +#include "openarm_hardware/motor_control.hpp" +#include "openarm_hardware/motor.hpp" MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {} diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp index 799d2cf..e52f042 100644 --- a/openarm_hardware/src/openarm_hardware.cpp +++ b/openarm_hardware/src/openarm_hardware.cpp @@ -19,22 +19,14 @@ #include "openarm_hardware/openarm_hardware.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" namespace openarm_hardware { static const std::string& can_device_name = "can0"; -OpenArmHW::OpenArmHW(): - canbus_(std::make_unique(can_device_name)), - motor_control_(MotorControl(*canbus_)) { - for(size_t i = 0; i < MOTORS_TYPES.size(); ++i){ - motors_[i] = std::make_unique(MOTORS_TYPES[i], CAN_DEVICE_IDS[i], CAN_MASTER_IDS[i]); - } - for(const auto& motor: motors_){ - motor_control_.addMotor(*motor); - } -} +OpenArmHW::OpenArmHW() = default; hardware_interface::CallbackReturn OpenArmHW::on_init( const hardware_interface::HardwareInfo & info) @@ -43,13 +35,55 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( { return CallbackReturn::ERROR; } + + //read hardware parameters + if (info.hardware_parameters.find("can_device") == info.hardware_parameters.end()){ + RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), "No can_device parameter found"); + return CallbackReturn::ERROR; + } + + auto it = info.hardware_parameters.find("prefix"); + if (it == info.hardware_parameters.end()){ + prefix_ = ""; + } + else{ + prefix_ = it->second; + } + it = info.hardware_parameters.find("disable_torque"); + if (it == info.hardware_parameters.end()){ + disable_torque_ = false; + } + else{ + disable_torque_ = it->second == "true"; + } - pos_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - pos_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - vel_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - vel_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - tau_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - tau_ff_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + canbus_ = std::make_unique(info.hardware_parameters.at("can_device")); + motor_control_ = std::make_unique(*canbus_); + + if(USING_GRIPPER){ + motor_types.emplace_back(DM_Motor_Type::DM4310); + can_device_ids.emplace_back(0x08); + can_master_ids.emplace_back(0x18); + ++curr_dof; + } + + motors_.resize(curr_dof); + for(size_t i = 0; i < curr_dof; ++i){ + motors_[i] = std::make_unique(motor_types[i], can_device_ids[i], can_master_ids[i]); + } + for(const auto& motor: motors_){ + motor_control_->addMotor(*motor); + } + + pos_states_.resize(curr_dof, 0.0); + pos_commands_.resize(curr_dof, 0.0); + vel_states_.resize(curr_dof, 0.0); + vel_commands_.resize(curr_dof, 0.0); + tau_states_.resize(curr_dof, 0.0); + tau_ff_commands_.resize(curr_dof, 0.0); + refresh_motors(); + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); return CallbackReturn::SUCCESS; } @@ -57,7 +91,14 @@ hardware_interface::CallbackReturn OpenArmHW::on_init( hardware_interface::CallbackReturn OpenArmHW::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { + + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); // zero position or calibrate to pose + // for (std::size_t i = 0; i < curr_dof; ++i) + // { + // motor_control_->set_zero_position(*motors_[i]); + // } + return CallbackReturn::SUCCESS; } @@ -65,11 +106,12 @@ hardware_interface::CallbackReturn OpenArmHW::on_configure( std::vector OpenArmHW::export_state_interfaces() { std::vector state_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < curr_dof; ++i) { state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_states_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_states_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_states_[i])); + RCLCPP_INFO(rclcpp::get_logger("OpenArmHW"), "Exporting state interface for joint %s", info_.joints[i].name.c_str()); } return state_interfaces; @@ -78,7 +120,7 @@ std::vector OpenArmHW::export_state_interfac std::vector OpenArmHW::export_command_interfaces() { std::vector command_interfaces; - for (size_t i = 0; i < info_.joints.size(); ++i) + for (size_t i = 0; i < curr_dof; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_POSITION, &pos_commands_[i])); command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &vel_commands_[i])); @@ -88,11 +130,37 @@ std::vector OpenArmHW::export_command_inte return command_interfaces; } +void OpenArmHW::refresh_motors() +{ + for(const auto& motor: motors_){ + motor_control_->controlMIT(*motor, 0.0, 0.0, 0.0, 0.0, 0.0); + } +} + hardware_interface::CallbackReturn OpenArmHW::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); + + // for (std::size_t m = 0; m < curr_dof; ++m){ + // double diff = pos_states_[m] - pos_commands_[m]; + // while(abs(diff) > START_POS_TOLERANCE_RAD){ + // // linear interpolation + // // take the min of max_step and the difference + + // double max_step = std::min(POS_JUMP_TOLERANCE_RAD, std::abs(diff)); + // if (diff > 0){ + // pos_commands_[m] = pos_states_[m] - max_step; + // } + // else{ + // pos_commands_[m] = pos_states_[m] + max_step; + // } + // motor_control_->controlMIT(*motors_[m], SLOW_KP[m], KD[m], pos_commands_[m], 0.0, 0.0); + // } + // } + refresh_motors(); for(const auto& motor: motors_){ - motor_control_.enable(*motor); + motor_control_->enable(*motor); } read(rclcpp::Time(0), rclcpp::Duration(0, 0)); @@ -102,8 +170,9 @@ hardware_interface::CallbackReturn OpenArmHW::on_activate( hardware_interface::CallbackReturn OpenArmHW::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + refresh_motors(); for(const auto& motor: motors_){ - motor_control_.disable(*motor); + motor_control_->disable(*motor); } return CallbackReturn::SUCCESS; @@ -112,11 +181,17 @@ hardware_interface::CallbackReturn OpenArmHW::on_deactivate( hardware_interface::return_type OpenArmHW::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for(size_t i = 0; i < motors_.size(); ++i){ + for(size_t i = 0; i < ARM_DOF; ++i){ pos_states_[i] = motors_[i]->getPosition(); vel_states_[i] = motors_[i]->getVelocity(); tau_states_[i] = motors_[i]->getTorque(); } + if(USING_GRIPPER){ + pos_states_[GRIPPER_INDEX] = -motors_[GRIPPER_INDEX]->getPosition() * GRIPPER_REFERENCE_GEAR_RADIUS_M; + vel_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getVelocity() * GRIPPER_REFERENCE_GEAR_RADIUS_M; + tau_states_[GRIPPER_INDEX] = motors_[GRIPPER_INDEX]->getTorque() * GRIPPER_REFERENCE_GEAR_RADIUS_M; + } + return hardware_interface::return_type::OK; } @@ -124,8 +199,27 @@ hardware_interface::return_type OpenArmHW::read( hardware_interface::return_type OpenArmHW::write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - for(size_t i = 0; i < motors_.size(); ++i){ - motor_control_.controlMIT(*motors_[i], DEFAULT_KP, DEFAULT_KD, pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]); + disable_torque_ = false; + + if (disable_torque_){ + // refresh motor state on write + for(size_t i = 0; i < TOTAL_DOF; ++i){ + motor_control_->controlMIT(*motors_[i], 0.0, 0.0, 0.0, 0.0, 0.0); + return hardware_interface::return_type::OK; + } + } + + + for(size_t i = 0; i < ARM_DOF; ++i){ + if (std::abs(pos_commands_[i] - pos_states_[i]) > POS_JUMP_TOLERANCE_RAD) + { + RCLCPP_ERROR(rclcpp::get_logger("OpenArmHW"), "Position jump detected for joint %s: %f -> %f", info_.joints[i].name.c_str(), pos_states_[i], pos_commands_[i]); + return hardware_interface::return_type::ERROR; + } + motor_control_->controlMIT(*motors_[i], KP.at(i), KD.at(i), pos_commands_[i], vel_commands_[i], tau_ff_commands_[i]); + } + if(USING_GRIPPER){ + motor_control_->controlMIT(*motors_[GRIPPER_INDEX], KP.at(GRIPPER_INDEX), KD.at(GRIPPER_INDEX), -pos_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, vel_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M, tau_ff_commands_[GRIPPER_INDEX] / GRIPPER_REFERENCE_GEAR_RADIUS_M); } return hardware_interface::return_type::OK; } diff --git a/openarm_moveit_config/.setup_assistant b/openarm_moveit_config/.setup_assistant index db9cf4d..049131a 100644 --- a/openarm_moveit_config/.setup_assistant +++ b/openarm_moveit_config/.setup_assistant @@ -7,25 +7,4 @@ moveit_setup_assistant_config: package_settings: author_name: Thomason Zhou author_email: t95zhou@uwaterloo.ca - generated_timestamp: 1741767765 - control_xacro: - command: - - position - - velocity - - effort - state: - - position - - velocity - - effort - modified_urdf: - xacros: - - control_xacro - control_xacro: - command: - - position - - velocity - - effort - state: - - position - - velocity - - effort \ No newline at end of file + generated_timestamp: 1744186910 \ No newline at end of file diff --git a/openarm_moveit_config/config/initial_positions.yaml b/openarm_moveit_config/config/initial_positions.yaml index b636de3..11d9ef4 100644 --- a/openarm_moveit_config/config/initial_positions.yaml +++ b/openarm_moveit_config/config/initial_positions.yaml @@ -2,11 +2,10 @@ initial_positions: left_pris1: 0 - right_pris2: 0 rev1: 0 rev2: 0 rev3: 0 rev4: 0 rev5: 0 rev6: 0 - rev7: 0 + rev7: 0 \ No newline at end of file diff --git a/openarm_moveit_config/config/joint_limits.yaml b/openarm_moveit_config/config/joint_limits.yaml index 605dd2a..88f6c0d 100644 --- a/openarm_moveit_config/config/joint_limits.yaml +++ b/openarm_moveit_config/config/joint_limits.yaml @@ -13,11 +13,6 @@ joint_limits: max_velocity: 0 has_acceleration_limits: false max_acceleration: 0 - right_pris2: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 rev1: has_velocity_limits: false max_velocity: 0 diff --git a/openarm_moveit_config/config/moveit.rviz b/openarm_moveit_config/config/moveit.rviz index 9bbdad1..a323fcf 100644 --- a/openarm_moveit_config/config/moveit.rviz +++ b/openarm_moveit_config/config/moveit.rviz @@ -27,7 +27,7 @@ Visualization Manager: Robot Alpha: 0.5 Value: true Global Options: - Fixed Frame: link1 + Fixed Frame: dummy_link Tools: - Class: rviz_default_plugins/Interact - Class: rviz_default_plugins/MoveCamera @@ -43,7 +43,7 @@ Visualization Manager: Z: 0.30 Name: Current View Pitch: 0.5 - Target Frame: link1 + Target Frame: dummy_link Yaw: -0.623 Window Geometry: Height: 975 diff --git a/openarm_moveit_config/config/moveit_controllers.yaml b/openarm_moveit_config/config/moveit_controllers.yaml index 351d5b3..e36ca6a 100644 --- a/openarm_moveit_config/config/moveit_controllers.yaml +++ b/openarm_moveit_config/config/moveit_controllers.yaml @@ -17,6 +17,8 @@ moveit_simple_controller_manager: - rev5 - rev6 - rev7 + action_ns: follow_joint_trajectory + default: true gripper_controller: type: GripperCommand joints: diff --git a/openarm_moveit_config/config/openarm.srdf b/openarm_moveit_config/config/openarm.srdf index b372d79..92d400c 100644 --- a/openarm_moveit_config/config/openarm.srdf +++ b/openarm_moveit_config/config/openarm.srdf @@ -55,7 +55,7 @@ - + diff --git a/openarm_moveit_config/config/openarm.urdf.xacro b/openarm_moveit_config/config/openarm.urdf.xacro index 7f0add7..3757c95 100644 --- a/openarm_moveit_config/config/openarm.urdf.xacro +++ b/openarm_moveit_config/config/openarm.urdf.xacro @@ -1,5 +1,6 @@ - + + diff --git a/openarm_moveit_config/config/ros2_controllers.yaml b/openarm_moveit_config/config/ros2_controllers.yaml index 388016c..913303a 100644 --- a/openarm_moveit_config/config/ros2_controllers.yaml +++ b/openarm_moveit_config/config/ros2_controllers.yaml @@ -30,9 +30,6 @@ openarm_arm_controller: command_interfaces: - position - velocity - - effort state_interfaces: - position - - velocity - - effort - allow_nonzero_velocity_at_trajectory_end: false + - velocity \ No newline at end of file diff --git a/openarm_moveit_config/config/sensors_3d.yaml b/openarm_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..99dcaeb --- /dev/null +++ b/openarm_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,25 @@ +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_moveit_config/package.xml b/openarm_moveit_config/package.xml index 8bd15d5..522696f 100644 --- a/openarm_moveit_config/package.xml +++ b/openarm_moveit_config/package.xml @@ -8,11 +8,11 @@ Thomason Zhou - BSD-3-Clause + BSD http://moveit.ros.org/ - https://github.com/moveit/moveit2/issues - https://github.com/moveit/moveit2 + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 Thomason Zhou @@ -43,7 +43,6 @@ rviz_default_plugins tf2_ros warehouse_ros_mongo - xacro