HiveCoreRD/hivecore_robot_arm
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor packages

Browse Source
This commit is contained in:
NuoDaJia
2026-01-28 19:37:26 +08:00
parent 4be8691b53
commit 237f9482c0
62 changed files with 4209 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
dual_arm_action_server/CMakeLists.txt Normal file
View File

@@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 3.8)
project(dual_arm_action_server)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
)
add_executable(dual_arm_action_server_node
src/dual_arm_action_server_node.cpp
src/real_robot_driver.cpp
)
ament_target_dependencies(dual_arm_action_server_node
rclcpp
rclcpp_action
interfaces
geometry_msgs
tf2
)
find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/../lib")
target_link_libraries(dual_arm_action_server_node ${RMAN_API_LIB})
install(TARGETS
dual_arm_action_server_node
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY include/
DESTINATION include/${PROJECT_NAME}
)
install(
DIRECTORY launch config
DESTINATION share/${PROJECT_NAME}
OPTIONAL
)
ament_package()

10
dual_arm_action_server/config/dual_arm_action_server.yaml Normal file
View File

@@ -0,0 +1,10 @@
dual_arm_action_server:
ros__parameters:
left_arm_ip: "192.168.2.2"
right_arm_ip: "192.168.2.18"
arm_port: 8080
left_push_ip: "192.168.2.3"
right_push_ip: "192.168.2.19"
push_port: 8089
push_cycle: 20

48
dual_arm_action_server/include/dual_arm_action_server/dual_arm_action_server.hpp Normal file
View File

@@ -0,0 +1,48 @@
#pragma once
#include "dual_arm_action_server/real_robot_driver.hpp"
#include "interfaces/action/dual_arm.hpp"
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <atomic>
#include <mutex>
#include <string>
#include <vector>
namespace dual_arm_action_server
{
class DualArmActionServer
{
public:
explicit DualArmActionServer(const rclcpp::Node::SharedPtr & node);
void initialize();
private:
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const DualArm::Goal> goal);
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleDualArm> goal_handle);
void handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);
void execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);
bool validate_goal(const DualArm::Goal & goal, std::string & error_msg) const;
std::vector<interfaces::msg::ArmMotionParams> normalize_goal_params(
const DualArm::Goal & goal) const;
void get_target_arms(const DualArm::Goal & goal, bool & need_left, bool & need_right) const;
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<DualArm>::SharedPtr action_server_;
RealRobotDriver driver_;
std::atomic<bool> executing_left_;
std::atomic<bool> executing_right_;
mutable std::mutex exec_state_mutex_;
};
} // namespace dual_arm_action_server

83
dual_arm_action_server/include/dual_arm_action_server/real_robot_driver.hpp Normal file
View File

@@ -0,0 +1,83 @@
#pragma once
#include "rm_service.h"
#include "rm_define.h"
#include "interfaces/msg/arm_motion_params.hpp"
#include "arm_action_define.h"
#include <rclcpp/rclcpp.hpp>
#include <atomic>
#include <mutex>
#include <string>
#include <vector>
namespace dual_arm_action_server
{
class RealRobotDriver
{
public:
RealRobotDriver();
~RealRobotDriver();
bool initialize(
const rclcpp::Logger & logger,
const std::string & left_ip,
const std::string & right_ip,
int arm_port,
const std::string & left_push_ip,
const std::string & right_push_ip,
int push_port,
int push_cycle);
void shutdown();
bool is_connected() const;
bool execute_motion(
const interfaces::msg::ArmMotionParams & params,
uint8_t arm_id,
double velocity_scaling,
double blend_radius,
std::string * error_msg);
bool get_current_joints(
std::vector<double> & joints_left,
std::vector<double> & joints_right);
void stop_arm(uint8_t arm_id);
void stop_all();
private:
static void realtime_state_callback(rm_realtime_arm_joint_state_t data);
static void api_log_callback(const char * message, va_list args);
void update_joints(const rm_realtime_arm_joint_state_t & data);
bool init_handle(
rm_robot_handle ** handle,
const std::string & ip,
int port,
const std::string & push_ip,
int push_port,
int push_cycle);
rm_robot_handle * handle_for(uint8_t arm_id) const;
static int clamp_speed_percent(double scaling);
static int clamp_blend_radius_mm(double blend_radius_m);
static RealRobotDriver * instance_;
rclcpp::Logger logger_;
std::unique_ptr<RM_Service> rm_service_;
rm_robot_handle * left_handle_;
rm_robot_handle * right_handle_;
std::atomic<bool> connected_;
std::string left_ip_;
std::string right_ip_;
std::mutex joints_mutex_;
std::vector<double> joints_left_;
std::vector<double> joints_right_;
};
} // namespace dual_arm_action_server

23
dual_arm_action_server/launch/dual_arm_action_server.launch.py Normal file
View File

@@ -0,0 +1,23 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
config_path = os.path.join(
get_package_share_directory("dual_arm_action_server"),
"config",
"dual_arm_action_server.yaml",
)
return LaunchDescription([
Node(
package="dual_arm_action_server",
executable="dual_arm_action_server_node",
name="dual_arm_action_server",
output="screen",
parameters=[config_path],
)
])

24
dual_arm_action_server/package.xml Normal file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<package format="3">
<name>dual_arm_action_server</name>
<version>0.1.0</version>
<description>Dual arm action server for handling DualArm.action requests</description>
<maintainer email="dev@hivecore.ai">hivecore</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>interfaces</depend>
<depend>geometry_msgs</depend>
<depend>tf2</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

355
dual_arm_action_server/src/dual_arm_action_server_node.cpp Normal file
View File

