diff --git a/ada_description/launch/view_ada.launch.py b/ada_description/launch/view_ada.launch.py index 0d05eec..c1bbd4a 100644 --- a/ada_description/launch/view_ada.launch.py +++ b/ada_description/launch/view_ada.launch.py @@ -60,15 +60,16 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="ada.xacro", + default_value="ada_standalone.xacro", description="URDF/XACRO description file with the robot.", ) ) declared_arguments.append( DeclareLaunchArgument( - "use_forque", - default_value="false", - description="If the forque apparatus is being used.", + "end_effector_tool", + default_value="none", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], ) ) @@ -76,7 +77,7 @@ def generate_launch_description(): # General arguments description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") - use_forque = LaunchConfiguration("use_forque") + end_effector_tool = LaunchConfiguration("end_effector_tool") robot_description_content = Command( [ @@ -86,8 +87,8 @@ def generate_launch_description(): [FindPackageShare(description_package), "urdf", description_file] ), " ", - "use_forque:=", - use_forque, + "end_effector_tool:=", + end_effector_tool, ] ) robot_description = { diff --git a/ada_description/meshes/forque/AFStabilizer.stl b/ada_description/meshes/forque/AFStabilizer.stl new file mode 100644 index 0000000..8d70e22 Binary files /dev/null and b/ada_description/meshes/forque/AFStabilizer.stl differ diff --git a/ada_description/meshes/forque/XC-430_idle.stl b/ada_description/meshes/forque/XC-430_idle.stl new file mode 100644 index 0000000..8afa37e Binary files /dev/null and b/ada_description/meshes/forque/XC-430_idle.stl differ diff --git a/ada_description/meshes/forque/fr12-h103.stl b/ada_description/meshes/forque/fr12-h103.stl new file mode 100644 index 0000000..9d93e1b Binary files /dev/null and b/ada_description/meshes/forque/fr12-h103.stl differ diff --git a/ada_description/meshes/forque/fr12-s101.stl b/ada_description/meshes/forque/fr12-s101.stl new file mode 100644 index 0000000..b34d5a1 Binary files /dev/null and b/ada_description/meshes/forque/fr12-s101.stl differ diff --git a/ada_description/meshes/forque/roll_to_ft_adapter.stl b/ada_description/meshes/forque/roll_to_ft_adapter.stl new file mode 100644 index 0000000..c45ab7d Binary files /dev/null and b/ada_description/meshes/forque/roll_to_ft_adapter.stl differ diff --git a/ada_description/rviz/view_robot.rviz b/ada_description/rviz/view_robot.rviz index d51156d..15546ea 100644 --- a/ada_description/rviz/view_robot.rviz +++ b/ada_description/rviz/view_robot.rviz @@ -67,6 +67,30 @@ Visualization Manager: Expand Joint Details: false Expand Link Details: false Expand Tree: false + af_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + af_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + af_motor_fixture_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + af_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + af_ft_adapter_link: + Alpha: 1 + Show Axes: false + Show Trail: false FT: Alpha: 1 Show Axes: false diff --git a/ada_description/urdf/ada.xacro b/ada_description/urdf/ada.xacro index 9eb7a14..a0c2fc3 100644 --- a/ada_description/urdf/ada.xacro +++ b/ada_description/urdf/ada.xacro @@ -12,65 +12,63 @@ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" - xmlns:xacro="http://www.ros.org/wiki/xacro" name="ada"> + xmlns:xacro="http://www.ros.org/wiki/xacro"> - - - + + + + - - - - - - - - - - - - - - + + + + + + + + + + - + - + - + - - - - - - - - - + + + + + + + + + - - - - + + + + - - + + - - - - - - - + + + + + + + + diff --git a/ada_description/urdf/ada_standalone.xacro b/ada_description/urdf/ada_standalone.xacro new file mode 100644 index 0000000..1ef2a3a --- /dev/null +++ b/ada_description/urdf/ada_standalone.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + diff --git a/ada_description/urdf/forque/forque.xacro b/ada_description/urdf/forque/forque.xacro index bee27dc..c5ca7e7 100644 --- a/ada_description/urdf/forque/forque.xacro +++ b/ada_description/urdf/forque/forque.xacro @@ -48,57 +48,118 @@ - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - + - + - - - - - + + + + + - - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + - + diff --git a/ada_hardware/CMakeLists.txt b/ada_hardware/CMakeLists.txt index 8b4534d..0a43d51 100644 --- a/ada_hardware/CMakeLists.txt +++ b/ada_hardware/CMakeLists.txt @@ -14,16 +14,18 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(pluginlib REQUIRED) find_package(hardware_interface REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(dynamixel_workbench_toolbox REQUIRED) add_library(ada_hardware SHARED src/jaco2.cpp - src/isaacsim.cpp) + src/isaacsim.cpp + src/dynamixel_hardware.cpp) target_include_directories(ada_hardware PUBLIC include) target_link_libraries(ada_hardware -l:Kinova.API.CommLayerUbuntu.so -l:Kinova.API.USBCommandLayerUbuntu.so) -ament_target_dependencies(ada_hardware rclcpp rclcpp_lifecycle pluginlib hardware_interface sensor_msgs) +ament_target_dependencies(ada_hardware rclcpp rclcpp_lifecycle lifecycle_msgs pluginlib hardware_interface sensor_msgs dynamixel_workbench_toolbox) pluginlib_export_plugin_description_file(hardware_interface ada_hardware.xml) @@ -72,7 +74,17 @@ if(BUILD_TESTING) hardware_interface sensor_msgs ) + + ament_add_gmock(test_dynamixel test/test_dynamixel.cpp) + ament_target_dependencies(test_dynamixel + pluginlib + ros2_control_test_assets + rclcpp + rclcpp_lifecycle + hardware_interface + sensor_msgs + ) endif() -ament_export_dependencies(rclcpp rclcpp_lifecycle pluginlib hardware_interface) +ament_export_dependencies(rclcpp rclcpp_lifecycle pluginlib hardware_interface dynamixel_workbench_toolbox) ament_package() diff --git a/ada_hardware/ada_hardware.xml b/ada_hardware/ada_hardware.xml index 9d35dd7..9ee6a6c 100644 --- a/ada_hardware/ada_hardware.xml +++ b/ada_hardware/ada_hardware.xml @@ -19,4 +19,12 @@ Interface for controlling a 6DOF arm (+ 2 fingers) in IsaacSim. + + + + The ROBOTIS Dynamixel system interface. + + diff --git a/ada_hardware/config/ada_hardware_forward_controllers.yaml b/ada_hardware/config/ada_hardware_forward_controllers.yaml index 61baf32..86adf79 100644 --- a/ada_hardware/config/ada_hardware_forward_controllers.yaml +++ b/ada_hardware/config/ada_hardware_forward_controllers.yaml @@ -28,6 +28,8 @@ forward_position_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 forward_velocity_controller: ros__parameters: @@ -40,6 +42,8 @@ forward_velocity_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 forward_effort_controller: ros__parameters: @@ -52,3 +56,5 @@ forward_effort_controller: - j2n6s200_joint_6 - j2n6s200_joint_finger_1 - j2n6s200_joint_finger_2 + - af_joint_1 + - af_joint_2 diff --git a/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp new file mode 100644 index 0000000..ef93f07 --- /dev/null +++ b/ada_hardware/include/ada_hardware/dynamixel_hardware.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 Yutaka Kondo +// +// 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 DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ +#define DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include + +#include "ada_hardware/visiblity_control.h" +#include "rclcpp/macros.hpp" + +using hardware_interface::CallbackReturn; +using hardware_interface::return_type; + +namespace dynamixel_hardware { +struct JointValue { + double position{0.0}; + double velocity{0.0}; + double effort{0.0}; +}; + +struct Joint { + JointValue state{}; + JointValue command{}; + JointValue prev_command{}; +}; + +enum class ControlMode { + Position, + Velocity, + Torque, + Current, + ExtendedPosition, + MultiTurn, + CurrentBasedPosition, + PWM, +}; + +class DynamixelHardware : public hardware_interface::SystemInterface { +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DynamixelHardware) + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + + DYNAMIXEL_HARDWARE_PUBLIC + std::vector + export_state_interfaces() override; + + DYNAMIXEL_HARDWARE_PUBLIC + std::vector + export_command_interfaces() override; + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_PUBLIC + CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + DYNAMIXEL_HARDWARE_PUBLIC + return_type read(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + DYNAMIXEL_HARDWARE_PUBLIC + return_type write(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + +private: + return_type enable_torque(const bool enabled); + + return_type set_control_mode(const ControlMode &mode, + const bool force_set = false); + + return_type reset_command(); + + CallbackReturn set_joint_positions(); + CallbackReturn set_joint_velocities(); + CallbackReturn set_joint_params(); + + DynamixelWorkbench dynamixel_workbench_; + std::map control_items_; + std::vector joints_; + std::vector joint_ids_; + bool torque_enabled_{false}; + ControlMode control_mode_{ControlMode::Position}; + bool mode_changed_{false}; + bool use_dummy_{false}; +}; +} // namespace dynamixel_hardware + +#endif // DYNAMIXEL_HARDWARE__DYNAMIXEL_HARDWARE_HPP_ diff --git a/ada_hardware/include/ada_hardware/visiblity_control.h b/ada_hardware/include/ada_hardware/visiblity_control.h new file mode 100644 index 0000000..3b842bb --- /dev/null +++ b/ada_hardware/include/ada_hardware/visiblity_control.h @@ -0,0 +1,56 @@ +// Copyright 2020 Yutaka Kondo +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef DYNAMIXEL_HARDWARE__VISIBLITY_CONTROL_H_ +#define DYNAMIXEL_HARDWARE__VISIBLITY_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 DYNAMIXEL_HARDWARE_EXPORT __attribute__((dllexport)) +#define DYNAMIXEL_HARDWARE_IMPORT __attribute__((dllimport)) +#else +#define DYNAMIXEL_HARDWARE_EXPORT __declspec(dllexport) +#define DYNAMIXEL_HARDWARE_IMPORT __declspec(dllimport) +#endif +#ifdef DYNAMIXEL_HARDWARE_BUILDING_DLL +#define DYNAMIXEL_HARDWARE_PUBLIC DYNAMIXEL_HARDWARE_EXPORT +#else +#define DYNAMIXEL_HARDWARE_PUBLIC DYNAMIXEL_HARDWARE_IMPORT +#endif +#define DYNAMIXEL_HARDWARE_PUBLIC_TYPE DYNAMIXEL_HARDWARE_PUBLIC +#define DYNAMIXEL_HARDWARE_LOCAL +#else +#define DYNAMIXEL_HARDWARE_EXPORT __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_IMPORT +#if __GNUC__ >= 4 +#define DYNAMIXEL_HARDWARE_PUBLIC __attribute__((visibility("default"))) +#define DYNAMIXEL_HARDWARE_LOCAL __attribute__((visibility("hidden"))) +#else +#define DYNAMIXEL_HARDWARE_PUBLIC +#define DYNAMIXEL_HARDWARE_LOCAL +#endif +#define DYNAMIXEL_HARDWARE_PUBLIC_TYPE +#endif + +#endif // DYNAMIXEL_HARDWARE__VISIBLITY_CONTROL_H_ diff --git a/ada_hardware/launch/ada_hardware.launch.py b/ada_hardware/launch/ada_hardware.launch.py index 63d25b6..171be36 100644 --- a/ada_hardware/launch/ada_hardware.launch.py +++ b/ada_hardware/launch/ada_hardware.launch.py @@ -98,9 +98,10 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - "use_forque", - default_value="true", - description="Whether to include F/T sensor.", + "end_effector_tool", + default_value="none", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], ) ) declared_arguments.append( @@ -140,7 +141,7 @@ def generate_launch_description(): description_file = LaunchConfiguration("description_file") sim = LaunchConfiguration("sim") readonly = LaunchConfiguration("readonly") - use_forque = LaunchConfiguration("use_forque") + end_effector_tool = LaunchConfiguration("end_effector_tool") robot_controller = LaunchConfiguration("robot_controller") start_controller = LaunchConfiguration("start_controller") start_rviz = LaunchConfiguration("start_rviz") @@ -162,8 +163,8 @@ def generate_launch_description(): "readonly:=", readonly, " ", - "use_forque:=", - use_forque, + "end_effector_tool:=", + end_effector_tool, ] ) robot_description = { diff --git a/ada_hardware/package.xml b/ada_hardware/package.xml index 2a31563..3a272fd 100644 --- a/ada_hardware/package.xml +++ b/ada_hardware/package.xml @@ -2,7 +2,7 @@ ada_hardware - 0.0.1 + 0.0.2 ROS Hardware Interface for ADA Ethan K. Gordon BSD-3-Clause @@ -12,8 +12,10 @@ rclcpp rclcpp_lifecycle pluginlib + lifecycle_msgs hardware_interface sensor_msgs + dynamixel_workbench_toolbox ament_lint_auto ament_cmake_copyright diff --git a/ada_hardware/src/dynamixel_hardware.cpp b/ada_hardware/src/dynamixel_hardware.cpp new file mode 100644 index 0000000..edf2d4a --- /dev/null +++ b/ada_hardware/src/dynamixel_hardware.cpp @@ -0,0 +1,525 @@ +// Copyright 2020 Yutaka Kondo +// +// 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 "ada_hardware/dynamixel_hardware.hpp" + +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace dynamixel_hardware { +constexpr const char *kDynamixelHardware = "DynamixelHardware"; +constexpr uint8_t kGoalPositionIndex = 0; +constexpr uint8_t kGoalVelocityIndex = 1; +constexpr uint8_t kPresentPositionVelocityCurrentIndex = 0; +constexpr const char *kGoalPositionItem = "Goal_Position"; +constexpr const char *kGoalVelocityItem = "Goal_Velocity"; +constexpr const char *kMovingSpeedItem = "Moving_Speed"; +constexpr const char *kPresentPositionItem = "Present_Position"; +constexpr const char *kPresentVelocityItem = "Present_Velocity"; +constexpr const char *kPresentSpeedItem = "Present_Speed"; +constexpr const char *kPresentCurrentItem = "Present_Current"; +constexpr const char *kPresentLoadItem = "Present_Load"; +constexpr const char *const kExtraJointParameters[] = { + "Profile_Velocity", "Profile_Acceleration", "Position_P_Gain", + "Position_I_Gain", "Position_D_Gain", "Velocity_P_Gain", + "Velocity_I_Gain", +}; + +CallbackReturn +DynamixelHardware::on_init(const hardware_interface::HardwareInfo &info) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "configure"); + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + joints_.resize(info_.joints.size(), Joint()); + joint_ids_.resize(info_.joints.size(), 0); + + for (uint i = 0; i < info_.joints.size(); i++) { + joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id")); + joints_[i].state.position = std::numeric_limits::quiet_NaN(); + joints_[i].state.velocity = std::numeric_limits::quiet_NaN(); + joints_[i].state.effort = std::numeric_limits::quiet_NaN(); + joints_[i].command.position = std::numeric_limits::quiet_NaN(); + joints_[i].command.velocity = std::numeric_limits::quiet_NaN(); + joints_[i].command.effort = std::numeric_limits::quiet_NaN(); + joints_[i].prev_command.position = joints_[i].command.position; + joints_[i].prev_command.velocity = joints_[i].command.velocity; + joints_[i].prev_command.effort = joints_[i].command.effort; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, + joint_ids_[i]); + } + + if (info_.hardware_parameters.find("use_dummy") != + info_.hardware_parameters.end() && + info_.hardware_parameters.at("use_dummy") == "true") { + use_dummy_ = true; + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "dummy mode"); + return CallbackReturn::SUCCESS; + } + + auto usb_port = info_.hardware_parameters.at("usb_port"); + auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate")); + const char *log = nullptr; + + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", + usb_port.c_str()); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", + baud_rate); + + if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + for (uint i = 0; i < info_.joints.size(); ++i) { + uint16_t model_number = 0; + if (!dynamixel_workbench_.ping(joint_ids_[i], &model_number, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + } + + enable_torque(false); + set_control_mode(ControlMode::Position, true); + set_joint_params(); + enable_torque(true); + + const ControlItem *goal_position = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem); + if (goal_position == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *goal_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalVelocityItem); + if (goal_velocity == nullptr) { + goal_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kMovingSpeedItem); + } + if (goal_velocity == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_position = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentPositionItem); + if (present_position == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentVelocityItem); + if (present_velocity == nullptr) { + present_velocity = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentSpeedItem); + } + if (present_velocity == nullptr) { + return CallbackReturn::ERROR; + } + + const ControlItem *present_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentCurrentItem); + if (present_current == nullptr) { + present_current = + dynamixel_workbench_.getItemInfo(joint_ids_[0], kPresentLoadItem); + } + if (present_current == nullptr) { + return CallbackReturn::ERROR; + } + + control_items_[kGoalPositionItem] = goal_position; + control_items_[kGoalVelocityItem] = goal_velocity; + control_items_[kPresentPositionItem] = present_position; + control_items_[kPresentVelocityItem] = present_velocity; + control_items_[kPresentCurrentItem] = present_current; + + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalPositionItem]->address, + control_items_[kGoalPositionItem]->data_length, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + if (!dynamixel_workbench_.addSyncWriteHandler( + control_items_[kGoalVelocityItem]->address, + control_items_[kGoalVelocityItem]->data_length, &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + uint16_t start_address = + std::min(control_items_[kPresentPositionItem]->address, + control_items_[kPresentCurrentItem]->address); + uint16_t read_length = control_items_[kPresentPositionItem]->data_length + + control_items_[kPresentVelocityItem]->data_length + + control_items_[kPresentCurrentItem]->data_length + 2; + if (!dynamixel_workbench_.addSyncReadHandler(start_address, read_length, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; +} + +std::vector +DynamixelHardware::export_state_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), + "export_state_interfaces"); + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &joints_[i].state.position)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &joints_[i].state.velocity)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, + &joints_[i].state.effort)); + } + + return state_interfaces; +} + +std::vector +DynamixelHardware::export_command_interfaces() { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), + "export_command_interfaces"); + std::vector command_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &joints_[i].command.position)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &joints_[i].command.velocity)); + } + + return command_interfaces; +} + +CallbackReturn DynamixelHardware::on_activate( + const rclcpp_lifecycle::State & /* previous_state */) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "start"); + for (uint i = 0; i < joints_.size(); i++) { + if (use_dummy_ && std::isnan(joints_[i].state.position)) { + joints_[i].state.position = 0.0; + joints_[i].state.velocity = 0.0; + joints_[i].state.effort = 0.0; + } + } + read(rclcpp::Time{}, rclcpp::Duration(0, 0)); + reset_command(); + write(rclcpp::Time{}, rclcpp::Duration(0, 0)); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::on_deactivate( + const rclcpp_lifecycle::State & /* previous_state */) { + RCLCPP_DEBUG(rclcpp::get_logger(kDynamixelHardware), "stop"); + return CallbackReturn::SUCCESS; +} + +return_type DynamixelHardware::read(const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { + if (use_dummy_) { + return return_type::OK; + } + + std::vector ids(info_.joints.size(), 0); + std::vector positions(info_.joints.size(), 0); + std::vector velocities(info_.joints.size(), 0); + std::vector currents(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + const char *log = nullptr; + + if (!dynamixel_workbench_.syncRead(kPresentPositionVelocityCurrentIndex, + ids.data(), ids.size(), &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentCurrentItem]->address, + control_items_[kPresentCurrentItem]->data_length, currents.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentVelocityItem]->address, + control_items_[kPresentVelocityItem]->data_length, velocities.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + if (!dynamixel_workbench_.getSyncReadData( + kPresentPositionVelocityCurrentIndex, ids.data(), ids.size(), + control_items_[kPresentPositionItem]->address, + control_items_[kPresentPositionItem]->data_length, positions.data(), + &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + + for (uint i = 0; i < ids.size(); i++) { + joints_[i].state.position = + dynamixel_workbench_.convertValue2Radian(ids[i], positions[i]); + joints_[i].state.velocity = + dynamixel_workbench_.convertValue2Velocity(ids[i], velocities[i]); + joints_[i].state.effort = + dynamixel_workbench_.convertValue2Current(currents[i]); + } + + return return_type::OK; +} + +return_type DynamixelHardware::write(const rclcpp::Time & /* time */, + const rclcpp::Duration & /* period */) { + if (use_dummy_) { + for (auto &joint : joints_) { + joint.prev_command.position = joint.command.position; + joint.state.position = joint.command.position; + } + return return_type::OK; + } + + // Velocity control + if (std::any_of(joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.velocity != j.prev_command.velocity; + })) { + set_control_mode(ControlMode::Velocity); + if (mode_changed_) { + set_joint_params(); + } + set_joint_velocities(); + return return_type::OK; + } + + // Position control + if (std::any_of(joints_.cbegin(), joints_.cend(), [](auto j) { + return j.command.position != j.prev_command.position; + })) { + set_control_mode(ControlMode::Position); + if (mode_changed_) { + set_joint_params(); + } + set_joint_positions(); + return return_type::OK; + } + + // Effort control + if (std::any_of(joints_.cbegin(), joints_.cend(), + [](auto j) { return j.command.effort != 0.0; })) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), + "Effort control is not implemented"); + return return_type::ERROR; + } + + // If all command values are unchanged, then remain in existing control mode + // and set corresponding command values + switch (control_mode_) { + case ControlMode::Velocity: + set_joint_velocities(); + return return_type::OK; + break; + case ControlMode::Position: + set_joint_positions(); + return return_type::OK; + break; + default: // effort, etc + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), + "Control mode not implemented"); + return return_type::ERROR; + break; + } +} + +return_type DynamixelHardware::enable_torque(const bool enabled) { + const char *log = nullptr; + + if (enabled && !torque_enabled_) { + for (uint i = 0; i < info_.joints.size(); ++i) { + if (!dynamixel_workbench_.torqueOn(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + reset_command(); + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque enabled"); + } else if (!enabled && torque_enabled_) { + for (uint i = 0; i < info_.joints.size(); ++i) { + if (!dynamixel_workbench_.torqueOff(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Torque disabled"); + } + + torque_enabled_ = enabled; + return return_type::OK; +} + +return_type DynamixelHardware::set_control_mode(const ControlMode &mode, + const bool force_set) { + const char *log = nullptr; + mode_changed_ = false; + + if (mode == ControlMode::Velocity && + (force_set || control_mode_ != ControlMode::Velocity)) { + bool torque_enabled = torque_enabled_; + if (torque_enabled) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setVelocityControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Velocity control"); + if (control_mode_ != ControlMode::Velocity) { + mode_changed_ = true; + control_mode_ = ControlMode::Velocity; + } + + if (torque_enabled) { + enable_torque(true); + } + return return_type::OK; + } + + if (mode == ControlMode::Position && + (force_set || control_mode_ != ControlMode::Position)) { + bool torque_enabled = torque_enabled_; + if (torque_enabled) { + enable_torque(false); + } + + for (uint i = 0; i < joint_ids_.size(); ++i) { + if (!dynamixel_workbench_.setPositionControlMode(joint_ids_[i], &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return return_type::ERROR; + } + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Position control"); + if (control_mode_ != ControlMode::Position) { + mode_changed_ = true; + control_mode_ = ControlMode::Position; + } + + if (torque_enabled) { + enable_torque(true); + } + return return_type::OK; + } + + if (control_mode_ != ControlMode::Velocity && + control_mode_ != ControlMode::Position) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), + "Only position/velocity control are implemented"); + return return_type::ERROR; + } + + return return_type::OK; +} + +return_type DynamixelHardware::reset_command() { + for (uint i = 0; i < joints_.size(); i++) { + joints_[i].command.position = joints_[i].state.position; + joints_[i].command.velocity = 0.0; + joints_[i].command.effort = 0.0; + joints_[i].prev_command.position = joints_[i].command.position; + joints_[i].prev_command.velocity = joints_[i].command.velocity; + joints_[i].prev_command.effort = joints_[i].command.effort; + } + + return return_type::OK; +} + +CallbackReturn DynamixelHardware::set_joint_positions() { + const char *log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + for (uint i = 0; i < ids.size(); i++) { + joints_[i].prev_command.position = joints_[i].command.position; + commands[i] = dynamixel_workbench_.convertRadian2Value( + ids[i], static_cast(joints_[i].command.position)); + } + if (!dynamixel_workbench_.syncWrite(kGoalPositionIndex, ids.data(), + ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_velocities() { + const char *log = nullptr; + std::vector commands(info_.joints.size(), 0); + std::vector ids(info_.joints.size(), 0); + + std::copy(joint_ids_.begin(), joint_ids_.end(), ids.begin()); + for (uint i = 0; i < ids.size(); i++) { + joints_[i].prev_command.velocity = joints_[i].command.velocity; + commands[i] = dynamixel_workbench_.convertVelocity2Value( + ids[i], static_cast(joints_[i].command.velocity)); + } + if (!dynamixel_workbench_.syncWrite(kGoalVelocityIndex, ids.data(), + ids.size(), commands.data(), 1, &log)) { + RCLCPP_ERROR(rclcpp::get_logger(kDynamixelHardware), "%s", log); + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn DynamixelHardware::set_joint_params() { + const char *log = nullptr; + for (uint i = 0; i < info_.joints.size(); ++i) { + for (auto paramName : kExtraJointParameters) { + if (info_.joints[i].parameters.find(paramName) != + info_.joints[i].parameters.end()) { + auto value = std::stoi(info_.joints[i].parameters.at(paramName)); + if (!dynamixel_workbench_.itemWrite(joint_ids_[i], paramName, value, + &log)) { + RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), + "%s set to %d for joint %d", paramName, value, i); + } + } + } + return CallbackReturn::SUCCESS; +} + +} // namespace dynamixel_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(dynamixel_hardware::DynamixelHardware, + hardware_interface::SystemInterface) diff --git a/ada_hardware/test/test_dynamixel.cpp b/ada_hardware/test/test_dynamixel.cpp new file mode 100644 index 0000000..b3b8f6a --- /dev/null +++ b/ada_hardware/test/test_dynamixel.cpp @@ -0,0 +1,102 @@ +// Copyright 2024 Personal Robotics Lab, University of Washington +// +// 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. +// Author: Jose Jaime + +#include "hardware_interface/resource_manager.hpp" +#include "ros2_control_test_assets/descriptions.hpp" +#include +#include + +namespace { +const auto TIME = rclcpp::Time(0); +const auto PERIOD = rclcpp::Duration::from_seconds(0.01); +} // namespace + +class TestDynamixelHardware : public ::testing::Test { +protected: + void SetUp() override { + hardware_system_articulable_fork_ = + R"( + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + 1 + + + + + + + + 2 + + + + + + + +)"; + } + + std::string hardware_system_articulable_fork_; +}; + +// Forward declaration +namespace hardware_interface { +class ResourceStorage; +} + +class TestableResourceManager : public hardware_interface::ResourceManager { +public: + friend TestDynamixelHardware; + + TestableResourceManager() : hardware_interface::ResourceManager() {} + + TestableResourceManager(const std::string &urdf, + bool validate_interfaces = true, + bool activate_all = false) + : hardware_interface::ResourceManager(urdf, validate_interfaces, + activate_all) {} +}; + +TEST_F(TestDynamixelHardware, load_articulable_fork) { + auto urdf = ros2_control_test_assets::urdf_head + + hardware_system_articulable_fork_ + + ros2_control_test_assets::urdf_tail; + try { + TestableResourceManager rm(urdf); + SUCCEED(); + } catch (std::exception const &err) { + FAIL() << err.what(); + } +} diff --git a/ada_hardware/urdf/ada_hardware.ros2_control.xacro b/ada_hardware/urdf/ada_hardware.ros2_control.xacro index 9b08d1a..09cc970 100644 --- a/ada_hardware/urdf/ada_hardware.ros2_control.xacro +++ b/ada_hardware/urdf/ada_hardware.ros2_control.xacro @@ -107,4 +107,43 @@ + + + + + mock_components/GenericSystem + false + 0.0 + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + + + 1 + + + + + + + + + + 2 + + + + + + + + + + + + diff --git a/ada_hardware/urdf/ada_hardware.xacro b/ada_hardware/urdf/ada_hardware.xacro index 3f79482..62ac5d5 100644 --- a/ada_hardware/urdf/ada_hardware.xacro +++ b/ada_hardware/urdf/ada_hardware.xacro @@ -7,14 +7,25 @@ + + + + + + + + diff --git a/ada_moveit/config/ada.ros2_control.xacro b/ada_moveit/config/ada.ros2_control.xacro index 5a6282a..99b0d30 100644 --- a/ada_moveit/config/ada.ros2_control.xacro +++ b/ada_moveit/config/ada.ros2_control.xacro @@ -133,4 +133,53 @@ + + + + + + + + mock_components/GenericSystem + false + 0.0 + + + dynamixel_hardware/DynamixelHardware + /dev/ttyUSB0 + 1000000 + + + + + 1 + + + + ${initial_positions['af_joint_1']} + + + 0.0 + + + 0.0 + + + + 2 + + + + ${initial_positions['af_joint_2']} + + + 0.0 + + + 0.0 + + + + + diff --git a/ada_moveit/config/ada.srdf b/ada_moveit/config/ada.srdf index f5a2cb8..ff09e0b 100644 --- a/ada_moveit/config/ada.srdf +++ b/ada_moveit/config/ada.srdf @@ -3,14 +3,17 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + + + + - + @@ -21,6 +24,14 @@ + + + + + + + + @@ -50,6 +61,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -266,4 +307,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ada_moveit/config/ada.urdf.xacro b/ada_moveit/config/ada.urdf.xacro index fa76185..3a324ef 100644 --- a/ada_moveit/config/ada.urdf.xacro +++ b/ada_moveit/config/ada.urdf.xacro @@ -2,10 +2,16 @@ + + + + + + @@ -14,4 +20,10 @@ initial_positions_file="$(arg initial_positions_file)" sim="$(arg sim)" /> + + + + diff --git a/ada_moveit/config/initial_positions.yaml b/ada_moveit/config/initial_positions.yaml index fda99f6..3ac1cdf 100644 --- a/ada_moveit/config/initial_positions.yaml +++ b/ada_moveit/config/initial_positions.yaml @@ -23,3 +23,5 @@ initial_positions: j2n6s200_joint_6: 1.32575 j2n6s200_joint_finger_1: 1.317 j2n6s200_joint_finger_2: 1.317 + af_joint_1: 0.0 + af_joint_2: 0.0 diff --git a/ada_moveit/config/joint_limits.yaml b/ada_moveit/config/joint_limits.yaml index 0e2f1dc..03feab8 100644 --- a/ada_moveit/config/joint_limits.yaml +++ b/ada_moveit/config/joint_limits.yaml @@ -51,3 +51,13 @@ joint_limits: max_velocity: 1.0 has_acceleration_limits: false max_acceleration: 0 + af_joint_1: + has_velocity_limits: true + max_velocity: 0.83775804095727813 + has_acceleration_limits: false + max_acceleration: 0 + af_joint_2: + has_velocity_limits: true + max_velocity: 0.83775804095727813 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/ada_moveit/config/kinematics.yaml b/ada_moveit/config/kinematics.yaml index a556c84..22dcbc4 100644 --- a/ada_moveit/config/kinematics.yaml +++ b/ada_moveit/config/kinematics.yaml @@ -15,3 +15,31 @@ jaco_arm: cost_threshold: 0.001 minimal_displacement_weight: 0.0 gd_step_size: 0.0001 + +ada: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 + +articulable_fork: + kinematics_solver: pick_ik/PickIkPlugin + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + mode: global + stop_optimization_on_valid_solution: true + position_scale: 1.0 + rotation_scale: 0.5 + position_threshold: 0.001 + orientation_threshold: 0.01 + cost_threshold: 0.001 + minimal_displacement_weight: 0.0 + gd_step_size: 0.0001 diff --git a/ada_moveit/config/mock_controllers.yaml b/ada_moveit/config/mock_controllers.yaml index 8963902..70e4c1d 100644 --- a/ada_moveit/config/mock_controllers.yaml +++ b/ada_moveit/config/mock_controllers.yaml @@ -21,12 +21,21 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + af_controller: + type: force_gate_controller/ForceGateController + + + af_servo_controller: + type: force_gate_controller/ForceGatePositionController + jaco_arm_cartesian_controller: ros__parameters: - end_effector_link: "forkTip" + end_effector_link: "j2n6s200_end_effector" robot_base_link: "j2n6s200_link_base" ik_solver: "selectively_damped_least_squares" joints: @@ -85,6 +94,32 @@ jaco_arm_servo_controller: - j2n6s200_joint_5 - j2n6s200_joint_6 +af_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: false + open_loop_control: false + stopped_velocity_tolerance: 0.01 + # No gains on position interface + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + +af_servo_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/mock_servo.yaml b/ada_moveit/config/mock_servo.yaml index e833f90..9f007b1 100644 --- a/ada_moveit/config/mock_servo.yaml +++ b/ada_moveit/config/mock_servo.yaml @@ -47,10 +47,11 @@ servo_node: ## MoveIt properties move_group_name: jaco_arm # Often 'manipulator' or 'arm' + planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + ee_frame_name: j2n6s200_end_effector # The name of the end effector link, used to return the EE pose robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/ada_moveit/config/moveit_controllers.yaml b/ada_moveit/config/moveit_controllers.yaml index 168592f..d7a8e71 100644 --- a/ada_moveit/config/moveit_controllers.yaml +++ b/ada_moveit/config/moveit_controllers.yaml @@ -10,6 +10,8 @@ moveit_simple_controller_manager: - jaco_arm_controller - jaco_arm_cartesian_controller - jaco_arm_servo_controller + - af_controller + - af_servo_controller - hand_controller jaco_arm_controller: @@ -47,6 +49,23 @@ moveit_simple_controller_manager: - j2n6s200_joint_4 - j2n6s200_joint_5 - j2n6s200_joint_6 + + af_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - af_joint_1 + - af_joint_2 + + af_servo_controller: + type: "" + action_ns: commands + default: false + joints: + - af_joint_1 + - af_joint_2 + hand_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory diff --git a/ada_moveit/config/ompl_planning.yaml b/ada_moveit/config/ompl_planning.yaml index 7072746..941c971 100644 --- a/ada_moveit/config/ompl_planning.yaml +++ b/ada_moveit/config/ompl_planning.yaml @@ -64,6 +64,21 @@ planner_configs: max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 PRMstarkConfigDefault: type: geometric::PRMstar +ada: + enforce_constrained_state_space: true + projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault jaco_arm: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) @@ -79,6 +94,21 @@ jaco_arm: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault +articulable_fork: + enforce_constrained_state_space: true + projection_evaluator: joints(af_joint_1,af_joint_2) + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault hand: enforce_constrained_state_space: true projection_evaluator: joints(j2n6s200_joint_1,j2n6s200_joint_2) diff --git a/ada_moveit/config/real_controllers.yaml b/ada_moveit/config/real_controllers.yaml index cdf9edf..f74bbc3 100644 --- a/ada_moveit/config/real_controllers.yaml +++ b/ada_moveit/config/real_controllers.yaml @@ -21,12 +21,21 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + jaco_arm_cartesian_controller: type: cartesian_twist_controller/CartesianTwistController + + af_controller: + type: force_gate_controller/ForceGateController + + + af_servo_controller: + type: force_gate_controller/ForceGatePositionController + jaco_arm_cartesian_controller: ros__parameters: - end_effector_link: "forkTip" + end_effector_link: "j2n6s200_end_effector" robot_base_link: "j2n6s200_link_base" ik_solver: "selectively_damped_least_squares" joints: @@ -101,6 +110,38 @@ jaco_arm_servo_controller: topic: /wireless_ft/ftSensor3 fMag: 1.0 +af_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: false + open_loop_control: false + stopped_velocity_tolerance: 0.01 + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 + # No gains on position interface + af_joint_1: + goal: 0.02 + trajectory: 0.05 + af_joint_2: + goal: 0.02 + trajectory: 0.05 + +af_servo_controller: + ros__parameters: + joints: + - af_joint_1 + - af_joint_2 + wrench_threshold: + topic: /wireless_ft/ftSensor3 + fMag: 1.0 + hand_controller: ros__parameters: joints: diff --git a/ada_moveit/config/real_servo.yaml b/ada_moveit/config/real_servo.yaml index 04e327e..8b306ab 100644 --- a/ada_moveit/config/real_servo.yaml +++ b/ada_moveit/config/real_servo.yaml @@ -50,7 +50,7 @@ servo_node: planning_frame: j2n6s200_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames - ee_frame_name: forkTip # The name of the end effector link, used to return the EE pose + ee_frame_name: j2n6s200_end_effector # The name of the end effector link, used to return the EE pose robot_link_command_frame: j2n6s200_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 4dbf678..281c3d9 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -23,6 +23,7 @@ from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare from srdfdom.srdf import SRDF @@ -33,11 +34,6 @@ def generate_launch_description(): - # MoveIt Config - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() - # Calibration Launch Argument calib_da = DeclareLaunchArgument( "calib", @@ -54,6 +50,15 @@ def generate_launch_description(): ) sim = LaunchConfiguration("sim") + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + # Use Octomap Launch Argument use_octomap_da = DeclareLaunchArgument( "use_octomap", @@ -90,11 +95,22 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(calib_da) ld.add_action(sim_da) + ld.add_action(eet_da) ld.add_action(use_octomap_da) ld.add_action(ctrl_da) ld.add_action(servo_da) ld.add_action(log_level_da) + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + builder = builder.robot_description_semantic( + mappings={"end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + # Launch argument for whether to use moveit servo or not ld.add_action(DeclareBooleanLaunchArg("use_servo", default_value=False)) @@ -156,7 +172,9 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource(str(virtual_joints_launch)), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -168,7 +186,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/rsp.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -183,6 +203,7 @@ def generate_launch_description(): "sim": sim, "use_octomap": use_octomap, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) @@ -194,7 +215,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/moveit_rviz.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -207,29 +230,14 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/warehouse_db.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), condition=IfCondition(LaunchConfiguration("db")), ) ) - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - str(moveit_config.package_path / "config/ada.urdf.xacro") - ), - " ", - "sim:=", - sim, - ] - ) - robot_description = { - "robot_description": ParameterValue(robot_description_content, value_type=str) - } - # Launch MoveIt Servo servo_config = PathJoinSubstitution( [str(moveit_config.package_path), "config", servo_file] @@ -241,7 +249,7 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_config, - robot_description, + moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, # If set, use IK instead of the inverse jacobian ], @@ -260,7 +268,7 @@ def generate_launch_description(): Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, robot_controllers], + parameters=[moveit_config.robot_description, robot_controllers], arguments=["--ros-args", "--log-level", log_level], ) ) @@ -271,7 +279,9 @@ def generate_launch_description(): str(moveit_config.package_path / "launch/spawn_controllers.launch.py") ), launch_arguments={ + "sim": sim, "log_level": log_level, + "end_effector_tool": end_effector_tool, }.items(), ) ) diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index e687c63..11fe135 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -21,11 +21,17 @@ def get_move_group_launch(context): sim = LaunchConfiguration("sim").perform(context) use_octomap = LaunchConfiguration("use_octomap").perform(context) log_level = LaunchConfiguration("log_level").perform(context) + end_effector_tool = LaunchConfiguration("end_effector_tool").perform(context) # Get MoveIt Configs - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + builder = builder.robot_description_semantic( + mappings={"end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() # If sim is mock, set moveit_config.sensors_3d to an empty dictionary if sim == "mock" or use_octomap == "false": @@ -63,10 +69,17 @@ def generate_launch_description(): default_value="info", description="Logging level (debug, info, warn, error, fatal)", ) + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) ld = LaunchDescription() ld.add_action(sim_da) ld.add_action(octomap_da) ld.add_action(log_level_da) + ld.add_action(eet_da) ld.add_action(OpaqueFunction(function=get_move_group_launch)) return ld diff --git a/ada_moveit/launch/moveit_rviz.launch.py b/ada_moveit/launch/moveit_rviz.launch.py index a509c56..ec43525 100644 --- a/ada_moveit/launch/moveit_rviz.launch.py +++ b/ada_moveit/launch/moveit_rviz.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +32,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_moveit_rviz_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/rsp.launch.py b/ada_moveit/launch/rsp.launch.py index 36f3675..521535e 100644 --- a/ada_moveit/launch/rsp.launch.py +++ b/ada_moveit/launch/rsp.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +32,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_rsp_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/spawn_controllers.launch.py b/ada_moveit/launch/spawn_controllers.launch.py index cae35b7..15769ae 100644 --- a/ada_moveit/launch/spawn_controllers.launch.py +++ b/ada_moveit/launch/spawn_controllers.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +32,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_spawn_controllers_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/static_virtual_joint_tfs.launch.py b/ada_moveit/launch/static_virtual_joint_tfs.launch.py index ffec852..b005c7a 100644 --- a/ada_moveit/launch/static_virtual_joint_tfs.launch.py +++ b/ada_moveit/launch/static_virtual_joint_tfs.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +32,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_static_virtual_joint_tfs_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node): diff --git a/ada_moveit/launch/warehouse_db.launch.py b/ada_moveit/launch/warehouse_db.launch.py index 1d3dde3..90d89e7 100644 --- a/ada_moveit/launch/warehouse_db.launch.py +++ b/ada_moveit/launch/warehouse_db.launch.py @@ -12,6 +12,16 @@ def generate_launch_description(): ld = LaunchDescription() + + # Sim Launch Argument + sim_da = DeclareLaunchArgument( + "sim", + default_value="real", + description="Which sim to use: 'mock', 'isaac', or 'real'", + ) + sim = LaunchConfiguration("sim") + ld.add_action(sim_da) + # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -22,9 +32,23 @@ def generate_launch_description(): log_level_cmd_line_args = ["--ros-args", "--log-level", log_level] ld.add_action(log_level_da) - moveit_config = MoveItConfigsBuilder( - "ada", package_name="ada_moveit" - ).to_moveit_configs() + # End-effector Tool Launch Argument + eet_da = DeclareLaunchArgument( + "end_effector_tool", + default_value="fork", + description="The end-effector tool being used: 'none', 'fork', 'articulable_fork'", + choices=["none", "fork", "articulable_fork"], + ) + end_effector_tool = LaunchConfiguration("end_effector_tool") + ld.add_action(eet_da) + + # Get MoveIt Configs + builder = MoveItConfigsBuilder("ada", package_name="ada_moveit") + builder = builder.robot_description( + mappings={"sim": sim, "end_effector_tool": end_effector_tool} + ) + moveit_config = builder.to_moveit_configs() + entities = generate_warehouse_db_launch(moveit_config).entities for entity in entities: if isinstance(entity, Node):