Configure ada_hardware for Articulable Fork

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()

ada_hardware/ada_hardware.xml

@@ -19,4 +19,12 @@
       Interface for controlling a 6DOF arm (+ 2 fingers) in IsaacSim.
     </description>
   </class>
+
+  <class name="dynamixel_hardware/DynamixelHardware"
+         type="dynamixel_hardware::DynamixelHardware"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+      The ROBOTIS Dynamixel system interface.
+    </description>
+  </class>
 </library>

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

ada_hardware/include/ada_hardware/dynamixel_hardware.hpp

@@ -0,0 +1,112 @@
+// Copyright 2020 Yutaka Kondo <[email protected]>
+//
+// 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 <dynamixel_workbench_toolbox/dynamixel_workbench.h>
+
+#include <map>
+#include <vector>
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+
+#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<hardware_interface::StateInterface>
+  export_state_interfaces() override;
+
+  DYNAMIXEL_HARDWARE_PUBLIC
+  std::vector<hardware_interface::CommandInterface>
+  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<const char *const, const ControlItem *> control_items_;
+  std::vector<Joint> joints_;
+  std::vector<uint8_t> 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_

ada_hardware/include/ada_hardware/visiblity_control.h

@@ -0,0 +1,56 @@
+// Copyright 2020 Yutaka Kondo <[email protected]>
+//
+// 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_

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="fork",
+            description="The end-effector tool being used:",
+            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 = {

ada_hardware/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ada_hardware</name>
-  <version>0.0.1</version>
+  <version>0.0.2</version>
   <description>ROS Hardware Interface for ADA</description>
   <maintainer email="[email protected]">Ethan K. Gordon</maintainer>
   <license>BSD-3-Clause</license>
@@ -12,8 +12,10 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>pluginlib</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>sensor_msgs</depend>
+  <depend>dynamixel_workbench_toolbox</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>