@@ -0,0 +1,355 @@
#include "dual_arm_action_server/dual_arm_action_server.hpp"
#include "arm_action_define.h"
#include <chrono>
#include <future>
#include <thread>
namespace dual_arm_action_server
{
DualArmActionServer::DualArmActionServer(const rclcpp::Node::SharedPtr & node)
: node_(node),
executing_left_(false),
executing_right_(false)
{
}
void DualArmActionServer::initialize()
{
node_->declare_parameter<std::string>("left_arm_ip");
node_->declare_parameter<std::string>("right_arm_ip");
node_->declare_parameter<int>("arm_port");
node_->declare_parameter<std::string>("left_push_ip");
node_->declare_parameter<std::string>("right_push_ip");
node_->declare_parameter<int>("push_port");
node_->declare_parameter<int>("push_cycle");
std::string left_arm_ip;
std::string right_arm_ip;
std::string left_push_ip;
std::string right_push_ip;
int arm_port = 0;
int push_port = 0;
int push_cycle = 0;
const bool params_ok =
node_->get_parameter("left_arm_ip", left_arm_ip) &&
node_->get_parameter("right_arm_ip", right_arm_ip) &&
node_->get_parameter("arm_port", arm_port) &&
node_->get_parameter("left_push_ip", left_push_ip) &&
node_->get_parameter("right_push_ip", right_push_ip) &&
node_->get_parameter("push_port", push_port) &&
node_->get_parameter("push_cycle", push_cycle);
if (!params_ok ||
left_arm_ip.empty() ||
right_arm_ip.empty() ||
left_push_ip.empty() ||
right_push_ip.empty() ||
arm_port <= 0 ||
push_port <= 0 ||
push_cycle <= 0)
{
RCLCPP_ERROR(node_->get_logger(),
"Missing/invalid parameters. Please load config/dual_arm_action_server.yaml");
return;
}
if (!driver_.initialize(
node_->get_logger(),
left_arm_ip,
right_arm_ip,
arm_port,
left_push_ip,
right_push_ip,
push_port,
push_cycle))
{
RCLCPP_ERROR(node_->get_logger(), "Failed to initialize RealRobotDriver");
}
action_server_ = rclcpp_action::create_server<DualArm>(
node_,
"DualArm",
std::bind(&DualArmActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&DualArmActionServer::handle_cancel, this, std::placeholders::_1),
std::bind(&DualArmActionServer::handle_accepted, this, std::placeholders::_1));
RCLCPP_INFO(node_->get_logger(), "DualArm action server is ready");
}
rclcpp_action::GoalResponse DualArmActionServer::handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const DualArm::Goal> goal)
{
(void)uuid;
if (!driver_.is_connected()) {
RCLCPP_ERROR(node_->get_logger(), "DualArm rejected: real robot not connected");
return rclcpp_action::GoalResponse::REJECT;
}
std::string error_msg;
if (!validate_goal(*goal, error_msg)) {
RCLCPP_ERROR(node_->get_logger(), "DualArm rejected: %s", error_msg.c_str());
return rclcpp_action::GoalResponse::REJECT;
}
bool need_left = false;
bool need_right = false;
get_target_arms(*goal, need_left, need_right);
{
std::lock_guard<std::mutex> lock(exec_state_mutex_);
if ((need_left && executing_left_.load()) || (need_right && executing_right_.load())) {
RCLCPP_WARN(node_->get_logger(), "DualArm rejected: target arm is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (need_left) {
executing_left_.store(true);
}
if (need_right) {
executing_right_.store(true);
}
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse DualArmActionServer::handle_cancel(
const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
(void)goal_handle;
bool need_left = false;
bool need_right = false;
get_target_arms(*goal_handle->get_goal(), need_left, need_right);
if (need_left) {
driver_.stop_arm(LEFT_ARM);
}
if (need_right) {
driver_.stop_arm(RIGHT_ARM);
}
RCLCPP_INFO(node_->get_logger(), "DualArm cancel requested");
return rclcpp_action::CancelResponse::ACCEPT;
}
void DualArmActionServer::handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
std::thread{&DualArmActionServer::execute, this, goal_handle}.detach();
}
void DualArmActionServer::execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
auto result = std::make_shared<DualArm::Result>();
auto feedback = std::make_shared<DualArm::Feedback>();
result->success = false;
result->final_progress = 0.0;
result->message = "DualArm action failed";
const auto goal = goal_handle->get_goal();
const auto params_list = normalize_goal_params(*goal);
const size_t total = params_list.size();
bool need_left = false;
bool need_right = false;
get_target_arms(*goal, need_left, need_right);
std::vector<std::string> errors(total);
std::vector<std::future<bool>> futures;
futures.reserve(total);
for (size_t i = 0; i < total; ++i) {
const auto params = params_list[i];
futures.emplace_back(std::async(std::launch::async, [this, params, goal, &errors, i]() {
return driver_.execute_motion(
params,
params.arm_id,
goal->velocity_scaling,
params.blend_radius,
&errors[i]);
}));
}
while (rclcpp::ok()) {
if (goal_handle->is_canceling()) {
if (need_left) {
driver_.stop_arm(LEFT_ARM);
}
if (need_right) {
driver_.stop_arm(RIGHT_ARM);
}
result->message = "DualArm canceled";
result->success = false;
result->final_progress = 0.0;
goal_handle->canceled(result);
{
std::lock_guard<std::mutex> lock(exec_state_mutex_);
if (need_left) {
executing_left_.store(false);
}
if (need_right) {
executing_right_.store(false);
}
}
return;
}
size_t done_count = 0;
for (auto & future : futures) {
if (future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
++done_count;
}
}
std::vector<double> joints_left;
std::vector<double> joints_right;
driver_.get_current_joints(joints_left, joints_right);
feedback->joints_left = joints_left;
feedback->joints_right = joints_right;
feedback->progress = total > 0 ? static_cast<double>(done_count) / total : 0.0;
feedback->status = DualArm::Goal::STATUS_EXECUTING;
goal_handle->publish_feedback(feedback);
if (done_count == total) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
bool all_success = true;
for (size_t i = 0; i < total; ++i) {
bool ok = futures[i].get();
if (!ok) {
all_success = false;
if (!errors[i].empty()) {
result->message = errors[i];
}
}
}
result->success = all_success;
result->final_progress = all_success ? 1.0 : 0.0;
if (all_success) {
result->message = "DualArm succeeded";
goal_handle->succeed(result);
} else {
goal_handle->abort(result);
}
{
std::lock_guard<std::mutex> lock(exec_state_mutex_);
if (need_left) {
executing_left_.store(false);
}
if (need_right) {
executing_right_.store(false);
}
}
}
bool DualArmActionServer::validate_goal(const DualArm::Goal & goal, std::string & error_msg) const
{
if (goal.velocity_scaling < 0.0 || goal.velocity_scaling > 1.0) {
error_msg = "velocity_scaling out of range [0.0, 1.0]";
return false;
}
if (goal.arm_motion_params.empty() || goal.arm_motion_params.size() > 2) {
error_msg = "arm_motion_params must contain 1 or 2 items";
return false;
}
bool has_left = false;
bool has_right = false;
for (const auto & params : goal.arm_motion_params) {
if (params.motion_type != interfaces::msg::ArmMotionParams::MOVEJ &&
params.motion_type != interfaces::msg::ArmMotionParams::MOVEL)
{
error_msg = "unsupported motion_type";
return false;
}
if (params.blend_radius < 0.0 || params.blend_radius > 0.5) {
error_msg = "blend_radius out of range [0.0, 0.5]";
return false;
}
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEJ &&
params.target_joints.size() != USED_ARM_DOF)
{
error_msg = "MOVEJ expects target_joints size " + std::to_string(USED_ARM_DOF);
return false;
}
if (params.arm_id == LEFT_ARM) {
has_left = true;
} else if (params.arm_id == RIGHT_ARM) {
has_right = true;
} else {
error_msg = "invalid arm_id (expected 0=left, 1=right)";
return false;
}
}
if (goal.arm_motion_params.size() == 2 && (!has_left || !has_right)) {
error_msg = "dual-arm mode requires both left and right arm_id";
return false;
}
return true;
}
std::vector<interfaces::msg::ArmMotionParams> DualArmActionServer::normalize_goal_params(
const DualArm::Goal & goal) const
{
std::vector<interfaces::msg::ArmMotionParams> params;
params.reserve(goal.arm_motion_params.size());
if (goal.arm_motion_params.size() == 1) {
params.push_back(goal.arm_motion_params.front());
return params;
}
interfaces::msg::ArmMotionParams left;
interfaces::msg::ArmMotionParams right;
for (const auto & item : goal.arm_motion_params) {
if (item.arm_id == LEFT_ARM) {
left = item;
} else if (item.arm_id == RIGHT_ARM) {
right = item;
}
}
if (left.arm_id == LEFT_ARM) {
params.push_back(left);
}
if (right.arm_id == RIGHT_ARM) {
params.push_back(right);
}
return params;
}
void DualArmActionServer::get_target_arms(
const DualArm::Goal & goal,
bool & need_left,
bool & need_right) const
{
need_left = false;
need_right = false;
for (const auto & params : goal.arm_motion_params) {
if (params.arm_id == LEFT_ARM) {
need_left = true;
} else if (params.arm_id == RIGHT_ARM) {
need_right = true;
}
}
}
} // namespace dual_arm_action_server
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("dual_arm_action_server");
dual_arm_action_server::DualArmActionServer server(node);
server.initialize();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

