AX12 driver using ros2-control (#240)

* init

* add dynamixel_hardware to readme

* docker dialout permissions

* fix mep3 repos

* colcon ignore

* add example

* change branch name for dynamixel_hardware

* better naming

* add velocity control

* add velocity control

* add tolerance

* fix

* add bt example

* clean

* deg to rad

* debug

* dynamixel

---------

Co-authored-by: Filip Parag <filip@parag.rs>
Co-authored-by: Darko Lukic <lukicdarkoo@gmail.com>

README.md

@@ -36,10 +36,6 @@ vcs import src < src/mep3/mep3.repos
 rosdep update
 rosdep install --from-paths src --ignore-src -r
 
-# Create udev rules so rplidar and dynamixel usb ports static names
-sudo cp src/mep3/tools/rplidar.rules /etc/udev/rules.d/
-sudo cp src/mep3/tools/dynamixel.rules /etc/udev/rules.d/
-
 # Build the packages
 colcon build --symlink-install --packages-up-to mep3_bringup mep3_simulation --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
 

docker/Dockerfile.base

@@ -7,6 +7,7 @@ ARG UID=1000
 RUN useradd -d /memristor -m \
             -u $UID -U \
             -s /usr/bin/bash \
+            -G dialout \
             -c "Memristor Robotics" memristor && \
     echo "memristor ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers
 

docker/Makefile

@@ -32,8 +32,8 @@ run:
 		-v ~/.Xauthority:/memristor/.host/.Xauthority:ro \
 		-v /tmp/.X11-unix/:/tmp/.X11-unix:rw \
 		-v /dev/dri:/dev/dri:ro \
-		-v /dev/bus/usb:/dev/bus/usb:ro \
-		-v ${ROOT_DIR}/../..:/memristor/ros2_ws/src:rw \
+		-v /dev/serial:/dev/serial:rw \
+		-v ${ROOT_DIR}/../../..:/memristor/ros2_ws:rw \
 		-d -it ${IMAGE}
 
 exec:

docker/config/setup.sh

@@ -28,8 +28,12 @@ if dialog --title 'mep3 config' --yesno 'Run first time ROS setup' 5 30; then
     sudo -E rosdep init
     sudo -E apt-get install -y python3-vcstool
     cd /memristor/ros2_ws && vcs import --recursive src < /memristor/ros2_ws/src/mep3/mep3.repos
-	  rosdep --rosdistro "${ROS_DISTRO}" update
-	  cd /memristor/ros2_ws && yes | rosdep --rosdistro "${ROS_DISTRO}" install -r --from-paths src --ignore-src
+    rosdep --rosdistro "${ROS_DISTRO}" update
+    cd /memristor/ros2_ws && yes | rosdep --rosdistro "${ROS_DISTRO}" install -r --from-paths src --ignore-src
+    for pkg in 'dynamixel_hardware/open_manipulator_x_description' \
+                'dynamixel_hardware/pantilt_bot_description'; do
+        touch "/memristor/ros2_ws/src/$pkg/COLCON_IGNORE"
+    done
 fi
 
 if dialog --title 'mep3 config' --defaultno --yesno 'Enable enhanced shell prompt' 5 38; then

mep3.repos

@@ -3,3 +3,7 @@ repositories:
     type: git
     url: /~https://github.com/BehaviorTree/BehaviorTree.CPP.git
     version: dfc1a1eaa2af7efa4bd0b974c1960e89651eb9f9
+  dynamixel_hardware:
+    type: git
+    url: /~https://github.com/memristor/dynamixel_hardware.git
+    version: ax12_description

mep3_behavior/README.md

@@ -0,0 +1,8 @@
+# mep3_behavior
+
+Uses behavior trees to build strategies.
+
+One can run a BT tree without namespace as:
+```bash
+ros2 run mep3_behavior mep3_behavior --ros-args -p strategy:=test_servo
+```

mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp

@@ -59,10 +59,10 @@ namespace mep3_behavior
 
     bool setGoal(Goal &goal) override
     {
-      goal.position = position_;
-      goal.max_velocity = max_velocity_;
-      goal.max_acceleration = max_acceleration_;
-      goal.tolerance = tolerance_;
+      goal.position = position_ * M_PI / 180;
+      goal.max_velocity = max_velocity_ * M_PI / 180;
+      goal.max_acceleration = max_acceleration_ * M_PI / 180;
+      goal.tolerance = tolerance_ * M_PI / 180;
       goal.timeout = timeout_;
       return true;
     }

mep3_behavior/strategies/big/demo.xml

@@ -1,14 +1,13 @@
-<root main_tree_to_execute="BehaviorTree" BTCPP_format="4">
+<root main_tree_to_execute="BigDemo" BTCPP_format="4">
     <!-- ////////// -->
-    <include path="../tree_nodes_model.xml" />
-    <BehaviorTree ID="BehaviorTree">
+    <BehaviorTree ID="BigDemo">
         <Sequence>
             <WaitMatchStart state="1" />
 
             <AddObstacle label="initial" polygon="0.0;0.0|0.2;0.0|0.0;0.2" />
             <AddObstacle label="initial" polygon="0.0;0.0|-0.2;0.0|0.0;-0.2" />
             <!-- <Navigate goal="0.8;0.2;0" /> -->
-            <!-- <RemoveObstacle label="initial" /> -->
+            <RemoveObstacle label="initial" />
             <JointPosition instance="arm_joint" position="-1.57" />
 
             <Wait duration="100000.0" name="Wait one big blue main strategy" />

mep3_behavior/strategies/project.btproj

@@ -0,0 +1,37 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="big/demo.xml"/>
+    <include path="small/demo.xml"/>
+    <include path="test_servo.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="AddObstacle" editable="true">
+            <input_port name="label"/>
+            <input_port name="polygon"/>
+        </Action>
+        <Action ID="JointPosition" editable="true">
+            <input_port name="instance"/>
+            <input_port name="max_velocity" default="1"/>
+            <input_port name="position"/>
+            <input_port name="tolerance" default="0.3"/>
+        </Action>
+        <Action ID="Navigate" editable="false">
+            <input_port name="behavior_tree"/>
+            <input_port name="goal"/>
+        </Action>
+        <Action ID="RemoveObstacle" editable="true">
+            <input_port name="label"/>
+        </Action>
+        <Action ID="ScoreboardTask" editable="false">
+            <input_port name="points" default="0">points scored, can be negative</input_port>
+            <input_port name="task" default="store_sample_to_work_shed">unique task name</input_port>
+        </Action>
+        <Control ID="TaskSequenceControl" editable="false"/>
+        <Action ID="Wait" editable="true">
+            <input_port name="duration"/>
+        </Action>
+        <Action ID="WaitMatchStart" editable="false">
+            <input_port name="state" default="2">0 = unarmed; 1 = armed; 2 = started</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mep3_behavior/strategies/small/demo.xml

@@ -1,14 +1,13 @@
-<root main_tree_to_execute="BehaviorTree" BTCPP_format="4">
+<root main_tree_to_execute="SmallDemo" BTCPP_format="4">
     <!-- ////////// -->
-    <include path="../tree_nodes_model.xml" />
-    <BehaviorTree ID="BehaviorTree">
+    <BehaviorTree ID="SmallDemo">
         <Sequence>
             <WaitMatchStart state="1" />
 
             <AddObstacle label="initial" polygon="0.0;0.0|0.2;0.0|0.0;0.2" />
             <AddObstacle label="initial" polygon="0.0;0.0|-0.2;0.0|0.0;-0.2" />
             <!-- <Navigate goal="-0.1;-0.1;0" /> -->
-            <!-- <RemoveObstacle label="initial" /> -->
+            <RemoveObstacle label="initial" />
 
             <Wait duration="100000.0" name="Wait one big blue main strategy" />
         </Sequence>

mep3_behavior/strategies/test_servo.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="TestServo">
+    <Sequence>
+      <JointPosition instance="joint_cherry_gripper"
+                     max_velocity="200"
+                     position="165"
+                     tolerance="10"/>
+    </Sequence>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="JointPosition"
+            editable="true">
+      <input_port name="instance"/>
+      <input_port name="max_velocity"
+                  default="1"/>
+      <input_port name="position"/>
+      <input_port name="tolerance"
+                  default="0.3"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

mep3_controllers/include/mep3_controllers/joint_position_controller.hpp

@@ -16,13 +16,17 @@ namespace mep3_controllers
     struct Joint
     {
         Joint(){};
-        std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> position_handle;
+        std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> position_command_handle;
+        std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> velocity_command_handle;
+        std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> position_handle;
         std::string name;
 
         // TODO (darko -> lis): I think SimpleActionServer is not suitable here and that the regular server would perform much better.
         std::shared_ptr<nav2_util::SimpleActionServer<mep3_msgs::action::JointPositionCommand>> action_server;
 
         double target_position;
+        double max_velocity;
+        double tolerance;
         bool active;
     };
 

mep3_controllers/src/joint_position_controller.cpp

@@ -10,10 +10,26 @@ namespace mep3_controllers
 
     void JointPositionController::on_action_called(std::shared_ptr<Joint> joint)
     {
-        RCLCPP_WARN(get_node()->get_logger(), "Action execute!");
         auto goal = joint->action_server->get_current_goal();
+        RCLCPP_INFO(
+            get_node()->get_logger(),
+            "Motor %s action called to position %lf (velocity %lf, tolerance %lf)",
+            joint->name.c_str(),
+            goal->position,
+            goal->max_velocity,
+            goal->tolerance);
+
+        double max_velocity = 1.0;
+        if (goal->max_velocity != 0)
+            max_velocity = goal->max_velocity;
+
+        double tolerance = 99999;
+        if (goal->tolerance != 0)
+            tolerance = goal->tolerance;
 
         joint->target_position = goal->position;
+        joint->max_velocity = max_velocity;
+        joint->tolerance = tolerance;
         joint->active = true;
 
         while (rclcpp::ok() && joint->active)
@@ -73,6 +89,7 @@ namespace mep3_controllers
         for (std::shared_ptr<Joint> joint : joints_)
         {
             command_interfaces_config.names.push_back(joint->name + "/position");
+            command_interfaces_config.names.push_back(joint->name + "/velocity");
         }
 
         return command_interfaces_config;
@@ -81,6 +98,11 @@ namespace mep3_controllers
     controller_interface::InterfaceConfiguration JointPositionController::state_interface_configuration() const
     {
         controller_interface::InterfaceConfiguration state_interfaces_config;
+
+        for (std::shared_ptr<Joint> joint : joints_)
+        {
+            state_interfaces_config.names.push_back(joint->name + "/position");
+        }
         return state_interfaces_config;
     }
 
@@ -92,14 +114,29 @@ namespace mep3_controllers
         {
             if (joint->active)
             {
-                RCLCPP_WARN(get_node()->get_logger(), "%s is moving to %lf", joint->name.c_str(), joint->target_position);
-                joint->position_handle->get().set_value(joint->target_position);
+                joint->position_command_handle->get().set_value(joint->target_position);
+                joint->velocity_command_handle->get().set_value(joint->max_velocity);
 
                 // Return the result
-                auto result = std::make_shared<mep3_msgs::action::JointPositionCommand::Result>();
-                result->set__result(0);
-                joint->action_server->succeeded_current(result);
-                joint->active = false;
+                if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance)
+                {
+                    auto result = std::make_shared<mep3_msgs::action::JointPositionCommand::Result>();
+                    result->set__result(0);
+                    joint->action_server->succeeded_current(result);
+                    joint->active = false;
+                }
+                else if (joint->action_server->is_cancel_requested())
+                {
+                    auto result = std::make_shared<mep3_msgs::action::JointPositionCommand::Result>();
+                    result->set__result(0);
+                    joint->active = false;
+                }
+                else if (joint->action_server->is_preempt_requested())
+                {
+                    auto result = std::make_shared<mep3_msgs::action::JointPositionCommand::Result>();
+                    result->set__result(1);
+                    joint->active = false;
+                }
             }
         }
 
@@ -110,6 +147,7 @@ namespace mep3_controllers
     {
         for (std::shared_ptr<Joint> joint : joints_)
         {
+            // Position command
             const auto position_command_handle = std::find_if(
                 command_interfaces_.begin(), command_interfaces_.end(),
                 [&joint](const auto &interface)
@@ -122,7 +160,37 @@ namespace mep3_controllers
                 return controller_interface::CallbackReturn::FAILURE;
                 RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
             }
-            joint->position_handle = std::ref(*position_command_handle);
+            joint->position_command_handle = std::ref(*position_command_handle);
+
+            // Velocity command
+            const auto velocity_command_handle = std::find_if(
+                command_interfaces_.begin(), command_interfaces_.end(),
+                [&joint](const auto &interface)
+                {
+                    return interface.get_prefix_name() == joint->name &&
+                           interface.get_interface_name() == hardware_interface::HW_IF_VELOCITY;
+                });
+            if (velocity_command_handle == command_interfaces_.end())
+            {
+                return controller_interface::CallbackReturn::FAILURE;
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
+            }
+            joint->velocity_command_handle = std::ref(*velocity_command_handle);
+
+            // Position state
+            const auto position_handle = std::find_if(
+                state_interfaces_.begin(), state_interfaces_.end(),
+                [&joint](const auto &interface)
+                {
+                    return interface.get_prefix_name() == joint->name &&
+                           interface.get_interface_name() == hardware_interface::HW_IF_POSITION;
+                });
+            if (position_handle == state_interfaces_.end())
+            {
+                return controller_interface::CallbackReturn::FAILURE;
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str());
+            }
+            joint->position_handle = std::ref(*position_handle);
         }
         return controller_interface::CallbackReturn::SUCCESS;
     }