Stuff

CMakeLists.txt

@@ -7,12 +7,41 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+add_library(
+    hexapod_robot
+    SHARED
+    hardware/hexapod_robot_hwi.cpp
+)
+
+target_compile_features(hexapod_robot PUBLIC cxx_std_17)
+target_include_directories(hexapod_robot PUBLIC 
+    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+    $<INSTALL_INTERFACE:include/hexapod_robot>
+)
+
+ament_target_dependencies(
+    hexapod_robot PUBLIC
+    hardware_interface
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+)
+
+pluginlib_export_plugin_description_file(hardware_interface hexapod_robot.xml)
+
 
 install(
-   DIRECTORY   description/urdf
+    DIRECTORY hardware/include/
+    DESTINATION include/hexapod_robot
+)
+
+install(
+   DIRECTORY   description
    DESTINATION share/hexapod_robot
 )
 
@@ -21,6 +50,13 @@ install(
     DESTINATION share/hexapod_robot
 )
 
+install(TARGETS hexapod_robot
+    EXPORT export_hexapod_robot
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -34,4 +70,13 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+ament_export_targets(export_hexapod_robot HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+    hardware_interface 
+    pluginlib
+    rclcpp
+    rclcpp_lifecycle
+)
+
+
 ament_package()

bringup/config/controllers.yaml

@@ -0,0 +1,32 @@
+controller_manager:
+    ros__parameters:
+        update_rate: 10
+        
+        joint_state_broadcaster:
+            type: joint_state_broadcaster/JointStateBroadcaster
+            
+        forward_position_controller:
+            type: forward_command_controller/ForwardCommandController
+            
+forward_position_controller:
+    ros__parameters:
+        joints:
+            - c1_joint
+            - c2_joint
+            - c3_joint
+            - c4_joint
+            - c5_joint
+            - c6_joint
+            - f1_joint
+            - f2_joint
+            - f3_joint
+            - f4_joint
+            - f5_joint
+            - f6_joint
+            - t1_joint
+            - t2_joint
+            - t3_joint
+            - t4_joint
+            - t5_joint
+            - t6_joint
+        interface_name: position

bringup/launch/hexapod_robot.launch.py

@@ -1,17 +1,21 @@
 from launch import LaunchDescription
+from launch.actions import RegisterEventHandler
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.event_handlers import OnProcessExit
+
 
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 def generate_launch_description():
-    
+
     robot_description_content = Command([
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
                 [
                     FindPackageShare("hexapod_robot"),
+                    "description",
                     "urdf",
                     "hexapod_robot.urdf.xacro"
                 ]
@@ -19,21 +23,21 @@ def generate_launch_description():
         ]
     )
     robot_description = {"robot_description": robot_description_content}
-            
-    rviz_node = Node(
-        package="rviz2",
-        executable="rviz2",
-        arguments=['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
-        name="rviz2",
-        output="screen",
+
+    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
+    
+    control_node = Node(
+        package = "controller_manager",
+        executable = "ros2_control_node",
+        parameters = [robot_description, robot_controllers],
+        output="both"
     )
-            
+
     robot_state_publisher_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        parameters=[{
-            'robot_description': robot_description_content
-        }]
+        package = "robot_state_publisher",
+        executable = "robot_state_publisher",
+        parameters = [robot_description],
+        output = "both"
     )            
 
     joint_state_publisher_gui = Node(
@@ -41,11 +45,46 @@ def generate_launch_description():
         executable="joint_state_publisher_gui"
     )
             
+    rviz_node = Node(
+        package = "rviz2",
+        executable = "rviz2",
+        arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
+        name = "rviz2",
+        output = "screen",
+    )
+
+    joint_state_broadcaster_spawner = Node (
+        package = "controller_manager",
+        executable = "spawner",
+        arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
+    )
+    
+    robot_controller_spawner = Node(
+        package = "controller_manager",
+        executable = "spawner",
+        arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
+    )
+    
+    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler = OnProcessExit(
+            target_action = joint_state_broadcaster_spawner,
+            on_exit = [rviz_node]
+        )
+    )
+    
+    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+        event_handler = OnProcessExit(
+            target_action = joint_state_broadcaster_spawner,
+            on_exit = [robot_controller_spawner]
+        )
+    )
+    
     nodes = [
-        rviz_node,
+        control_node,
         robot_state_publisher_node,
-        joint_state_publisher_gui
-        
+        joint_state_broadcaster_spawner,
+        delay_rviz_after_joint_state_broadcaster_spawner,
+        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner        
     ]
             
     return LaunchDescription(nodes)