329
dual_arm_action_server/src/real_robot_driver.cpp Normal file
View File

@@ -0,0 +1,329 @@
#include "dual_arm_action_server/real_robot_driver.hpp"
#include <tf2/LinearMath/Matrix3x3.hpp>
#include <tf2/LinearMath/Quaternion.hpp>
#include <algorithm>
#include <cmath>
#include <cstring>
namespace dual_arm_action_server
{
RealRobotDriver * RealRobotDriver::instance_ = nullptr;
RealRobotDriver::RealRobotDriver()
: logger_(rclcpp::get_logger("dual_arm_action_server")),
left_handle_(nullptr),
right_handle_(nullptr),
connected_(false),
joints_left_(USED_ARM_DOF, 0.0),
joints_right_(USED_ARM_DOF, 0.0)
{
}
RealRobotDriver::~RealRobotDriver()
{
shutdown();
}
bool RealRobotDriver::initialize(
const rclcpp::Logger & logger,
const std::string & left_ip,
const std::string & right_ip,
int arm_port,
const std::string & left_push_ip,
const std::string & right_push_ip,
int push_port,
int push_cycle)
{
logger_ = logger;
left_ip_ = left_ip;
right_ip_ = right_ip;
rm_service_ = std::make_unique<RM_Service>();
if (!rm_service_) {
RCLCPP_ERROR(logger_, "Failed to allocate RM_Service");
return false;
}
if (rm_service_->rm_init(RM_TRIPLE_MODE_E) != 0) {
RCLCPP_ERROR(logger_, "rm_init failed");
rm_service_.reset();
return false;
}
instance_ = this;
rm_service_->rm_set_log_call_back(api_log_callback, 3);
rm_service_->rm_realtime_arm_state_call_back(realtime_state_callback);
if (!init_handle(&left_handle_, left_ip, arm_port, left_push_ip, push_port, push_cycle)) {
shutdown();
return false;
}
if (!init_handle(&right_handle_, right_ip, arm_port, right_push_ip, push_port, push_cycle)) {
shutdown();
return false;
}
connected_.store(true);
return true;
}
void RealRobotDriver::shutdown()
{
connected_.store(false);
if (rm_service_) {
rm_service_->rm_destroy();
rm_service_.reset();
}
left_handle_ = nullptr;
right_handle_ = nullptr;
instance_ = nullptr;
}
bool RealRobotDriver::is_connected() const
{
return connected_.load();
}
bool RealRobotDriver::execute_motion(
const interfaces::msg::ArmMotionParams & params,
uint8_t arm_id,
double velocity_scaling,
double blend_radius,
std::string * error_msg)
{
const int blend_radius_mm = clamp_blend_radius_mm(blend_radius);
const int trajectory_connect = (blend_radius_mm > 0) ? 1 : 0;
const int block_mode = (blend_radius_mm > 0) ? 0 : 1;
rm_robot_handle * handle = handle_for(arm_id);
if (!handle) {
if (error_msg) {
*error_msg = "Invalid arm_id or missing robot handle";
}
return false;
}
const int speed_percent = clamp_speed_percent(velocity_scaling);
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEJ) {
if (params.target_joints.size() != USED_ARM_DOF) {
if (error_msg) {
*error_msg = "MOVEJ expects target_joints size " + std::to_string(USED_ARM_DOF);
}
return false;
}
float joints[ARM_DOF] = {0.0f};
for (size_t i = 0; i < USED_ARM_DOF; ++i) {
joints[i] = static_cast<float>(params.target_joints[i] * 180.0 / M_PI);
}
const int result = rm_service_->rm_movej(
handle,
joints,
speed_percent,
blend_radius_mm,
trajectory_connect,
block_mode);
if (result != 0) {
if (error_msg) {
*error_msg = "rm_movej failed with code " + std::to_string(result);
}
return false;
}
return true;
}
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEL) {
rm_pose_t pose{};
pose.position.x = static_cast<float>(params.target_pose.position.x);
pose.position.y = static_cast<float>(params.target_pose.position.y);
pose.position.z = static_cast<float>(params.target_pose.position.z);
pose.quaternion.w = static_cast<float>(params.target_pose.orientation.w);
pose.quaternion.x = static_cast<float>(params.target_pose.orientation.x);
pose.quaternion.y = static_cast<float>(params.target_pose.orientation.y);
pose.quaternion.z = static_cast<float>(params.target_pose.orientation.z);
tf2::Quaternion q(
pose.quaternion.x,
pose.quaternion.y,
pose.quaternion.z,
pose.quaternion.w);
tf2::Matrix3x3 m(q);
double roll = 0.0;
double pitch = 0.0;
double yaw = 0.0;
m.getRPY(roll, pitch, yaw);
pose.euler.rx = static_cast<float>(roll);
pose.euler.ry = static_cast<float>(pitch);
pose.euler.rz = static_cast<float>(yaw);
const int result = rm_service_->rm_movel(
handle,
pose,
speed_percent,
blend_radius_mm,
trajectory_connect,
block_mode);
if (result != 0) {
if (error_msg) {
*error_msg = "rm_movel failed with code " + std::to_string(result);
}
return false;
}
return true;
}
if (error_msg) {
*error_msg = "Unsupported motion_type " + std::to_string(params.motion_type);
}
return false;
}
bool RealRobotDriver::get_current_joints(
std::vector<double> & joints_left,
std::vector<double> & joints_right)
{
std::lock_guard<std::mutex> lock(joints_mutex_);
joints_left.resize(USED_ARM_DOF);
joints_right.resize(USED_ARM_DOF);
for (size_t i = 0; i < USED_ARM_DOF; ++i) {
joints_left[i] = joints_left_[i] * M_PI / 180.0;
joints_right[i] = joints_right_[i] * M_PI / 180.0;
}
return true;
}
void RealRobotDriver::stop_arm(uint8_t arm_id)
{
rm_robot_handle * handle = handle_for(arm_id);
if (handle && rm_service_) {
rm_service_->rm_set_arm_stop(handle);
}
}
void RealRobotDriver::stop_all()
{
stop_arm(LEFT_ARM);
stop_arm(RIGHT_ARM);
}
void RealRobotDriver::realtime_state_callback(rm_realtime_arm_joint_state_t data)
{
if (instance_) {
instance_->update_joints(data);
}
}
void RealRobotDriver::api_log_callback(const char * message, va_list args)
{
if (!instance_ || !message) {
return;
}
char buffer[1024];
vsnprintf(buffer, sizeof(buffer), message, args);
RCLCPP_INFO(instance_->logger_, "%s", buffer);
}
void RealRobotDriver::update_joints(const rm_realtime_arm_joint_state_t & data)
{
std::lock_guard<std::mutex> lock(joints_mutex_);
if (std::strcmp(data.arm_ip, left_ip_.c_str()) == 0) {
for (size_t i = 0; i < USED_ARM_DOF; ++i) {
joints_left_[i] = data.joint_status.joint_position[i];
}
return;
}
if (std::strcmp(data.arm_ip, right_ip_.c_str()) == 0) {
for (size_t i = 0; i < USED_ARM_DOF; ++i) {
joints_right_[i] = data.joint_status.joint_position[i];
}
}
}
bool RealRobotDriver::init_handle(
rm_robot_handle ** handle,
const std::string & ip,
int port,
const std::string & push_ip,
int push_port,
int push_cycle)
{
if (!rm_service_) {
return false;
}
*handle = rm_service_->rm_create_robot_arm(ip.c_str(), port);
if (*handle == nullptr || (*handle)->id <= 0) {
RCLCPP_ERROR(logger_, "Failed to create handle for arm %s", ip.c_str());
return false;
}
rm_realtime_push_config_t config{};
config.cycle = push_cycle;
config.enable = true;
config.port = push_port;
config.force_coordinate = 0;
std::snprintf(config.ip, sizeof(config.ip), "%s", push_ip.c_str());
const int push_result = rm_service_->rm_set_realtime_push(*handle, config);
if (push_result != 0) {
RCLCPP_ERROR(
logger_,
"Failed to set realtime push for arm %s (code %d)",
ip.c_str(),
push_result);
return false;
}
rm_robot_info_t robot_info{};
const int info_result = rm_service_->rm_get_robot_info(*handle, &robot_info);
if (info_result != 0 || robot_info.arm_dof != USED_ARM_DOF) {
RCLCPP_ERROR(
logger_,
"Invalid robot info for arm %s (code %d, dof %d)",
ip.c_str(),
info_result,
robot_info.arm_dof);
return false;
}
return true;
}
rm_robot_handle * RealRobotDriver::handle_for(uint8_t arm_id) const
{
if (arm_id == LEFT_ARM) {
return left_handle_;
}
if (arm_id == RIGHT_ARM) {
return right_handle_;
}
return nullptr;
}
int RealRobotDriver::clamp_speed_percent(double scaling)
{
if (!std::isfinite(scaling)) {
return 50;
}
int percent = static_cast<int>(std::round(scaling * 100.0));
percent = std::clamp(percent, 1, 100);
return percent;
}
int RealRobotDriver::clamp_blend_radius_mm(double blend_radius_m)
{
if (!std::isfinite(blend_radius_m) || blend_radius_m <= 0.0) {
return 0;
}
const double mm = blend_radius_m * 1000.0;
const int radius = static_cast<int>(std::lround(mm));
return std::clamp(radius, 0, 500);
}
} // namespace dual_arm_action_server

