Add dynamixel set position client behavior

src/custom_behaviors/dynamixel_control_msgs/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.22)
+project(dynamixel_control_msgs)
+
+# Default to C++20
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+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)
+find_package(builtin_interfaces REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SetPosition.srv"
+  "srv/SetVelocity.srv"
+  "action/SetVelocity.action"
+  DEPENDENCIES builtin_interfaces
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

src/custom_behaviors/dynamixel_control_msgs/action/SetVelocity.action

@@ -0,0 +1,30 @@
+# Goal
+# Currently supports 'XL430-W250-T' or 'MX-12W'.
+string motor_type
+
+# Normalized velocity value in the range [-1.0, 1.0].
+float32 normalized_velocity
+
+# Absolute normalized load threshold value in the range [0.0, 1.0].
+float32 abs_normalized_load_threshold
+
+# Will monitor the load and succeed when the load threshold is exceeded if true.
+bool monitor_load
+
+---
+# Result
+bool success
+---
+
+# Feedback
+uint8 temperature
+uint16 load
+float32 normalized_load
+
+# XL motor series
+uint32 xl_position
+uint32 xl_velocity
+
+# MX motor series
+uint16 mx_position
+uint16 mx_velocity

src/custom_behaviors/dynamixel_control_msgs/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dynamixel_control_msgs</name>
+  <version>1.0.0</version>
+  <description>ROS2 interfaces for DYNAMIXEL motor control code</description>
+  <maintainer email="[email protected]">David Yackzan</maintainer>
+
+  <license>Proprietary</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <build_depend>builtin_interfaces</build_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/custom_behaviors/dynamixel_control_msgs/srv/SetPosition.srv

@@ -0,0 +1,11 @@
+# Request
+#
+# Currently supports 'XL430-W250-T' or 'MX-12W'.
+string motor_type
+
+int32 position
+
+---
+# Response
+string error_msg
+bool success

src/custom_behaviors/dynamixel_control_msgs/srv/SetVelocity.srv

@@ -0,0 +1,7 @@
+# Request
+uint8 id
+int32 velocity
+---
+# Response
+string error_msg
+bool success

src/custom_behaviors/set_dynamixel_position/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.22)
+project(set_dynamixel_position CXX)
+
+find_package(moveit_studio_common REQUIRED)
+moveit_studio_package()
+
+find_package(dynamixel_control_msgs REQUIRED)
+find_package(moveit_studio_behavior_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS dynamixel_control_msgs moveit_studio_behavior_interface pluginlib)
+
+add_library(
+  set_dynamixel_position
+  SHARED
+  src/set_dynamixel_position.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  set_dynamixel_position
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(set_dynamixel_position
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Install Libraries
+install(
+  TARGETS set_dynamixel_position
+  EXPORT set_dynamixel_positionTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+  moveit_studio_behavior_interface set_dynamixel_position_plugin_description.xml)
+
+ament_export_targets(set_dynamixel_positionTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/custom_behaviors/set_dynamixel_position/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    set_dynamixel_position:
+      - "set_dynamixel_position::SetDynamixelPositionBehaviorsLoader"

src/custom_behaviors/set_dynamixel_position/config/tree_nodes_model.xml

@@ -0,0 +1,5 @@
+<root>
+    <TreeNodesModel>
+        <!-- Include additional SubTree metadata in this file. -->
+    </TreeNodesModel>
+</root>

src/custom_behaviors/set_dynamixel_position/include/set_dynamixel_position/set_dynamixel_position.hpp

@@ -0,0 +1,82 @@
+#pragma once
+
+#include <behaviortree_cpp/action_node.h>
+
+#include <moveit_studio_behavior_interface/service_client_behavior_base.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+#include <dynamixel_control_msgs/srv/set_position.hpp>
+
+namespace set_dynamixel_position
+{
+using SetPosition = dynamixel_control_msgs::srv::SetPosition;
+/**
+ * @brief TODO(...)
+ */
+class SetDynamixelPosition : public moveit_studio::behaviors::ServiceClientBehaviorBase<SetPosition>
+{
+public:
+  /**
+   * @brief Constructor for the set_dynamixel_position behavior.
+   * @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when
+   * this Behavior is created within a new behavior tree.
+   * @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in
+   * the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
+   * @param config This contains runtime configuration info for this Behavior, such as the mapping between the
+   * Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this
+   * Behavior is created within a new behavior tree.
+   * @details An important limitation is that the members of the base Behavior class are not instantiated until after
+   * the initialize() function is called, so these classes should not be used within the constructor.
+   */
+  SetDynamixelPosition(const std::string& name, const BT::NodeConfiguration& config,
+                       const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
+  SetDynamixelPosition(const std::string& name, const BT::NodeConfiguration& config,
+                       const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources,
+                       std::unique_ptr<moveit_studio::behaviors::ClientInterfaceBase<SetPosition>> client_interface);
+
+  /**
+   * @brief Implementation of the required providedPorts() function for the set_dynamixel_position Behavior.
+   * @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named
+   * providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function
+   * must return an empty BT::PortsList. This function returns a list of ports with their names and port info, which is
+   * used internally by the behavior tree.
+   * @return set_dynamixel_position does not use expose any ports, so this function returns an empty list.
+   */
+  static BT::PortsList providedPorts();
+
+  /**
+   * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+   * subcategory, in the MoveIt Studio Developer Tool.
+   * @return A BT::KeyValueVector containing the Behavior metadata.
+   */
+  static BT::KeyValueVector metadata();
+
+private:
+  tl::expected<std::string, std::string> getServiceName() override;
+
+  tl::expected<std::chrono::duration<double>, std::string> getResponseTimeout() override;
+
+  /**
+   * @brief Creates a service request message.
+   * @return Returns an instance of SetPosition::Request. Since the request message is empty, this always succeeds.
+   */
+  tl::expected<SetPosition::Request, std::string> createRequest() override;
+
+  /**
+   * @brief Determines if the service request succeeded or failed based on the contents of the response message's success field.
+   * @param response Response message received from the service server.
+   * @return The return value matches the value of response.success.
+   */
+  tl::expected<bool, std::string> processResponse(const SetPosition::Response& response) override;
+
+  /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+  std::shared_future<tl::expected<bool, std::string>>& getFuture() override
+  {
+    return future_;
+  }
+
+  /**
+   * @brief Holds the result of calling the service asynchronously.
+   */
+  std::shared_future<tl::expected<bool, std::string>> future_;
+};
+}  // namespace set_dynamixel_position

src/custom_behaviors/set_dynamixel_position/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>set_dynamixel_position</name>
+  <version>0.0.0</version>
+  <description>Call service to set the position of a dynamixel motor</description>
+
+  <maintainer email="[email protected]">John Doe</maintainer>
+  <author email="[email protected]">John Doe</author>
+
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>dynamixel_control_msgs</build_depend>
+  <build_depend>moveit_studio_common</build_depend>
+
+  <depend>moveit_studio_behavior_interface</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/custom_behaviors/set_dynamixel_position/set_dynamixel_position_plugin_description.xml

@@ -0,0 +1,5 @@
+<library path="set_dynamixel_position">
+  <class type="set_dynamixel_position::SetDynamixelPositionBehaviorsLoader"
+         base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase">
+  </class>
+</library>

src/custom_behaviors/set_dynamixel_position/src/register_behaviors.cpp

@@ -0,0 +1,24 @@
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit_studio_behavior_interface/behavior_context.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
+
+#include <set_dynamixel_position/set_dynamixel_position.hpp>
+
+#include <pluginlib/class_list_macros.hpp>
+
+namespace set_dynamixel_position
+{
+class SetDynamixelPositionBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
+{
+public:
+  void registerBehaviors(
+      BT::BehaviorTreeFactory& factory,
+      [[maybe_unused]] const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
+  {
+    moveit_studio::behaviors::registerBehavior<SetDynamixelPosition>(factory, "SetDynamixelPosition", shared_resources);
+  }
+};
+}  // namespace set_dynamixel_position
+
+PLUGINLIB_EXPORT_CLASS(set_dynamixel_position::SetDynamixelPositionBehaviorsLoader,
+                       moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

src/custom_behaviors/set_dynamixel_position/src/set_dynamixel_position.cpp

@@ -0,0 +1,82 @@
+#include <boost/geometry/util/range.hpp>
+#include <set_dynamixel_position/set_dynamixel_position.hpp>
+
+namespace
+{
+const rclcpp::Logger kLogger = rclcpp::get_logger("SetDynamixelPosition");
+constexpr std::chrono::seconds kServiceResponseTimeoutSeconds{ 3 };
+constexpr auto kPortServiceName = "service_name";
+constexpr auto kPortMotorType = "motor_type";
+constexpr auto kPortPosition = "position";
+}  // namespace
+
+namespace set_dynamixel_position
+{
+SetDynamixelPosition::SetDynamixelPosition(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
+  : moveit_studio::behaviors::ServiceClientBehaviorBase<SetPosition>(name, config, shared_resources)
+{
+}
+SetDynamixelPosition::SetDynamixelPosition(
+    const std::string& name, const BT::NodeConfiguration& config,
+    const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources,
+    std::unique_ptr<moveit_studio::behaviors::ClientInterfaceBase<SetPosition>> client_interface)
+  : moveit_studio::behaviors::ServiceClientBehaviorBase<SetPosition>(name, config, shared_resources,
+                                                                     std::move(client_interface))
+{
+}
+
+BT::PortsList SetDynamixelPosition::providedPorts()
+{
+  return { BT::InputPort<std::string>(kPortServiceName, "/set_dynamixel_position",
+                                      "Name of the service to send a request to."),
+           BT::InputPort<std::string>(kPortMotorType, "XL430-W250-T", "Model of the motor."),
+           BT::InputPort<int>(kPortPosition, 0, "Position value to set the motor to.") };
+}
+
+BT::KeyValueVector SetDynamixelPosition::metadata()
+{
+  return { { "description", "Call service to set the position of a dynamixel motor" } };
+}
+
+tl::expected<std::string, std::string> SetDynamixelPosition::getServiceName()
+{
+  const auto service_name = getInput<std::string>(kPortServiceName);
+
+  if (const auto error = moveit_studio::behaviors::maybe_error(service_name))
+  {
+    return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+  }
+
+  return service_name.value();
+}
+
+tl::expected<std::chrono::duration<double>, std::string> SetDynamixelPosition::getResponseTimeout()
+{
+  return kServiceResponseTimeoutSeconds;
+}
+
+tl::expected<SetPosition::Request, std::string> SetDynamixelPosition::createRequest()
+{
+  const auto motor_type = getInput<std::string>(kPortMotorType);
+  const auto position = getInput<int>(kPortPosition);
+
+  if (const auto error = moveit_studio::behaviors::maybe_error(motor_type, position))
+  {
+    return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
+  }
+
+  // Create request message.
+  SetPosition::Request request;
+  request.motor_type = motor_type.value();
+  request.position = position.value();
+  return request;
+}
+
+tl::expected<bool, std::string> SetDynamixelPosition::processResponse(const SetPosition::Response& response)
+{
+  return response.success;
+}
+
+}  // namespace set_dynamixel_position

src/kinova_gen3_mujoco_config/objectives/set_dynamixel_position.xml

@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Set Dynamixel Position">
+  <!--//////////-->
+  <BehaviorTree ID="Set Dynamixel Position" _description="" _favorite="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="SetDynamixelPosition" motor_type="XL430-W250-T" position="1000"/>
+    </Control>
+  </BehaviorTree>
+</root>