description/urdf/hexapod_robot.urdf.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
-    <xacro:include filename="$(find hexapod_robot)/urdf/hexapod_robot_materials.xacro"/>
-    <xacro:include filename="$(find hexapod_robot)/urdf/hexapod_robot_macros.xacro"/>
+    <xacro:include filename="$(find hexapod_robot)/description/urdf/hexapod_robot_materials.xacro"/>
+    <xacro:include filename="$(find hexapod_robot)/description/urdf/hexapod_robot_macros.xacro"/>
 
     <link name="base_link">
         <xacro:create_frame_plate z_offset="-0.034"/>
@@ -23,5 +23,6 @@
     <xacro:create_leg number="5" side="r" xy_offset=" 0.060   0.000"/>
     <xacro:create_leg number="6" side="r" xy_offset=" 0.040   0.080"/>
         
+    <xacro:include filename="$(find hexapod_robot)/description/urdf/hexapod_robot_ros2_control.xacro"/>
 
 </robot>

description/urdf/hexapod_robot_macros.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="create_frame_plate" params="z_offset">
         <visual>
             <origin xyz="0 0 ${z_offset}"/>

description/urdf/hexapod_robot_ros2_control.xacro

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="create_leg_joints" params="number">
+
+        <joint name="c${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max">1</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="f${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max">1</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="t${number}_joint">
+            <command_interface name="position">
+                <param name="min">-1</param>
+                <param name="max">1</param>
+            </command_interface>
+            <state_interface name="position"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel">
+        <param name="${name}_joint_i2c_address">${i2c_address}</param>
+        <param name="${name}_joint_channel">${channel}</param>
+    </xacro:macro>
+
+    <ros2_control name="Hexapod" type="system">
+        <hardware>
+            <plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
+            <param name="i2c_device">/dev/i2c-1</param>
+            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13"/>
+            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14"/>
+            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15"/>
+
+            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"/>
+            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10"/>
+            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11"/>
+
+            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"/>
+            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"/>
+            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"/>
+
+            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13"/>
+            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14"/>
+            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15"/>
+
+            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"/>
+            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"/>
+            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"/>
+
+            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"/>
+            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"/>
+            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"/>
+        </hardware>
+        <xacro:create_leg_joints number="1"/>    
+        <xacro:create_leg_joints number="2"/>    
+        <xacro:create_leg_joints number="3"/>    
+        <xacro:create_leg_joints number="4"/>    
+        <xacro:create_leg_joints number="5"/>    
+        <xacro:create_leg_joints number="6"/>    
+    </ros2_control>
+</robot>

hardware/hexapod_robot_hwi.cpp