50
dual_arm_moveit_action_server/CMakeLists.txt Normal file
View File

@@ -0,0 +1,50 @@
cmake_minimum_required(VERSION 3.8)
project(dual_arm_moveit_action_server)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
add_compile_options(-g)
include_directories(include)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
add_executable(dual_arm_moveit_action_server
src/dual_arm_moveit_action_server.cpp
)
ament_target_dependencies(dual_arm_moveit_action_server
rclcpp
rclcpp_action
interfaces
geometry_msgs
moveit_ros_planning_interface
moveit_msgs
)
install(TARGETS
dual_arm_moveit_action_server
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY include/
DESTINATION include
)
install(DIRECTORY config meshes urdf
DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)
ament_package()

105
dual_arm_moveit_action_server/config/FHrobot.ros2_control.xacro Normal file
View File

@@ -0,0 +1,105 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="FHrobot_ros2_control" params="name initial_positions_file">
<xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
<ros2_control name="${name}" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="body_2_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['body_2_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint2">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint2']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint3">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint3']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint4">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint4']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint5">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint5']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="left_joint6">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['left_joint6']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint1">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint1']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint2">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint2']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint3">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint3']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint4">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint4']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint5">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint5']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="right_joint6">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['right_joint6']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>

460
dual_arm_moveit_action_server/config/FHrobot.srdf Normal file
View File

