From 595fbe7745770e2b91ab1398db301372bccc8c55 Mon Sep 17 00:00:00 2001 From: thomason <84239247+thomasonzhou@users.noreply.github.com> Date: Fri, 28 Mar 2025 18:05:38 +0900 Subject: [PATCH] Implement openarm ros2_control support with openarm_hardware and openarm_bringup (#2) - openarm_bringup: ros2_control bringup - openarm_hardware: hardware interface for ros2_control --- README.md | 11 +- .../urdf/openarm_bimanual.urdf | 576 ------------------ openarm_bringup/CMakeLists.txt | 30 + openarm_bringup/LICENSE | 25 + .../config/openarm_controllers.yaml | 84 +++ .../config/test_goal_publishers_config.yaml | 58 ++ openarm_bringup/launch/openarm.launch.py | 239 ++++++++ ...test_forward_position_controller.launch.py | 46 ++ ...test_joint_trajectory_controller.launch.py | 46 ++ openarm_bringup/package.xml | 18 + .../resource/openarm_description | 0 .../urdf/initial_positions.yaml | 12 + .../urdf/openarm.ros2_control.xacro | 46 +- openarm_description/urdf/openarm.urdf | 89 +++ openarm_description/urdf/openarm.urdf.xacro | 1 - openarm_description/urdf/openarm.xacro | 572 ++++++++--------- openarm_hardware/CMakeLists.txt | 64 ++ openarm_hardware/LICENSE | 25 + .../include/openarm_hardware/canbus.hpp | 26 + .../include/openarm_hardware/motor.hpp | 94 +++ .../openarm_hardware/motor_control.hpp | 47 ++ .../openarm_hardware/openarm_hardware.hpp | 95 +++ .../openarm_hardware/visibility_control.h | 50 ++ openarm_hardware/openarm_hardware.xml | 9 + openarm_hardware/package.xml | 18 + openarm_hardware/setup.bash | 20 + openarm_hardware/src/canbus.cpp | 68 +++ openarm_hardware/src/motor.cpp | 120 ++++ openarm_hardware/src/motor_control.cpp | 163 +++++ openarm_hardware/src/openarm_hardware.cpp | 138 +++++ .../test/test_openarm_hardware.cpp | 131 ++++ openarm_moveit_config/.setup_assistant | 10 +- .../config/initial_positions.yaml | 8 +- .../config/joint_limits.yaml | 73 +-- openarm_moveit_config/config/moveit.rviz | 8 +- .../config/moveit_controllers.yaml | 13 +- .../config/ompl_planning.yaml | 38 ++ openarm_moveit_config/config/openarm.srdf | 89 +++ .../config/openarm.urdf.xacro | 5 + .../config/openarm_grip.srdf | 55 -- .../config/openarm_grip.urdf.xacro | 14 - .../config/ros2_controllers.yaml | 19 +- openarm_moveit_config/launch/demo.launch.py | 2 +- .../launch/move_group.launch.py | 2 +- .../launch/moveit_rviz.launch.py | 2 +- openarm_moveit_config/launch/rsp.launch.py | 2 +- .../launch/setup_assistant.launch.py | 2 +- .../launch/spawn_controllers.launch.py | 2 +- .../launch/static_virtual_joint_tfs.launch.py | 2 +- .../launch/warehouse_db.launch.py | 2 +- openarm_moveit_config/package.xml | 4 +- 51 files changed, 2248 insertions(+), 1025 deletions(-) create mode 100644 openarm_bringup/CMakeLists.txt create mode 100644 openarm_bringup/LICENSE create mode 100644 openarm_bringup/config/openarm_controllers.yaml create mode 100644 openarm_bringup/config/test_goal_publishers_config.yaml create mode 100644 openarm_bringup/launch/openarm.launch.py create mode 100644 openarm_bringup/launch/test_forward_position_controller.launch.py create mode 100644 openarm_bringup/launch/test_joint_trajectory_controller.launch.py create mode 100644 openarm_bringup/package.xml create mode 100644 openarm_description/resource/openarm_description create mode 100644 openarm_description/urdf/initial_positions.yaml rename openarm_moveit_config/config/openarm_grip.ros2_control.xacro => openarm_description/urdf/openarm.ros2_control.xacro (71%) create mode 100644 openarm_hardware/CMakeLists.txt create mode 100644 openarm_hardware/LICENSE create mode 100644 openarm_hardware/include/openarm_hardware/canbus.hpp create mode 100644 openarm_hardware/include/openarm_hardware/motor.hpp create mode 100644 openarm_hardware/include/openarm_hardware/motor_control.hpp create mode 100644 openarm_hardware/include/openarm_hardware/openarm_hardware.hpp create mode 100644 openarm_hardware/include/openarm_hardware/visibility_control.h create mode 100644 openarm_hardware/openarm_hardware.xml create mode 100644 openarm_hardware/package.xml create mode 100755 openarm_hardware/setup.bash create mode 100644 openarm_hardware/src/canbus.cpp create mode 100644 openarm_hardware/src/motor.cpp create mode 100644 openarm_hardware/src/motor_control.cpp create mode 100644 openarm_hardware/src/openarm_hardware.cpp create mode 100644 openarm_hardware/test/test_openarm_hardware.cpp create mode 100644 openarm_moveit_config/config/ompl_planning.yaml create mode 100644 openarm_moveit_config/config/openarm.srdf create mode 100644 openarm_moveit_config/config/openarm.urdf.xacro delete mode 100644 openarm_moveit_config/config/openarm_grip.srdf delete mode 100644 openarm_moveit_config/config/openarm_grip.urdf.xacro diff --git a/README.md b/README.md index 2e2a9d9..7d53729 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ # ROS2 packages for OpenArm robots -- openarm_bimanual_description: urdf with pedestal torso and arm on each side -- openarm_description: urdf with gripper actuator +- openarm_bimanual_description: humanoid upper body with two arms (urdf) +- openarm_description: single arm (urdf) - openarm_moveit_config: motion planning with [moveit2](https://github.com/moveit/moveit2) +- openarm_bringup: [ros2_control](https://control.ros.org/humble/index.html) bringup +- openarm_hardware: hardware interface for ros2_control ## Description Packages @@ -11,15 +13,10 @@ Each link has a visual mesh and a collision mesh, as shown in the figures below: visual meshes of openarm_bimanual_description urdf in rviz2 collision meshes of openarm_bimanual_description urdf in rviz2 -### TODO: -- [ ] Add results from true inertia tests to URDF - ## MoveIt2 Support https://github.com/user-attachments/assets/a0f962e5-6150-49ce-b18e-9914bcb322ef -### TODO: -- [ ] ROS 2 control packages (separate branch) Tested with: - [x] Rolling diff --git a/openarm_bimanual_description/urdf/openarm_bimanual.urdf b/openarm_bimanual_description/urdf/openarm_bimanual.urdf index 119b42d..e69de29 100644 --- a/openarm_bimanual_description/urdf/openarm_bimanual.urdf +++ b/openarm_bimanual_description/urdf/openarm_bimanual.urdf @@ -1,576 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/openarm_bringup/CMakeLists.txt b/openarm_bringup/CMakeLists.txt new file mode 100644 index 0000000..451f441 --- /dev/null +++ b/openarm_bringup/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(openarm_bringup) + +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) + +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() + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/openarm_bringup/LICENSE b/openarm_bringup/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/openarm_bringup/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_bringup/config/openarm_controllers.yaml b/openarm_bringup/config/openarm_controllers.yaml new file mode 100644 index 0000000..5f172e0 --- /dev/null +++ b/openarm_bringup/config/openarm_controllers.yaml @@ -0,0 +1,84 @@ +# Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + forward_velocity_controller: + type: forward_command_controller/ForwardCommandController + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +forward_position_controller: + ros__parameters: + joints: + - rev1 + - rev2 + - rev3 + - rev4 + - rev5 + - rev6 + - rev7 + interface_name: position + +forward_velocity_controller: + ros__parameters: + joints: + - rev1 + - rev2 + - rev3 + - rev4 + - rev5 + - rev6 + - rev7 + interface_name: velocity + +joint_trajectory_controller: + ros__parameters: + joints: + - rev1 + - rev2 + - rev3 + - rev4 + - rev5 + - rev6 + - rev7 + + command_interfaces: + - position + state_interfaces: + - position + + state_publish_rate: 50.0 # Defaults to 50 + action_monitor_rate: 50.0 # Defaults to 20 + + allow_partial_joints_goal: false # Defaults to false + constraints: + stopped_velocity_tolerance: 0.01 # Defaults to 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/openarm_bringup/config/test_goal_publishers_config.yaml b/openarm_bringup/config/test_goal_publishers_config.yaml new file mode 100644 index 0000000..f750f34 --- /dev/null +++ b/openarm_bringup/config/test_goal_publishers_config.yaml @@ -0,0 +1,58 @@ +# Copyright (c) 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# + +publisher_forward_position_controller: + ros__parameters: + + controller_name: "forward_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.185, 0.185, 0.185, 0.185, 0.185, 0.185, 0.185] + pos2: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pos3: [-0.185, -0.185, -0.185, -0.185, -0.185, -0.185, -0.185] + pos4: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_controller" + wait_sec_between_publish: 6 + repeat_the_same_goal: 1 # useful to simulate continuous inputs + + goal_time_from_start: 3.0 + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: + positions: [0.185, 0.185, 0.185, 0.185, 0.185, 0.185, 0.185] + pos2: + positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + pos3: + positions: [-0.185, -0.185, -0.185, -0.185, -0.185, -0.185, -0.185] + pos4: + positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + joints: + - rev1 + - rev2 + - rev3 + - rev4 + - rev5 + - rev6 + - rev7 diff --git a/openarm_bringup/launch/openarm.launch.py b/openarm_bringup/launch/openarm.launch.py new file mode 100644 index 0000000..039d9e6 --- /dev/null +++ b/openarm_bringup/launch/openarm.launch.py @@ -0,0 +1,239 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="openarm_bringup", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="openarm_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="openarm_description", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="openarm.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_mock_hardware", + default_value="true", + description="Start robot with mock hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "mock_sensor_commands", + default_value="false", + description="Enable mock command interfaces for sensors used for simple simulations. \ + Used only if 'use_mock_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="joint_trajectory_controller", + choices=["forward_position_controller", "joint_trajectory_controller"], + description="Robot controller to start.", + ) + ) + + # Initialize Arguments + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_mock_hardware = LaunchConfiguration("use_mock_hardware") + mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") + robot_controller = LaunchConfiguration("robot_controller") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_mock_hardware:=", + use_mock_hardware, + " ", + "mock_sensor_commands:=", + mock_sensor_commands, + " ", + ] + ) + + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "openarm.rviz"] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + output="both", + parameters=[robot_description, robot_controllers], + ) + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_names = [robot_controller] + robot_controller_spawners = [] + for controller in robot_controller_names: + robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager"], + ) + ] + + inactive_robot_controller_names = [] + inactive_robot_controller_spawners = [] + for controller in inactive_robot_controller_names: + inactive_robot_controller_spawners += [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller, "-c", "/controller_manager", "--inactive"], + ) + ] + + # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node + delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=control_node, + on_start=[ + TimerAction( + period=3.0, + actions=[joint_state_broadcaster_spawner], + ), + ], + ) + ) + + # Delay loading and activation of robot_controller_names after `joint_state_broadcaster` + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(robot_controller_spawners): + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + robot_controller_spawners[i - 1] + if i > 0 + else joint_state_broadcaster_spawner + ), + on_exit=[controller], + ) + ) + ] + + # Delay start of inactive_robot_controller_names after other controllers + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] + for i, controller in enumerate(inactive_robot_controller_spawners): + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=( + inactive_robot_controller_spawners[i - 1] + if i > 0 + else robot_controller_spawners[-1] + ), + on_exit=[controller], + ) + ) + ] + + return LaunchDescription( + declared_arguments + + [ + control_node, + robot_state_pub_node, + rviz_node, + delay_joint_state_broadcaster_spawner_after_ros2_control_node, + ] + + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + ) diff --git a/openarm_bringup/launch/test_forward_position_controller.launch.py b/openarm_bringup/launch/test_forward_position_controller.launch.py new file mode 100644 index 0000000..e6a4b3a --- /dev/null +++ b/openarm_bringup/launch/test_forward_position_controller.launch.py @@ -0,0 +1,46 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + position_goals = PathJoinSubstitution( + [FindPackageShare("openarm_bringup"), "config", "test_goal_publishers_config.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + ) diff --git a/openarm_bringup/launch/test_joint_trajectory_controller.launch.py b/openarm_bringup/launch/test_joint_trajectory_controller.launch.py new file mode 100644 index 0000000..f882dfe --- /dev/null +++ b/openarm_bringup/launch/test_joint_trajectory_controller.launch.py @@ -0,0 +1,46 @@ +# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Source of this file are templates in +# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. +# +# Author: Dr. Denis +# + +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + position_goals = PathJoinSubstitution( + [FindPackageShare("openarm_bringup"), "config", "test_goal_publishers_config.yaml"] + ) + + return LaunchDescription( + [ + Node( + package="ros2_controllers_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + ) diff --git a/openarm_bringup/package.xml b/openarm_bringup/package.xml new file mode 100644 index 0000000..9ef5f5b --- /dev/null +++ b/openarm_bringup/package.xml @@ -0,0 +1,18 @@ + + + + openarm_bringup + 0.0.0 + Bringup script for OpenArm + Thomason Zhou + BSD-3-Clause + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/openarm_description/resource/openarm_description b/openarm_description/resource/openarm_description new file mode 100644 index 0000000..e69de29 diff --git a/openarm_description/urdf/initial_positions.yaml b/openarm_description/urdf/initial_positions.yaml new file mode 100644 index 0000000..b636de3 --- /dev/null +++ b/openarm_description/urdf/initial_positions.yaml @@ -0,0 +1,12 @@ +# Default initial positions for openarm's ros2_control fake system + +initial_positions: + left_pris1: 0 + right_pris2: 0 + rev1: 0 + rev2: 0 + rev3: 0 + rev4: 0 + rev5: 0 + rev6: 0 + rev7: 0 diff --git a/openarm_moveit_config/config/openarm_grip.ros2_control.xacro b/openarm_description/urdf/openarm.ros2_control.xacro similarity index 71% rename from openarm_moveit_config/config/openarm_grip.ros2_control.xacro rename to openarm_description/urdf/openarm.ros2_control.xacro index c5d7e6e..c269488 100644 --- a/openarm_moveit_config/config/openarm_grip.ros2_control.xacro +++ b/openarm_description/urdf/openarm.ros2_control.xacro @@ -1,84 +1,96 @@ - + - + mock_components/GenericSystem + openarm_hardware/OpenArmHW - + + ${initial_positions['rev1']} + - + + ${initial_positions['rev2']} + - + + ${initial_positions['rev3']} + - + + ${initial_positions['rev4']} + - + + ${initial_positions['rev5']} + - + + ${initial_positions['rev6']} + - + + ${initial_positions['rev7']} + - + - - ${initial_positions['slider_left']} + ${initial_positions['left_pris1']} - - - + + - ${initial_positions['slider_right']} + ${initial_positions['left_pris1']} - diff --git a/openarm_description/urdf/openarm.urdf b/openarm_description/urdf/openarm.urdf index 4d767af..06a7b76 100644 --- a/openarm_description/urdf/openarm.urdf +++ b/openarm_description/urdf/openarm.urdf @@ -10,6 +10,95 @@ + + + + mock_components/GenericSystem + openarm_hardware/OpenArmHW + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + 0 + + + + + + 0 + + + diff --git a/openarm_description/urdf/openarm.urdf.xacro b/openarm_description/urdf/openarm.urdf.xacro index 65a59f3..0fbad44 100644 --- a/openarm_description/urdf/openarm.urdf.xacro +++ b/openarm_description/urdf/openarm.urdf.xacro @@ -2,5 +2,4 @@ - diff --git a/openarm_description/urdf/openarm.xacro b/openarm_description/urdf/openarm.xacro index 33427ea..033f81d 100644 --- a/openarm_description/urdf/openarm.xacro +++ b/openarm_description/urdf/openarm.xacro @@ -1,282 +1,290 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_hardware/CMakeLists.txt b/openarm_hardware/CMakeLists.txt new file mode 100644 index 0000000..0997c50 --- /dev/null +++ b/openarm_hardware/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(openarm_hardware) + +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(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/openarm_hardware.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + +ament_target_dependencies(${PROJECT_NAME} + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) +ament_export_libraries(${PROJECT_NAME}) +pluginlib_export_plugin_description_file(hardware_interface openarm_hardware.xml) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES + openarm_hardware.xml + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + + ament_add_gmock(test_openarm_hardware + test/test_openarm_hardware.cpp + ) + target_link_libraries(test_openarm_hardware + ${PROJECT_NAME} + ) + ament_target_dependencies(test_openarm_hardware + hardware_interface + ) + + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/openarm_hardware/LICENSE b/openarm_hardware/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/openarm_hardware/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_hardware/include/openarm_hardware/canbus.hpp b/openarm_hardware/include/openarm_hardware/canbus.hpp new file mode 100644 index 0000000..0fe6be1 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/canbus.hpp @@ -0,0 +1,26 @@ +#ifndef CANBUS_H +#define CANBUS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class CANBus { +public: + explicit CANBus(const std::string& interface); + ~CANBus(); + + bool send(uint16_t motor_id, const std::array& data); + struct can_frame recv(); + +private: + int sock_; +}; + +#endif // CANBUS_H diff --git a/openarm_hardware/include/openarm_hardware/motor.hpp b/openarm_hardware/include/openarm_hardware/motor.hpp new file mode 100644 index 0000000..83e7c84 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/motor.hpp @@ -0,0 +1,94 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include +#include +#include +#include +#include +#include +#include +#include + +enum class DM_Motor_Type : uint8_t { + DM4310 = 0, + DM4310_48V, + DM4340, + DM4340_48V, + DM6006, + DM8006, + DM8009, + DM10010L, + DM10010, + DMH3510, + DMH6215, + DMG6220, + COUNT +}; + +enum class DM_variable : uint8_t { + UV_Value = 0, KT_Value, OT_Value, OC_Value, ACC, DEC, MAX_SPD, MST_ID, + ESC_ID, TIMEOUT, CTRL_MODE, Damp, Inertia, hw_ver, sw_ver, SN, NPP, Rs, + LS, Flux, Gr, PMAX, VMAX, TMAX, I_BW, KP_ASR, KI_ASR, KP_APR, KI_APR, + OV_Value, GREF, Deta, V_BW, IQ_c1, VL_c1, can_br, sub_ver, + u_off = 50, v_off, k1, k2, m_off, dir, + p_m = 80, xout, + COUNT +}; + +enum class Control_Type : uint8_t { + MIT = 1, + POS_VEL, + VEL, + Torque_Pos, + COUNT +}; + +class Motor { +public: + Motor() = default; + Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID); + + void recv_data(double q, double dq, double tau, int tmos, int trotor); + double getPosition() const; + double getVelocity() const; + double getTorque() const; + int getParam(int RID) const; + + uint16_t SlaveID; + uint16_t MasterID; + bool isEnable; + Control_Type NowControlMode; + DM_Motor_Type MotorType; + + int getStateTmos() const; + int getStateTrotor() const; + double getGoalPosition() const; + double getGoalTau() const; + + void setGoalPosition(double pos); + void setGoalTau(double tau); + void setStateTmos(int tmos); + void setStateTrotor(int trotor); +private: + double Pd, Vd; + double goal_position, goal_tau; + double state_q, state_dq, state_tau; + int state_tmos, state_trotor; + std::map temp_param_dict; +}; + +double LIMIT_MIN_MAX(double x, double min, double max); +uint16_t double_to_uint(double x, double x_min, double x_max, int bits); +double uint_to_double(uint16_t x, double min, double max, int bits); +std::array double_to_uint8s(double value); +std::array data_to_uint8s(uint32_t value); +uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4); +double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4); +bool is_in_ranges(int number); +void print_hex(const std::vector& data); + +template +T get_enum_by_index(int index); + +#endif // MOTOR_H diff --git a/openarm_hardware/include/openarm_hardware/motor_control.hpp b/openarm_hardware/include/openarm_hardware/motor_control.hpp new file mode 100644 index 0000000..534afa7 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/motor_control.hpp @@ -0,0 +1,47 @@ +#ifndef MOTOR_CONTROL_H +#define MOTOR_CONTROL_H + +#include +#include +#include +#include +#include +#include +#include "motor.hpp" +#include "canbus.hpp" +#include +#include + +class MotorControl { +public: + explicit MotorControl(CANBus& canbus); + + void controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau); + void controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau); + + void enable(Motor& motor); + void disable(Motor& motor); + void set_zero_position(Motor& motor); + void recv(); + void sendData(uint16_t motor_id, const std::array& data); + bool addMotor(Motor& motor); + void recv_set_param_data(); + +private: + CANBus& canbus_; + + std::map motors_map; + + static constexpr double Limit_Param[12][3] = { + {12.5, 30, 10}, {12.5, 50, 10}, {12.5, 8, 28}, {12.5, 10, 28}, + {12.5, 45, 20}, {12.5, 45, 40}, {12.5, 45, 54}, {12.5, 25, 200}, + {12.5, 20, 200}, {12.5, 280, 1}, {12.5, 45, 10}, {12.5, 45, 10} + }; + + void processPacket(const can_frame& frame); + void controlCmd(Motor &motor, uint8_t cmd ); + void readRIDParam(Motor& motor, DM_variable RID); + void writeMotorParam(Motor& motor, DM_variable RID, double value); + +}; +#endif // MOTOR_CONTROL_H diff --git a/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp new file mode 100644 index 0000000..164f0e3 --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/openarm_hardware.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2025, Reazon Holdings, Inc. +// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ +#define OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ + +#include +#include +#include + +#include "openarm_hardware/visibility_control.h" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "canbus.hpp" +#include "motor.hpp" +#include "motor_control.hpp" + +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; + +class OpenArmHW : public hardware_interface::SystemInterface +{ +public: + OpenArmHW(); + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_state_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector export_command_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + std::unique_ptr canbus_; + MotorControl motor_control_; + std::vector pos_commands_; + std::vector pos_states_; + std::vector vel_commands_; + std::vector vel_states_; + std::vector tau_ff_commands_; + std::vector tau_states_; + std::vector> motors_; +}; + +} // namespace openarm_hardware + +#endif // OPENARM_HARDWARE__OPENARM_HARDWARE_HPP_ diff --git a/openarm_hardware/include/openarm_hardware/visibility_control.h b/openarm_hardware/include/openarm_hardware/visibility_control.h new file mode 100644 index 0000000..6398cfe --- /dev/null +++ b/openarm_hardware/include/openarm_hardware/visibility_control.h @@ -0,0 +1,50 @@ +// Copyright (c) 2025, Reazon Holdings, Inc. +// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// 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. + +#ifndef OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ +#define OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL +#endif +#define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // OPENARM_HARDWARE__VISIBILITY_CONTROL_H_ diff --git a/openarm_hardware/openarm_hardware.xml b/openarm_hardware/openarm_hardware.xml new file mode 100644 index 0000000..3542069 --- /dev/null +++ b/openarm_hardware/openarm_hardware.xml @@ -0,0 +1,9 @@ + + + + ros2_control hardware interface. + + + diff --git a/openarm_hardware/package.xml b/openarm_hardware/package.xml new file mode 100644 index 0000000..bae1bb7 --- /dev/null +++ b/openarm_hardware/package.xml @@ -0,0 +1,18 @@ + + + + openarm_hardware + 0.0.0 + Hardware interface for OpenArm + Thomason Zhou + BSD-3-Clause + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/openarm_hardware/setup.bash b/openarm_hardware/setup.bash new file mode 100755 index 0000000..6005104 --- /dev/null +++ b/openarm_hardware/setup.bash @@ -0,0 +1,20 @@ +#!/bin/bash + +CAN_INTERFACE=can0 + +for DEVICE in /dev/ttyACM*; do + if [ -e "$DEVICE" ]; then + echo "Using device: $DEVICE" + break + fi +done + +if [ -z "$DEVICE" ]; then + echo "No /dev/ttyACM* device found." + exit 1 +fi + +sudo pkill -f slcand +sudo slcand -o -c -s8 "$DEVICE" "$CAN_INTERFACE" +sudo ip link set "$CAN_INTERFACE" up type can bitrate 1000000 +ip link show "$CAN_INTERFACE" \ No newline at end of file diff --git a/openarm_hardware/src/canbus.cpp b/openarm_hardware/src/canbus.cpp new file mode 100644 index 0000000..8b34423 --- /dev/null +++ b/openarm_hardware/src/canbus.cpp @@ -0,0 +1,68 @@ +#include "canbus.hpp" +#include + +CANBus::CANBus(const std::string& interface) { + struct ifreq ifr; + struct sockaddr_can addr; + + sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (sock_ < 0) { + perror("Error while opening CAN socket"); + exit(EXIT_FAILURE); + } + + std::strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ); + if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) { + perror("Error getting interface index"); + exit(EXIT_FAILURE); + } + + std::memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(sock_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + perror("Error in CAN socket bind"); + exit(EXIT_FAILURE); + } +} + +CANBus::~CANBus() { + close(sock_); +} + +bool CANBus::send(uint16_t motor_id, const std::array& data) { + struct can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + frame.can_id = motor_id; + frame.can_dlc = data.size(); + std::copy(data.begin(), data.end(), frame.data); + + if (write(sock_, &frame, sizeof(frame)) != sizeof(frame)) { + perror("Error sending CAN frame"); + return false; + } else { + // std::cout << "Sent CAN frame to motor ID " << motor_id << std::endl; + return true; + } +} + +struct can_frame CANBus::recv() { + struct can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + + // change socket into non blocking mode + // int flags = fcntl(sock_, F_GETFL, 0); + // fcntl(sock_, F_SETFL, flags | O_NONBLOCK); + + int nbytes = read(sock_, &frame, sizeof(struct can_frame)); + + if (nbytes < 0) { + perror("CAN read error"); + } else { + // std::cout << "Received CAN frame from motor ID " << frame.can_id << std::endl; + } + + return frame; +} diff --git a/openarm_hardware/src/motor.cpp b/openarm_hardware/src/motor.cpp new file mode 100644 index 0000000..1b23c32 --- /dev/null +++ b/openarm_hardware/src/motor.cpp @@ -0,0 +1,120 @@ +#include "motor.hpp" + +Motor::Motor(DM_Motor_Type motorType, uint16_t slaveID, uint16_t masterID) + : MotorType(motorType), SlaveID(slaveID), MasterID(masterID), + Pd(0.0), Vd(0.0), goal_position(0.0), goal_tau(0.0), + state_q(0.0), state_dq(0.0), state_tau(0.0), + state_tmos(0), state_trotor(0), + isEnable(false), NowControlMode(Control_Type::MIT) {} + +void Motor::recv_data(double q, double dq, double tau, int tmos, int trotor) { + state_q = q; + state_dq = dq; + state_tau = tau; + state_tmos = tmos; + state_trotor = trotor; +} + +double Motor::getPosition() const { return state_q; } +double Motor::getVelocity() const { return state_dq; } +double Motor::getTorque() const { return state_tau; } + + +double Motor::getGoalPosition() const { + return goal_position; +} + +void Motor::setGoalPosition(double pos) { + goal_position = pos; +} + +double Motor::getGoalTau() const { + return goal_tau; +} + +void Motor::setGoalTau(double tau) { + goal_tau = tau; +} + +int Motor::getStateTmos() const { + return state_tmos; +} + +void Motor::setStateTmos(int tmos) { + state_tmos = tmos; +} + +int Motor::getStateTrotor() const { + return state_trotor; +} + +void Motor::setStateTrotor(int trotor) { + state_trotor = trotor; +} + +int Motor::getParam(int RID) const { + auto it = temp_param_dict.find(RID); + return (it != temp_param_dict.end()) ? it->second : -1; +} + +double LIMIT_MIN_MAX(double x, double min, double max) { + return std::max(min, std::min(x, max)); +} + +uint16_t double_to_uint(double x, double x_min, double x_max, int bits) { + x = LIMIT_MIN_MAX(x, x_min, x_max); + double span = x_max - x_min; + double data_norm = (x - x_min) / span; + return static_cast(data_norm * ((1 << bits) - 1)); +} + +double uint_to_double(uint16_t x, double min, double max, int bits) { + double span = max - min; + double data_norm = static_cast(x) / ((1 << bits) - 1); + return data_norm * span + min; +} + +std::array double_to_uint8s(double value) { + std::array bytes; + std::memcpy(bytes.data(), &value, sizeof(double)); + return bytes; +} + +std::array data_to_uint8s(uint32_t value) { + std::array bytes; + std::memcpy(bytes.data(), &value, sizeof(uint32_t)); + return bytes; +} + +uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) { + uint32_t value; + uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; + std::memcpy(&value, bytes, sizeof(uint32_t)); + return value; +} + +double uint8s_to_double(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4) { + double value; + uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; + std::memcpy(&value, bytes, sizeof(double)); + return value; +} + +bool is_in_ranges(int number) { + return (7 <= number && number <= 10) || (13 <= number && number <= 16) || (35 <= number && number <= 36); +} + +void print_hex(const std::vector& data) { + for (auto byte : data) { + std::cout << std::hex << std::uppercase << (int)byte << " "; + } + std::cout << std::dec << std::endl; +} + +template +T get_enum_by_index(int index) { + if (index >= 0 && index < static_cast(T::COUNT)) { + return static_cast(index); + } + return static_cast(-1); +} diff --git a/openarm_hardware/src/motor_control.cpp b/openarm_hardware/src/motor_control.cpp new file mode 100644 index 0000000..9bafe67 --- /dev/null +++ b/openarm_hardware/src/motor_control.cpp @@ -0,0 +1,163 @@ +#include "motor_control.hpp" +#include "motor.hpp" + +MotorControl::MotorControl(CANBus& canbus) : canbus_(canbus) {} + +bool MotorControl::addMotor(Motor& motor) { + motors_map[motor.SlaveID] = &motor; + if (motor.MasterID != 0) { + motors_map[motor.MasterID] = &motor; + } + return true; +} + +void MotorControl::enable(Motor& motor) { + controlCmd(motor, 0xFC); + sleep(0.3); +} + +void MotorControl::disable(Motor& motor) { + controlCmd(motor, 0xFD); + sleep(0.3); +} + +void MotorControl::set_zero_position(Motor& motor){ + controlCmd(motor, 0xFE); + sleep(0.3); + recv(); +} + +void MotorControl::controlMIT(Motor& motor, double kp, double kd, double q, double dq, double tau) { + controlMIT2(motor, kp, kd, q, dq, tau); + recv(); +} + +void MotorControl::controlMIT2(Motor& motor, double kp, double kd, double q, double dq, double tau) { + + if (motors_map.find(motor.SlaveID) == motors_map.end()) { + std::cerr << "controlMIT ERROR: Motor ID not found" << std::endl; + return; + } + + uint16_t kp_uint = double_to_uint(kp, 0, 500, 12); + uint16_t kd_uint = double_to_uint(kd, 0, 5, 12); + + int motor_index = static_cast(motor.MotorType); + double Q_MAX = Limit_Param[motor_index][0]; + double DQ_MAX = Limit_Param[motor_index][1]; + double TAU_MAX = Limit_Param[motor_index][2]; + + uint16_t q_uint = double_to_uint(q, -Q_MAX, Q_MAX, 16); + uint16_t dq_uint = double_to_uint(dq, -DQ_MAX, DQ_MAX, 12); + uint16_t tau_uint = double_to_uint(tau, -TAU_MAX, TAU_MAX, 12); + + std::array data = { + static_cast((q_uint >> 8) & 0xFF), + static_cast(q_uint & 0xFF), + static_cast(dq_uint >> 4), + static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), + static_cast(kp_uint & 0xFF), + static_cast(kd_uint >> 4), + static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), + static_cast(tau_uint & 0xFF) + }; + + sendData(motor.SlaveID, data); +} + +void MotorControl::sendData(uint16_t motor_id, const std::array& data) { + canbus_.send(motor_id, data); +} + +void MotorControl::recv() { + can_frame frame = canbus_.recv(); + processPacket(frame); +} + +void MotorControl::processPacket(const can_frame& frame) { + uint16_t motorID = frame.data[0]; + uint8_t cmd = 0x11; + + if (cmd == 0x11) { + if (motorID != 0x00) { + auto it = motors_map.find(motorID); + if (it != motors_map.end() && it->second) { + Motor* motor = it->second; + + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; + + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } else { + uint16_t MasterID = frame.data[0] & 0x0F; + auto it = motors_map.find(MasterID); + if (it != motors_map.end() && it->second) { + Motor* motor = it->second; + + uint16_t q_uint = (frame.data[1] << 8) | frame.data[2]; + uint16_t dq_uint = (frame.data[3] << 4) | (frame.data[4] >> 4); + uint16_t tau_uint = ((frame.data[4] & 0xf) << 8) | frame.data[5]; + int t_mos = frame.data[6]; + int t_rotor = frame.data[7]; + + double Q_MAX = Limit_Param[static_cast(motor->MotorType)][0]; + double DQ_MAX = Limit_Param[static_cast(motor->MotorType)][1]; + double TAU_MAX = Limit_Param[static_cast(motor->MotorType)][2]; + + double recv_q = uint_to_double(q_uint, -Q_MAX, Q_MAX, 16); + double recv_dq = uint_to_double(dq_uint, -DQ_MAX, DQ_MAX, 12); + double recv_tau = uint_to_double(tau_uint, -TAU_MAX, TAU_MAX, 12); + + motor->recv_data(recv_q, recv_dq, recv_tau, t_mos, t_rotor); + } + } + } +} + +void MotorControl::controlCmd(Motor& motor, uint8_t cmd) { + std::array data_buf = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd}; + sendData(motor.SlaveID, data_buf); +} + +void MotorControl::readRIDParam(Motor& motor, DM_variable RID) { + std::array data = { + static_cast(motor.SlaveID & 0xFF), + static_cast((motor.SlaveID >> 8) & 0xFF), + 0x33, + static_cast(RID), + 0x00, 0x00, 0x00, 0x00 + }; + canbus_.send(0x7FF, data); +} + +void MotorControl::writeMotorParam(Motor& motor, DM_variable RID, double value) { + std::array data = { + static_cast(motor.SlaveID & 0xFF), + static_cast((motor.SlaveID >> 8) & 0xFF), + 0x55, + static_cast(RID) + }; + + if (is_in_ranges(static_cast(RID))) { + auto intData = data_to_uint8s(static_cast(value)); + std::copy(intData.begin(), intData.end(), data.begin() + 4); + } else { + auto doubleData = double_to_uint8s(value); + std::copy(doubleData.begin(), doubleData.end(), data.begin() + 4); + } + + canbus_.send(0x7FF, data); +} + diff --git a/openarm_hardware/src/openarm_hardware.cpp b/openarm_hardware/src/openarm_hardware.cpp new file mode 100644 index 0000000..799d2cf --- /dev/null +++ b/openarm_hardware/src/openarm_hardware.cpp @@ -0,0 +1,138 @@ +// Copyright (c) 2025, Reazon Holdings, Inc. +// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "openarm_hardware/openarm_hardware.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.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); + } +} + +hardware_interface::CallbackReturn OpenArmHW::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + 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()); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArmHW::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // zero position or calibrate to pose + + return CallbackReturn::SUCCESS; +} + +std::vector OpenArmHW::export_state_interfaces() +{ + std::vector state_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++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])); + } + + return state_interfaces; +} + +std::vector OpenArmHW::export_command_interfaces() +{ + std::vector command_interfaces; + for (size_t i = 0; i < info_.joints.size(); ++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])); + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &tau_ff_commands_[i])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn OpenArmHW::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for(const auto& motor: motors_){ + motor_control_.enable(*motor); + } + read(rclcpp::Time(0), rclcpp::Duration(0, 0)); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn OpenArmHW::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + for(const auto& motor: motors_){ + motor_control_.disable(*motor); + } + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type OpenArmHW::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for(size_t i = 0; i < motors_.size(); ++i){ + pos_states_[i] = motors_[i]->getPosition(); + vel_states_[i] = motors_[i]->getVelocity(); + tau_states_[i] = motors_[i]->getTorque(); + } + + return hardware_interface::return_type::OK; +} + +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]); + } + return hardware_interface::return_type::OK; +} + +} // namespace openarm_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + openarm_hardware::OpenArmHW, hardware_interface::SystemInterface) diff --git a/openarm_hardware/test/test_openarm_hardware.cpp b/openarm_hardware/test/test_openarm_hardware.cpp new file mode 100644 index 0000000..ffdbb7e --- /dev/null +++ b/openarm_hardware/test/test_openarm_hardware.cpp @@ -0,0 +1,131 @@ +// Copyright (c) 2025, Reazon Holdings, Inc. +// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +class TestOpenArmHW : public ::testing::Test +{ +protected: + void SetUp() override + { + openarm_hardware_7dof_ = + R"( + + + + mock_components/GenericSystem + openarm_hardware/OpenArmHW + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + + + 0 + + + + + + + + 0 + + + + + + 0 + + + + )"; + } + + std::string openarm_hardware_7dof_; +}; + +TEST_F(TestOpenArmHW, load_openarm_hardware_7dof) +{ + auto urdf = ros2_control_test_assets::urdf_head + openarm_hardware_7dof_ + + ros2_control_test_assets::urdf_tail; + ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); +} diff --git a/openarm_moveit_config/.setup_assistant b/openarm_moveit_config/.setup_assistant index 879c027..db9cf4d 100644 --- a/openarm_moveit_config/.setup_assistant +++ b/openarm_moveit_config/.setup_assistant @@ -1,20 +1,22 @@ moveit_setup_assistant_config: urdf: package: openarm_description - relative_path: urdf/openarm_grip.urdf + relative_path: urdf/openarm.urdf srdf: - relative_path: config/openarm_grip.srdf + relative_path: config/openarm.srdf package_settings: author_name: Thomason Zhou author_email: t95zhou@uwaterloo.ca - generated_timestamp: 1738914276 + generated_timestamp: 1741767765 control_xacro: command: - position - velocity + - effort state: - position - velocity + - effort modified_urdf: xacros: - control_xacro @@ -22,6 +24,8 @@ moveit_setup_assistant_config: command: - position - velocity + - effort state: - position - velocity + - effort \ 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 6acd557..b636de3 100644 --- a/openarm_moveit_config/config/initial_positions.yaml +++ b/openarm_moveit_config/config/initial_positions.yaml @@ -1,12 +1,12 @@ -# Default initial positions for openarm_grip's ros2_control fake system +# Default initial positions for openarm's ros2_control fake system initial_positions: - slider_left: 0 - slider_right: 0 + left_pris1: 0 + right_pris2: 0 rev1: 0 rev2: 0 rev3: 0 rev4: 0 rev5: 0 rev6: 0 - rev7: 0 \ No newline at end of file + rev7: 0 diff --git a/openarm_moveit_config/config/joint_limits.yaml b/openarm_moveit_config/config/joint_limits.yaml index 9c07140..605dd2a 100644 --- a/openarm_moveit_config/config/joint_limits.yaml +++ b/openarm_moveit_config/config/joint_limits.yaml @@ -8,48 +8,53 @@ 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: - slider_left: - has_velocity_limits: true - max_velocity: 100.0 + left_pris1: + has_velocity_limits: false + max_velocity: 0 has_acceleration_limits: false - max_acceleration: 0.0 - slider_right: - has_velocity_limits: true - max_velocity: 100.0 + max_acceleration: 0 + right_pris2: + has_velocity_limits: false + max_velocity: 0 has_acceleration_limits: false - max_acceleration: 0.0 + max_acceleration: 0 rev1: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev2: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev3: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev4: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev5: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev6: - has_velocity_limits: true - max_velocity: 0.5 - has_acceleration_limits: true - max_acceleration: 0.5 + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 rev7: has_velocity_limits: false - max_velocity: 0.0 + max_velocity: 0 has_acceleration_limits: false - max_acceleration: 0.0 \ No newline at end of file + max_acceleration: 0 + right_pris2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/openarm_moveit_config/config/moveit.rviz b/openarm_moveit_config/config/moveit.rviz index f52a245..9bbdad1 100644 --- a/openarm_moveit_config/config/moveit.rviz +++ b/openarm_moveit_config/config/moveit.rviz @@ -19,10 +19,6 @@ Visualization Manager: Loop Animation: true State Display Time: 0.05 s Trajectory Topic: display_planned_path - Planning Request: - Interactive Marker Size: 0.1 - Planning Group: openarm_arm - Query Goal State: true Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: @@ -31,7 +27,7 @@ Visualization Manager: Robot Alpha: 0.5 Value: true Global Options: - Fixed Frame: world + Fixed Frame: link1 Tools: - Class: rviz_default_plugins/Interact - Class: rviz_default_plugins/MoveCamera @@ -47,7 +43,7 @@ Visualization Manager: Z: 0.30 Name: Current View Pitch: 0.5 - Target Frame: world + Target Frame: link1 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 3f36553..351d5b3 100644 --- a/openarm_moveit_config/config/moveit_controllers.yaml +++ b/openarm_moveit_config/config/moveit_controllers.yaml @@ -16,13 +16,10 @@ moveit_simple_controller_manager: - rev4 - rev5 - rev6 - action_ns: follow_joint_trajectory - default: true - gripper_controller: - type: FollowJointTrajectory - joints: - rev7 - - slider_left - - slider_right - action_ns: follow_joint_trajectory + gripper_controller: + type: GripperCommand + joints: + - left_pris1 + action_ns: gripper_cmd default: true \ No newline at end of file diff --git a/openarm_moveit_config/config/ompl_planning.yaml b/openarm_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..96e2be2 --- /dev/null +++ b/openarm_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,38 @@ +openarm_moveit_config: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.005 + +planning_plugin: 'ompl_interface/OMPLPlanner' +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints +start_state_max_bounds_error: 0.1 diff --git a/openarm_moveit_config/config/openarm.srdf b/openarm_moveit_config/config/openarm.srdf new file mode 100644 index 0000000..b372d79 --- /dev/null +++ b/openarm_moveit_config/config/openarm.srdf @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/openarm_moveit_config/config/openarm.urdf.xacro b/openarm_moveit_config/config/openarm.urdf.xacro new file mode 100644 index 0000000..7f0add7 --- /dev/null +++ b/openarm_moveit_config/config/openarm.urdf.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/openarm_moveit_config/config/openarm_grip.srdf b/openarm_moveit_config/config/openarm_grip.srdf deleted file mode 100644 index e910649..0000000 --- a/openarm_moveit_config/config/openarm_grip.srdf +++ /dev/null @@ -1,55 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/openarm_moveit_config/config/openarm_grip.urdf.xacro b/openarm_moveit_config/config/openarm_grip.urdf.xacro deleted file mode 100644 index e7607a0..0000000 --- a/openarm_moveit_config/config/openarm_grip.urdf.xacro +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/openarm_moveit_config/config/ros2_controllers.yaml b/openarm_moveit_config/config/ros2_controllers.yaml index b887623..388016c 100644 --- a/openarm_moveit_config/config/ros2_controllers.yaml +++ b/openarm_moveit_config/config/ros2_controllers.yaml @@ -3,17 +3,20 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - openarm_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - - gripper_controller: type: position_controllers/GripperActionController + openarm_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster +gripper_controller: + ros__parameters: + joint: left_pris1 openarm_arm_controller: ros__parameters: joints: @@ -23,13 +26,13 @@ openarm_arm_controller: - rev4 - rev5 - rev6 + - rev7 command_interfaces: - position - velocity + - effort state_interfaces: - position - velocity - allow_nonzero_velocity_at_trajectory_end: true -gripper_controller: - ros__parameters: - joint: rev7 \ No newline at end of file + - effort + allow_nonzero_velocity_at_trajectory_end: false diff --git a/openarm_moveit_config/launch/demo.launch.py b/openarm_moveit_config/launch/demo.launch.py index f7ab4b3..b74a3b1 100644 --- a/openarm_moveit_config/launch/demo.launch.py +++ b/openarm_moveit_config/launch/demo.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_demo_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_demo_launch(moveit_config) diff --git a/openarm_moveit_config/launch/move_group.launch.py b/openarm_moveit_config/launch/move_group.launch.py index bbb1667..06f38ff 100644 --- a/openarm_moveit_config/launch/move_group.launch.py +++ b/openarm_moveit_config/launch/move_group.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_move_group_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_move_group_launch(moveit_config) diff --git a/openarm_moveit_config/launch/moveit_rviz.launch.py b/openarm_moveit_config/launch/moveit_rviz.launch.py index a25cbb7..c9815b6 100644 --- a/openarm_moveit_config/launch/moveit_rviz.launch.py +++ b/openarm_moveit_config/launch/moveit_rviz.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_moveit_rviz_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_moveit_rviz_launch(moveit_config) diff --git a/openarm_moveit_config/launch/rsp.launch.py b/openarm_moveit_config/launch/rsp.launch.py index 0f098a9..2f40bee 100644 --- a/openarm_moveit_config/launch/rsp.launch.py +++ b/openarm_moveit_config/launch/rsp.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_rsp_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_rsp_launch(moveit_config) diff --git a/openarm_moveit_config/launch/setup_assistant.launch.py b/openarm_moveit_config/launch/setup_assistant.launch.py index 27d984b..d8016bf 100644 --- a/openarm_moveit_config/launch/setup_assistant.launch.py +++ b/openarm_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_setup_assistant_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_setup_assistant_launch(moveit_config) diff --git a/openarm_moveit_config/launch/spawn_controllers.launch.py b/openarm_moveit_config/launch/spawn_controllers.launch.py index 327d046..be7db39 100644 --- a/openarm_moveit_config/launch/spawn_controllers.launch.py +++ b/openarm_moveit_config/launch/spawn_controllers.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_spawn_controllers_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_spawn_controllers_launch(moveit_config) diff --git a/openarm_moveit_config/launch/static_virtual_joint_tfs.launch.py b/openarm_moveit_config/launch/static_virtual_joint_tfs.launch.py index b937f99..1eedb5a 100644 --- a/openarm_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/openarm_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_laun def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/openarm_moveit_config/launch/warehouse_db.launch.py b/openarm_moveit_config/launch/warehouse_db.launch.py index 2a5a228..6f9214b 100644 --- a/openarm_moveit_config/launch/warehouse_db.launch.py +++ b/openarm_moveit_config/launch/warehouse_db.launch.py @@ -3,5 +3,5 @@ from moveit_configs_utils.launches import generate_warehouse_db_launch def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("openarm_grip", package_name="openarm_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder("openarm", package_name="openarm_moveit_config").to_moveit_configs() return generate_warehouse_db_launch(moveit_config) diff --git a/openarm_moveit_config/package.xml b/openarm_moveit_config/package.xml index ea844bb..8bd15d5 100644 --- a/openarm_moveit_config/package.xml +++ b/openarm_moveit_config/package.xml @@ -4,7 +4,7 @@ openarm_moveit_config 0.3.0 - Configuration and launch files for using OpenArm with the MoveIt Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the openarm with the MoveIt Motion Planning Framework Thomason Zhou @@ -47,6 +47,6 @@ - ament_cmake + ament_cmake