wuselfuzz
/
hexapod_robot
關注 1
收藏 0
複製 0
程式碼 問題 0 合併請求 0 版本發佈 0 Wiki 活動
瀏覽代碼

Stuff

main
Marcus Grieger 1 年之前
父節點
61eb0f92c6
當前提交
cee8f1721f
共有 10 個檔案被更改，包括 414 行新增 和 24 行删除
分割檢視
Diff Options
Show Stats Download Patch File Download Diff File
  1. +49
    -4
      CMakeLists.txt
  2. +32
    -0
      bringup/config/controllers.yaml
  3. +56
    -17
      bringup/launch/hexapod_robot.launch.py
  4. +3
    -2
      description/urdf/hexapod_robot.urdf.xacro
  5. +1
    -1
      description/urdf/hexapod_robot_macros.xacro
  6. +70
    -0
      description/urdf/hexapod_robot_ros2_control.xacro
  7. +98
    -0
      hardware/hexapod_robot_hwi.cpp
  8. +56
    -0
      hardware/include/hexapod_robot/hexapod_robot_hwi.hpp
  9. +7
    -0
      hexapod_robot.xml
  10. +42
    -0
      python/ik.py

+ 49
- 4
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()

+ 32
- 0
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

+ 56
- 17
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)

+ 3
- 2
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>

+ 1
- 1
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}"/>


+ 70
- 0
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>

+ 98
- 0
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)

+ 56
- 0
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_

+ 7
- 0
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>

+ 42
- 0
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))

Write Preview
Loading…
取消
儲存