@@ -0,0 +1,460 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="FHrobot">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="LeftArm">
<link name="body_2_link"/>
<link name="base_link_leftarm"/>
<link name="LeftLink1"/>
<link name="LeftLink2"/>
<link name="LeftLink3"/>
<link name="LeftLink4"/>
<link name="LeftLink5"/>
<link name="LeftLink6"/>
<joint name="left_arm_base_joint"/>
<joint name="left_joint1"/>
<joint name="left_joint2"/>
<joint name="left_joint3"/>
<joint name="left_joint4"/>
<joint name="left_joint5"/>
<joint name="left_joint6"/>
<chain base_link="body_2_link" tip_link="LeftLink6"/>
</group>
<group name="RightArm">
<link name="body_2_link"/>
<link name="base_link_rightarm"/>
<link name="RightLink1"/>
<link name="RightLink2"/>
<link name="RightLink3"/>
<link name="RightLink4"/>
<link name="RightLink5"/>
<link name="RightLink6"/>
<joint name="right_arm_base_joint"/>
<joint name="right_joint1"/>
<joint name="right_joint2"/>
<joint name="right_joint3"/>
<joint name="right_joint4"/>
<joint name="right_joint5"/>
<joint name="right_joint6"/>
<chain base_link="body_2_link" tip_link="RightLink6"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="LeftZero" group="LeftArm">
<joint name="body_2_joint" value="0"/>
<joint name="left_joint1" value="0"/>
<joint name="left_joint2" value="0"/>
<joint name="left_joint3" value="0"/>
<joint name="left_joint4" value="0"/>
<joint name="left_joint5" value="0"/>
<joint name="left_joint6" value="0"/>
</group_state>
<group_state name="RightZero" group="RightArm">
<joint name="body_2_joint" value="0"/>
<joint name="right_joint1" value="0"/>
<joint name="right_joint2" value="0"/>
<joint name="right_joint3" value="0"/>
<joint name="right_joint4" value="0"/>
<joint name="right_joint5" value="0"/>
<joint name="right_joint6" value="0"/>
</group_state>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="base_joint_to_world" type="fixed" parent_frame="world" child_link="base_link"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="LeftLink1" link2="LeftLink2" reason="Adjacent"/>
<disable_collisions link1="LeftLink1" link2="LeftLink3" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="LeftLink4" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="RightLink1" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="RightLink2" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="RightLink3" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="RightLink4" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="base_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="base_link_leftarm" reason="Adjacent"/>
<disable_collisions link1="LeftLink1" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="body_2_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="body_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="head_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink1" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="LeftLink3" reason="Adjacent"/>
<disable_collisions link1="LeftLink2" link2="LeftLink4" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="RightLink1" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="RightLink2" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="RightLink3" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="base_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="body_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink2" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="LeftLink4" reason="Adjacent"/>
<disable_collisions link1="LeftLink3" link2="LeftLink5" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="LeftLink6" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="RightLink1" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="RightLink2" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="RightLink3" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="base_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="body_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink3" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="LeftLink5" reason="Adjacent"/>
<disable_collisions link1="LeftLink4" link2="LeftLink6" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="RightLink1" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="LeftLink4" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="LeftLink6" reason="Adjacent"/>
<disable_collisions link1="LeftLink5" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink5" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="LeftLink6" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="RightLink2" reason="Adjacent"/>
<disable_collisions link1="RightLink1" link2="RightLink3" reason="Never"/>
<disable_collisions link1="RightLink1" link2="RightLink4" reason="Never"/>
<disable_collisions link1="RightLink1" link2="base_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="RightLink1" link2="base_link_rightarm" reason="Adjacent"/>
<disable_collisions link1="RightLink1" link2="body_2_link" reason="Default"/>
<disable_collisions link1="RightLink1" link2="body_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="head_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink1" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="RightLink3" reason="Adjacent"/>
<disable_collisions link1="RightLink2" link2="RightLink4" reason="Never"/>
<disable_collisions link1="RightLink2" link2="base_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="RightLink2" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="RightLink2" link2="body_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink2" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="RightLink4" reason="Adjacent"/>
<disable_collisions link1="RightLink3" link2="RightLink5" reason="Never"/>
<disable_collisions link1="RightLink3" link2="RightLink6" reason="Never"/>
<disable_collisions link1="RightLink3" link2="base_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="RightLink3" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="RightLink3" link2="body_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink3" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="RightLink5" reason="Adjacent"/>
<disable_collisions link1="RightLink4" link2="RightLink6" reason="Never"/>
<disable_collisions link1="RightLink4" link2="base_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="RightLink4" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="RightLink6" reason="Adjacent"/>
<disable_collisions link1="RightLink5" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink5" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="RightLink6" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link" link2="base_link_leftarm" reason="Never"/>
<disable_collisions link1="base_link" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="base_link" link2="body_2_link" reason="Never"/>
<disable_collisions link1="base_link" link2="body_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link" link2="left_behind_hip_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link" link2="left_front_roll_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link" link2="right_behind_hip_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="base_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link" link2="right_front_roll_link" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="base_link_rightarm" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="body_2_link" reason="Adjacent"/>
<disable_collisions link1="base_link_leftarm" link2="body_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_leftarm" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="body_2_link" reason="Adjacent"/>
<disable_collisions link1="base_link_rightarm" link2="body_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="head_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="base_link_rightarm" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="body_link" reason="Adjacent"/>
<disable_collisions link1="body_2_link" link2="head_link" reason="Adjacent"/>
<disable_collisions link1="body_2_link" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="body_2_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_link" link2="head_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="body_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="body_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_behind_hip_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_behind_leg_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_front_knee_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="head_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="head_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="left_behind_leg_link" reason="Adjacent"/>
<disable_collisions link1="left_behind_hip_link" link2="left_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_hip_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="left_behind_wheel_link" reason="Adjacent"/>
<disable_collisions link1="left_behind_leg_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_leg_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="left_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_behind_wheel_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="left_front_knee_link" reason="Adjacent"/>
<disable_collisions link1="left_front_hip_link" link2="left_front_roll_link" reason="Adjacent"/>
<disable_collisions link1="left_front_hip_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_front_hip_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="left_front_roll_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="left_front_wheel_link" reason="Adjacent"/>
<disable_collisions link1="left_front_knee_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_front_knee_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="left_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_front_roll_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_behind_hip_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_behind_leg_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_front_knee_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="left_front_wheel_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="right_behind_hip_link" link2="right_behind_leg_link" reason="Adjacent"/>
<disable_collisions link1="right_behind_hip_link" link2="right_behind_wheel_link" reason="Never"/>
<disable_collisions link1="right_behind_hip_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="right_behind_hip_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="right_behind_leg_link" link2="right_behind_wheel_link" reason="Adjacent"/>
<disable_collisions link1="right_behind_leg_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="right_behind_leg_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="right_behind_wheel_link" link2="right_front_hip_link" reason="Never"/>
<disable_collisions link1="right_behind_wheel_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="right_front_hip_link" link2="right_front_knee_link" reason="Adjacent"/>
<disable_collisions link1="right_front_hip_link" link2="right_front_roll_link" reason="Adjacent"/>
<disable_collisions link1="right_front_hip_link" link2="right_front_wheel_link" reason="Never"/>
<disable_collisions link1="right_front_knee_link" link2="right_front_roll_link" reason="Never"/>
<disable_collisions link1="right_front_knee_link" link2="right_front_wheel_link" reason="Adjacent"/>
<disable_collisions link1="right_front_roll_link" link2="right_front_wheel_link" reason="Never"/>
</robot>

14
dual_arm_moveit_action_server/config/FHrobot.urdf.xacro Normal file
View File

@@ -0,0 +1,14 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="FHrobot">
<xacro:arg name="initial_positions_file" default="initial_positions.yaml" />
<!-- Import FHrobot urdf file -->
<xacro:include filename="$(find dual_arm_moveit_action_server)/urdf/FHrobot.urdf" />
<!-- Import control_xacro -->
<xacro:include filename="FHrobot.ros2_control.xacro" />
<xacro:FHrobot_ros2_control name="FakeSystem" initial_positions_file="$(arg initial_positions_file)"/>
</robot>

16
dual_arm_moveit_action_server/config/initial_positions.yaml Normal file
View File

@@ -0,0 +1,16 @@
# Default initial positions for FHrobot's ros2_control fake system
initial_positions:
body_2_joint: 0
left_joint1: 0
left_joint2: 0
left_joint3: 0
left_joint4: 0
left_joint5: 0
left_joint6: 0
right_joint1: 0
right_joint2: 0
right_joint3: 0
right_joint4: 0
right_joint5: 0
right_joint6: 0

75
dual_arm_moveit_action_server/config/joint_limits.yaml Normal file
View File

@@ -0,0 +1,75 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
body_2_joint:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
left_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
left_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
left_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
left_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint1:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
right_joint2:
has_velocity_limits: true
max_velocity: 3.1400000000000001
has_acceleration_limits: false
max_acceleration: 0
right_joint3:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint4:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint5:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0
right_joint6:
has_velocity_limits: true
max_velocity: 3.9199999999999999
has_acceleration_limits: false
max_acceleration: 0

8
dual_arm_moveit_action_server/config/kinematics.yaml Normal file
View File

@@ -0,0 +1,8 @@
LeftArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001
RightArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001

51
dual_arm_moveit_action_server/config/moveit.rviz Normal file
View File