@@ -0,0 +1,98 @@
+#include "hexapod_robot/hexapod_robot_hwi.hpp"
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include <fcntl.h>
+
+namespace hexapod_robot {
+
+hardware_interface::CallbackReturn 
+HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) {
+    if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
+        return hardware_interface::CallbackReturn::ERROR;
+    }
+    
+    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_init");
+    
+    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+        joint_states_[joint.name]= 0.0;
+        joint_commands_[joint.name] = 0.0;
+
+        std::string i2c_address_parameter = joint.name + "_i2c_address";
+        std::string channel_parameter = joint.name + "_channel";
+
+        PWMOutput pwm_output;
+        pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
+        pwm_output.channel     = stoi(info_.hardware_parameters[channel_parameter], nullptr, 0);
+
+        pca9685_controller_available_[pwm_output.i2c_address] = false;
+        joint_pwm_mapping_[joint.name] = pwm_output;
+
+        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "%s mapped to 0x%02x:%d", joint.name.c_str(), pwm_output.i2c_address, pwm_output.channel);
+    }
+
+    for (const auto& [i2c_address, available]: pca9685_controller_available_) {
+        RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "require PCA9685 at i2c 0x%02x.", i2c_address);
+    }
+
+    i2c_available_ = false;
+    i2c_fd_ = open(info_.hardware_parameters["i2c_device"].c_str(), O_RDWR);
+    if (i2c_fd_ < 0) {
+        RCLCPP_WARN(rclcpp::get_logger("HexapodRobotHardwareInterface"), "Cannot open %s, continuing in dummy mode.", info_.hardware_parameters["i2c_device"].c_str());
+    } else {
+        i2c_available_ = true;
+    }
+
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn
+HexapodRobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &) {
+    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_activate");
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn
+HexapodRobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) {
+    RCLCPP_INFO(rclcpp::get_logger("HexapodRobotHardwareInterface"), "on_deactivate");
+    return hardware_interface::CallbackReturn::SUCCESS;
+}
+
+
+std::vector<hardware_interface::StateInterface>
+HexapodRobotHardwareInterface::export_state_interfaces() {
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+        state_interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_states_[joint.name]));
+    }
+    return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+HexapodRobotHardwareInterface::export_command_interfaces() {
+    std::vector<hardware_interface::CommandInterface> command_interfaces;
+    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+        command_interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint_commands_[joint.name]));
+    }
+    return command_interfaces;
+}
+
+
+hardware_interface::return_type
+HexapodRobotHardwareInterface::read(const rclcpp::Time &, const rclcpp::Duration &) {
+    for (const hardware_interface::ComponentInfo &joint: info_.joints) {
+        joint_states_[joint.name]=joint_commands_[joint.name];
+    }
+    return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type
+HexapodRobotHardwareInterface::write(const rclcpp::Time &, const rclcpp::Duration &) {
+    return hardware_interface::return_type::OK;
+}
+
+} // namespace
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(hexapod_robot::HexapodRobotHardwareInterface, hardware_interface::SystemInterface)

hardware/include/hexapod_robot/hexapod_robot_hwi.hpp

@@ -0,0 +1,56 @@
+#ifndef HEXAPOD_ROBOT_HWI_
+#define HEXAPOD_ROBOT_HWI_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_set>
+#include <map>
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+
+namespace hexapod_robot {
+
+struct PWMOutput {
+    uint8_t i2c_address;
+    uint8_t channel;
+};
+
+class HexapodRobotHardwareInterface : public hardware_interface::SystemInterface {
+
+public:
+    RCLCPP_SHARED_PTR_DEFINITIONS(HexapodRobotHardwareInterface)
+    
+    hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
+    
+    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+    
+    hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
+    hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+    
+    hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+    hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+private:
+    int i2c_fd_;
+    bool i2c_available_;
+
+    std::map<uint8_t, bool> pca9685_controller_available_;
+
+    std::map<std::string, double> joint_states_;
+    std::map<std::string, double> joint_commands_;
+
+    std::map<std::string, PWMOutput> joint_pwm_mapping_;
+
+};
+
+} // namespace
+
+#endif // HEXAPOD_ROBOT_HWI_

hexapod_robot.xml

@@ -0,0 +1,7 @@
+<library path="hexapod_robot">
+    <class name="hexapod_robot/HexapodRobotHardwareInterface"
+           type="hexapod_robot::HexapodRobotHardwareInterface"
+           base_class_type="hardware_interface::SystemInterface">
+        <description>Hexapod Hardware Interface</description>
+    </class>
+</library>

python/ik.py

@@ -0,0 +1,42 @@
+import math
+
+
+def ik(side, x, y, z):
+
+    c = 20
+    f = 80
+    t = 130
+    
+    ff = f*f
+    tt = t*t
+
+    print()
+    print(f"ik {side} {x} {y} {z}")
+
+    if side == 'l':
+        x = -x
+
+    coxa_angle = math.atan2(y,x)
+    
+    r = math.sqrt(x*x + y * y) - c
+    print(f"r: {r}")
+
+    rr = r*r
+    zz = z*z
+    dd = rr + zz
+    d  = math.sqrt(dd)
+    
+    print(f"coxa_angle: {coxa_angle}")
+
+    femur_angle = math.acos((ff + dd - tt) / (2 * f * d)) + math.acos(-z/d) - math.pi/2
+    print(f"femur_angle: {femur_angle}")
+    tibia_angle = math.acos((tt + ff - dd) / (2 * t * f))                   - math.pi/2
+    print(f"tibia_angle: {tibia_angle}")
+
+    return(coxa_angle, femur_angle, tibia_angle)
+        
+print(ik('l', -100, 0, -130))
+print(ik('r',  100, 0, -130))
+
+print(ik('l', -120, 0, -130))
+print(ik('r',  120, 0, -130))