@@ -0,0 +1,51 @@
Panels:
- Class: rviz_common/Displays
Name: Displays
Property Tree Widget:
Expanded:
- /MotionPlanning1
- Class: rviz_common/Help
Name: Help
- Class: rviz_common/Views
Name: Views
Visualization Manager:
Displays:
- Class: rviz_default_plugins/Grid
Name: Grid
Value: true
- Class: moveit_rviz_plugin/MotionPlanning
Name: MotionPlanning
Planned Path:
Loop Animation: true
State Display Time: 0.05 s
Trajectory Topic: display_planned_path
Planning Scene Topic: monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
Scene Robot:
Robot Alpha: 0.5
Value: true
Global Options:
Fixed Frame: base_link
Tools:
- Class: rviz_default_plugins/Interact
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 2.0
Focal Point:
X: -0.1
Y: 0.25
Z: 0.30
Name: Current View
Pitch: 0.5
Target Frame: base_link
Yaw: -0.623
Window Geometry:
Height: 975
QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Width: 1200

33
dual_arm_moveit_action_server/config/moveit_controllers.yaml Normal file
View File

@@ -0,0 +1,33 @@
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- leftarm_controller
- rightarm_controller
leftarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
- left_joint4
- left_joint5
- left_joint6
action_ns: follow_joint_trajectory
default: true
rightarm_controller:
type: FollowJointTrajectory
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3
- right_joint4
- right_joint5
- right_joint6
action_ns: follow_joint_trajectory
default: true

6
dual_arm_moveit_action_server/config/pilz_cartesian_limits.yaml Normal file
View File

@@ -0,0 +1,6 @@
# Limits for the Pilz planner
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
max_trans_dec: -5.0
max_rot_vel: 1.57

46
dual_arm_moveit_action_server/config/ros2_controllers.yaml Normal file
View File

@@ -0,0 +1,46 @@
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
leftarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
rightarm_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
leftarm_controller:
ros__parameters:
joints:
- body_2_joint
- left_joint1
- left_joint2
- left_joint3
- left_joint4
- left_joint5
- left_joint6
command_interfaces:
- position
state_interfaces:
- position
- velocity
rightarm_controller:
ros__parameters:
joints:
- body_2_joint
- right_joint1
- right_joint2
- right_joint3
- right_joint4
- right_joint5
- right_joint6
command_interfaces:
- position
state_interfaces:
- position
- velocity

84
dual_arm_moveit_action_server/include/dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp Normal file
View File

@@ -0,0 +1,84 @@
#pragma once
#include <atomic>
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/pose.hpp"
#include "interfaces/action/dual_arm.hpp"
#include "interfaces/msg/arm_motion_params.hpp"
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/robot_state/conversions.h"
#include "moveit/robot_trajectory/robot_trajectory.h"
#include "moveit/trajectory_processing/iterative_time_parameterization.h"
#include "moveit_msgs/msg/display_trajectory.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
namespace dual_arm_moveit_action_server
{
using DualArm = interfaces::action::DualArm;
using GoalHandleDualArm = rclcpp_action::ServerGoalHandle<DualArm>;
class DualArmMoveitActionServer
{
public:
explicit DualArmMoveitActionServer(const rclcpp::Node::SharedPtr & node);
void initialize();
private:
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const DualArm::Goal> goal);
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleDualArm> goal_handle);
void handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle);
void execute(const std::shared_ptr<GoalHandleDualArm> goal_handle);
bool validate_goal(const DualArm::Goal & goal, std::string * error_msg) const;
std::vector<interfaces::msg::ArmMotionParams> normalize_goal_params(
const DualArm::Goal & goal) const;
bool plan_for_arm(
const interfaces::msg::ArmMotionParams & params,
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
moveit_msgs::msg::RobotTrajectory * out_trajectory,
std::string * error_msg);
bool time_parameterize_trajectory(
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
moveit_msgs::msg::RobotTrajectory & trajectory,
std::string * error_msg);
void publish_display_trajectory(
const moveit_msgs::msg::RobotTrajectory & trajectory,
moveit::planning_interface::MoveGroupInterface & move_group);
void fill_feedback_joints(DualArm::Feedback & feedback);
rclcpp::Node::SharedPtr node_;
rclcpp_action::Server<DualArm>::SharedPtr action_server_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> left_move_group_;
std::shared_ptr<moveit::planning_interface::MoveGroupInterface> right_move_group_;
rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_pub_;
std::atomic_bool planning_busy_{false};
std::string left_group_name_;
std::string right_group_name_;
double planning_time_{5.0};
double acceleration_scaling_{0.5};
double cartesian_step_{0.01};
double cartesian_jump_threshold_{0.0};
double cartesian_min_fraction_{0.9};
};
} // namespace dual_arm_moveit_action_server

40
dual_arm_moveit_action_server/launch/dual_arm_moveit_action_server.launch.py Normal file
View File

@@ -0,0 +1,40 @@
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
moveit_config = MoveItConfigsBuilder(
"FHrobot", package_name="dual_arm_moveit_action_server"
).to_moveit_configs()
move_group_launch = generate_move_group_launch(moveit_config)
rviz_launch = generate_moveit_rviz_launch(moveit_config)
action_server_node = Node(
package="dual_arm_moveit_action_server",
executable="dual_arm_moveit_action_server",
output="screen",
parameters=[
moveit_config.to_dict(),
{
"left_group_name": "LeftArm",
"right_group_name": "RightArm",
"planning_time": 5.0,
"acceleration_scaling": 0.5,
"cartesian_step": 0.01,
"cartesian_jump_threshold": 0.0,
"cartesian_min_fraction": 0.9,
},
],
)
return LaunchDescription(
[
move_group_launch,
rviz_launch,
action_server_node,
]
)

BIN
dual_arm_moveit_action_server/meshes/arm_base_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/base_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/body_2_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/body_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/head_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_arm_1_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_arm_2_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_arm_3_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_arm_4_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_behind_hip_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_behind_leg_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_behind_wheel_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_front_hip_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_front_knee_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_front_roll_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_front_wheel_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_shoulder_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/left_wrist_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link1.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link2.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link3.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link4.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link5.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/link6.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_arm_1_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_arm_2_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_arm_3_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_arm_4_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_behind_hip_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_behind_leg_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_behind_wheel_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_front_hip_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_front_knee_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_front_roll_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_front_wheel_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_shoulder_link.STL Normal file
View File

Binary file not shown.

BIN
dual_arm_moveit_action_server/meshes/right_wrist_link.STL Normal file
View File

Binary file not shown.

31
dual_arm_moveit_action_server/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?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>dual_arm_moveit_action_server</name>
<version>0.0.0</version>
<description>DualArm MoveIt action server for RViz planning display</description>
<maintainer email="root@todo.todo">root</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>interfaces</depend>
<depend>geometry_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_msgs</depend>
<exec_depend>moveit_configs_utils</exec_depend>
<exec_depend>moveit_ros_move_group</exec_depend>
<exec_depend>moveit_ros_visualization</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>rviz2</exec_depend>
<exec_depend>xacro</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

420
dual_arm_moveit_action_server/src/dual_arm_moveit_action_server.cpp Normal file
View File

@@ -0,0 +1,420 @@
#include "dual_arm_moveit_action_server/dual_arm_moveit_action_server.hpp"
#include <thread>
namespace dual_arm_moveit_action_server
{
DualArmMoveitActionServer::DualArmMoveitActionServer(const rclcpp::Node::SharedPtr & node)
: node_(node)
{
}
void DualArmMoveitActionServer::initialize()
{
left_group_name_ = node_->declare_parameter<std::string>("left_group_name", "LeftArm");
right_group_name_ = node_->declare_parameter<std::string>("right_group_name", "RightArm");
planning_time_ = node_->declare_parameter<double>("planning_time", 5.0);
acceleration_scaling_ = node_->declare_parameter<double>("acceleration_scaling", 0.5);
cartesian_step_ = node_->declare_parameter<double>("cartesian_step", 0.01);
cartesian_jump_threshold_ = node_->declare_parameter<double>("cartesian_jump_threshold", 0.0);
cartesian_min_fraction_ = node_->declare_parameter<double>("cartesian_min_fraction", 0.9);
try {
left_move_group_ =
std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, left_group_name_);
right_move_group_ =
std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, right_group_name_);
} catch (const std::exception & e) {
RCLCPP_ERROR(node_->get_logger(), "Failed to create MoveGroupInterface: %s", e.what());
left_move_group_.reset();
right_move_group_.reset();
return;
}
left_move_group_->setPlanningTime(planning_time_);
right_move_group_->setPlanningTime(planning_time_);
display_pub_ = node_->create_publisher<moveit_msgs::msg::DisplayTrajectory>(
"display_planned_path", rclcpp::SystemDefaultsQoS());
action_server_ = rclcpp_action::create_server<DualArm>(
node_,
"DualArm",
std::bind(&DualArmMoveitActionServer::handle_goal, this, std::placeholders::_1,
std::placeholders::_2),
std::bind(&DualArmMoveitActionServer::handle_cancel, this, std::placeholders::_1),
std::bind(&DualArmMoveitActionServer::handle_accepted, this, std::placeholders::_1));
RCLCPP_INFO(node_->get_logger(), "DualArm MoveIt action server is ready");
}
rclcpp_action::GoalResponse DualArmMoveitActionServer::handle_goal(
const rclcpp_action::GoalUUID & /*uuid*/,
std::shared_ptr<const DualArm::Goal> goal)
{
if (!left_move_group_ || !right_move_group_) {
RCLCPP_ERROR(node_->get_logger(), "MoveGroupInterface not initialized");
return rclcpp_action::GoalResponse::REJECT;
}
bool expected = false;
if (!planning_busy_.compare_exchange_strong(expected, true)) {
RCLCPP_WARN(node_->get_logger(), "DualArm rejected: planner is busy");
return rclcpp_action::GoalResponse::REJECT;
}
std::string error_msg;
if (!validate_goal(*goal, &error_msg)) {
RCLCPP_ERROR(node_->get_logger(), "DualArm rejected: %s", error_msg.c_str());
planning_busy_.store(false);
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse DualArmMoveitActionServer::handle_cancel(
const std::shared_ptr<GoalHandleDualArm> /*goal_handle*/)
{
RCLCPP_INFO(node_->get_logger(), "DualArm cancel requested");
return rclcpp_action::CancelResponse::ACCEPT;
}
void DualArmMoveitActionServer::handle_accepted(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
std::thread{&DualArmMoveitActionServer::execute, this, goal_handle}.detach();
}
void DualArmMoveitActionServer::execute(const std::shared_ptr<GoalHandleDualArm> goal_handle)
{
struct BusyReset
{
std::atomic_bool & flag;
~BusyReset() { flag.store(false); }
} busy_reset{planning_busy_};
auto feedback = std::make_shared<DualArm::Feedback>();
auto result = std::make_shared<DualArm::Result>();
result->success = false;
result->final_progress = 0.0;
result->message = "DualArm planning failed";
const auto goal = goal_handle->get_goal();
const auto params_list = normalize_goal_params(*goal);
feedback->status = DualArm::Goal::STATUS_PLANNING;
feedback->progress = 0.0;
fill_feedback_joints(*feedback);
goal_handle->publish_feedback(feedback);
std::vector<moveit_msgs::msg::RobotTrajectory> trajectories;
trajectories.reserve(params_list.size());
for (size_t i = 0; i < params_list.size(); ++i) {
if (goal_handle->is_canceling()) {
result->message = "DualArm canceled";
result->success = false;
result->final_progress = 0.0;
goal_handle->canceled(result);
return;
}
const auto & params = params_list[i];
auto move_group = (params.arm_id == interfaces::msg::ArmMotionParams::ARM_LEFT) ?
left_move_group_ :
right_move_group_;
std::string plan_error;
moveit_msgs::msg::RobotTrajectory trajectory;
if (!plan_for_arm(params, *move_group, goal->velocity_scaling, &trajectory, &plan_error)) {
result->message = plan_error.empty() ? "MoveIt planning failed" : plan_error;
goal_handle->abort(result);
return;
}
publish_display_trajectory(trajectory, *move_group);
trajectories.push_back(trajectory);
feedback->status = (i + 1 == params_list.size()) ?
DualArm::Goal::STATUS_DONE :
DualArm::Goal::STATUS_EXECUTING;
feedback->progress =
static_cast<double>(i + 1) / static_cast<double>(params_list.size());
fill_feedback_joints(*feedback);
goal_handle->publish_feedback(feedback);
}
result->success = true;
result->final_progress = 1.0;
result->message = "DualArm planned successfully and published to RViz";
goal_handle->succeed(result);
}
bool DualArmMoveitActionServer::validate_goal(const DualArm::Goal & goal, std::string * error_msg) const
{
if (goal.velocity_scaling < 0.0 || goal.velocity_scaling > 1.0) {
if (error_msg) {
*error_msg = "velocity_scaling out of range [0.0, 1.0]";
}
return false;
}
if (goal.arm_motion_params.empty() || goal.arm_motion_params.size() > 2) {
if (error_msg) {
*error_msg = "arm_motion_params must contain 1 or 2 items";
}
return false;
}
bool has_left = false;
bool has_right = false;
for (const auto & params : goal.arm_motion_params) {
if (params.arm_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
has_left = true;
} else if (params.arm_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
has_right = true;
} else {
if (error_msg) {
*error_msg = "invalid arm_id (expected 0=left, 1=right)";
}
return false;
}
if (params.motion_type != interfaces::msg::ArmMotionParams::MOVEJ &&
params.motion_type != interfaces::msg::ArmMotionParams::MOVEL)
{
if (error_msg) {
*error_msg = "unsupported motion_type";
}
return false;
}
}
if (goal.arm_motion_params.size() == 2 && (!has_left || !has_right)) {
if (error_msg) {
*error_msg = "dual-arm mode requires both left and right arm_id";
}
return false;
}
return true;
}
std::vector<interfaces::msg::ArmMotionParams> DualArmMoveitActionServer::normalize_goal_params(
const DualArm::Goal & goal) const
{
std::vector<interfaces::msg::ArmMotionParams> params;
params.reserve(goal.arm_motion_params.size());
if (goal.arm_motion_params.size() == 1) {
params.push_back(goal.arm_motion_params.front());
return params;
}
interfaces::msg::ArmMotionParams left;
interfaces::msg::ArmMotionParams right;
bool has_left = false;
bool has_right = false;
for (const auto & item : goal.arm_motion_params) {
if (item.arm_id == interfaces::msg::ArmMotionParams::ARM_LEFT) {
left = item;
has_left = true;
} else if (item.arm_id == interfaces::msg::ArmMotionParams::ARM_RIGHT) {
right = item;
has_right = true;
}
}
if (has_left) {
params.push_back(left);
}
if (has_right) {
params.push_back(right);
}
return params;
}
bool DualArmMoveitActionServer::plan_for_arm(
const interfaces::msg::ArmMotionParams & params,
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
moveit_msgs::msg::RobotTrajectory * out_trajectory,
std::string * error_msg)
{
if (!out_trajectory) {
if (error_msg) {
*error_msg = "Null trajectory output";
}
return false;
}
move_group.setStartStateToCurrentState();
move_group.setMaxVelocityScalingFactor(velocity_scaling);
move_group.setMaxAccelerationScalingFactor(acceleration_scaling_);
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEJ) {
const auto robot_model = move_group.getRobotModel();
const auto * jmg = robot_model ? robot_model->getJointModelGroup(move_group.getName()) : nullptr;
if (!jmg) {
if (error_msg) {
*error_msg = "MoveGroup has no JointModelGroup";
}
return false;
}
const size_t expected = jmg->getVariableCount();
if (params.target_joints.size() != expected) {
if (error_msg) {
*error_msg =
"MOVEJ target_joints size mismatch (expected " + std::to_string(expected) + ")";
}
return false;
}
move_group.setJointValueTarget(params.target_joints);
moveit::planning_interface::MoveGroupInterface::Plan plan;
auto code = move_group.plan(plan);
move_group.clearPoseTargets();
if (code != moveit::core::MoveItErrorCode::SUCCESS) {
if (error_msg) {
*error_msg = "MOVEJ planning failed";
}
return false;
}
if (!time_parameterize_trajectory(move_group, velocity_scaling, plan.trajectory_, error_msg)) {
return false;
}
*out_trajectory = plan.trajectory_;
return true;
}
if (params.motion_type == interfaces::msg::ArmMotionParams::MOVEL) {
std::vector<geometry_msgs::msg::Pose> waypoints;
waypoints.push_back(params.target_pose);
moveit_msgs::msg::RobotTrajectory trajectory;
const double fraction = move_group.computeCartesianPath(
waypoints, cartesian_step_, cartesian_jump_threshold_, trajectory, true);
move_group.clearPoseTargets();
if (fraction < cartesian_min_fraction_) {
if (error_msg) {
*error_msg = "MOVEL cartesian path fraction too low: " + std::to_string(fraction);
}
return false;
}
if (!time_parameterize_trajectory(move_group, velocity_scaling, trajectory, error_msg)) {
return false;
}
*out_trajectory = trajectory;
return true;
}
if (error_msg) {
*error_msg = "Unsupported motion_type";
}
return false;
}
bool DualArmMoveitActionServer::time_parameterize_trajectory(
moveit::planning_interface::MoveGroupInterface & move_group,
double velocity_scaling,
moveit_msgs::msg::RobotTrajectory & trajectory,
std::string * error_msg)
{
const auto current_state = move_group.getCurrentState();
if (!current_state) {
if (error_msg) {
*error_msg = "Failed to get current state for time parameterization";
}
return false;
}
robot_trajectory::RobotTrajectory robot_traj(
move_group.getRobotModel(), move_group.getName());
robot_traj.setRobotTrajectoryMsg(*current_state, trajectory);
trajectory_processing::IterativeParabolicTimeParameterization time_param;
if (!time_param.computeTimeStamps(robot_traj, velocity_scaling, acceleration_scaling_)) {
if (error_msg) {
*error_msg = "Time parameterization failed";
}
return false;
}
robot_traj.getRobotTrajectoryMsg(trajectory);
return true;
}
void DualArmMoveitActionServer::publish_display_trajectory(
const moveit_msgs::msg::RobotTrajectory & trajectory,
moveit::planning_interface::MoveGroupInterface & move_group)
{
if (!display_pub_) {
return;
}
auto current_state = move_group.getCurrentState();
if (!current_state) {
RCLCPP_WARN(node_->get_logger(), "No current state available for display");
return;
}
moveit_msgs::msg::DisplayTrajectory display_msg;
display_msg.model_id = move_group.getRobotModel()->getName();
moveit_msgs::msg::RobotState start_state_msg;
moveit::core::robotStateToRobotStateMsg(*current_state, start_state_msg);
display_msg.trajectory_start = start_state_msg;
display_msg.trajectory.push_back(trajectory);
display_pub_->publish(display_msg);
}
void DualArmMoveitActionServer::fill_feedback_joints(DualArm::Feedback & feedback)
{
feedback.joints_left.clear();
feedback.joints_right.clear();
if (left_move_group_) {
auto state = left_move_group_->getCurrentState();
if (state) {
const auto * jmg = state->getJointModelGroup(left_group_name_);
if (jmg) {
state->copyJointGroupPositions(jmg, feedback.joints_left);
}
}
}
if (right_move_group_) {
auto state = right_move_group_->getCurrentState();
if (state) {
const auto * jmg = state->getJointModelGroup(right_group_name_);
if (jmg) {
state->copyJointGroupPositions(jmg, feedback.joints_right);
}
}
}
}
} // namespace dual_arm_moveit_action_server
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("dual_arm_moveit_action_server");
dual_arm_moveit_action_server::DualArmMoveitActionServer server(node);
server.initialize();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

1803
dual_arm_moveit_action_server/urdf/FHrobot.urdf Normal file
View File

File diff suppressed because it is too large Load Diff

44
include/arm_action_define.h Normal file
View File

@@ -0,0 +1,44 @@
#ifndef ARM_ACTION_DEFINE_H
#define ARM_ACTION_DEFINE_H
#define USED_ARM_DOF (6)
#define GOAL_DATA_LENGTH (7)
#define USED_OTHER_DOF (17)
typedef enum {
OK = 0,
UNKNOWN_ERR = -1,
ARM_NOW_FORCE_MOVING = -2,
ARM_COLLISION = -3,
ARM_AIM_CANNOT_REACH = -4,
ARM_NOW_NO_GOAL = -5,
ARM_GOAL_CANCELLED = -6,
} ErrCodeE;
typedef enum {
ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
ARM_COMMAND_TYPE_POSE_STEP_ON,
ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE,
ARM_COMMAND_TYPE_POSE_DIRECT_MOVE,
ARM_COMMAND_TYPE_ERR
} ArmCommandTypeE;
typedef enum {
POSE_POSITION_X = 0,
POSE_POSITION_Y,
POSE_POSITION_Z,
POSE_QUATERNION_X,
POSE_QUATERNION_Y,
POSE_QUATERNION_Z,
POSE_QUATERNION_W,
POSE_DIMENSION, // should be 7
} PoseDimensionE;
typedef enum {
LEFT_ARM = 0,
RIGHT_ARM,
ARM_ID_ERR
} ArmIdE;
#endif // ARM_ACTION_DEFINE_H