Refactor IK client: move from GraspPoseCalculator to CerebellumNode

This commit is contained in:
NuoDaJia02
2026-01-28 15:29:30 +08:00
parent da61cf8709
commit 9047e71206
15 changed files with 92 additions and 10376 deletions

View File

@@ -43,7 +43,6 @@ add_executable(brain_node
target_include_directories(brain_node PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
interfaces/include
)
@@ -71,7 +70,6 @@ ament_target_dependencies(brain_node
target_link_libraries(brain_node
Boost::thread
yaml-cpp
/usr/local/lib/libapi_cpp.so
)
install(TARGETS brain_node
@@ -174,10 +172,9 @@ if(BUILD_TESTING)
)
target_include_directories(test_cerebellum_node PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebellum_node Boost::thread)
target_link_libraries(test_cerebellum_node yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_cerebellum_node yaml-cpp)
endif()
ament_add_gtest(test_cerebrum_node
@@ -204,10 +201,9 @@ if(BUILD_TESTING)
)
target_include_directories(test_cerebrum_node PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebrum_node Boost::thread)
target_link_libraries(test_cerebrum_node yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_cerebrum_node yaml-cpp)
endif()
ament_add_gtest(test_sm_cerebellum
@@ -228,9 +224,8 @@ if(BUILD_TESTING)
)
target_include_directories(test_sm_cerebellum PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_sm_cerebellum /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_sm_cerebellum)
endif()
# ExecuteBtAction end-to-end single skill test
@@ -257,9 +252,8 @@ if(BUILD_TESTING)
)
target_include_directories(test_execute_bt_action PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
endif()
# Extended ExecuteBtAction scenario tests (multi-skill, failure, RUN interpolation, timeout, stats, cancel)
@@ -286,9 +280,8 @@ if(BUILD_TESTING)
)
target_include_directories(test_execute_bt_action_extended PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp)
endif()
# Additional isolated ExecuteBtAction scenario tests
@@ -305,9 +298,8 @@ if(BUILD_TESTING)
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
target_include_directories(${test_name} PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(${test_name} Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(${test_name} Boost::thread yaml-cpp)
endif()
endforeach()
@@ -323,9 +315,8 @@ if(BUILD_TESTING)
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp nav2_msgs std_srvs tf2 tf2_ros tf2_geometry_msgs)
target_include_directories(test_cerebrum_utils PRIVATE
${CMAKE_CURRENT_SOURCE_DIR}/include
${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/rm_arm/include
)
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp /usr/local/lib/libapi_cpp.so)
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
endif()
endif()

View File

@@ -6,6 +6,7 @@
#include <tf2/LinearMath/Transform.h>
#include <string>
#include <vector>
#include <functional>
namespace brain {
@@ -43,11 +44,6 @@ public:
const std::string& arm = "right",
int top_k = 3
);
static int rm_arm_inverse_kinematics(
const tf2::Vector3& target_pos,
const tf2::Quaternion& target_quat,
std::vector<double>& joint_angles
);
private:
static bool rm_arm_algo_init_done_;
@@ -59,7 +55,6 @@ private:
double safety_dist,
double finger_length
);
static void rm_arm_algo_init(void);
};

View File

@@ -24,6 +24,7 @@
#include "brain/payload_converters.hpp"
#include "brain/robot_config.hpp"
#include "brain/calc_grasp_pose.hpp"
#include "interfaces/srv/inverse_kinematics.hpp"
namespace brain
{
@@ -55,6 +56,17 @@ public:
std::string camera_position_;
std::unordered_map<std::string, std::string> gripper_info_;
/**
* @brief Call Inverse Kinematics service
*
* @param pos
* @param quat
* @param angles
* @param arm_id
* @return int
*/
int CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id);
private:
struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
@@ -73,6 +85,9 @@ private:
// SMACC2 execution context
std::unique_ptr<smacc2::SmExecution> sm_exec_;
// IK Client
rclcpp::Client<interfaces::srv::InverseKinematics>::SharedPtr ik_client_;
// ROS parameters (replacing hardcoded defaults)
bool abort_on_failure_{true};

View File

@@ -8,16 +8,12 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>
#include "arm_action_define.h"
#include "rm_interface.h"
namespace brain {
bool GraspPoseCalculator::rm_arm_algo_init_done_ = false;
GraspPoseCalculator::GraspPoseCalculator()
{
rm_arm_algo_init();
}
tf2::Vector3 normalize_vector(const tf2::Vector3& v) {
@@ -121,43 +117,6 @@ GraspResult GraspPoseCalculator::calculate(
GraspResult result = create_grasp_matrix(target_pos, approach_vec, palm_normal, safety_dist, finger_length);
result.name = name_suffix;
#if ENABLE_TOLERANCE
// Check IK and adjust position if needed (Tolerance +/- 2cm)
std::vector<double> q_out(7, 0.0);
// Try original position first
if (rm_arm_inverse_kinematics(result.grasp_pose.position, result.grasp_pose.orientation, q_out) != 0) {
// Search in +/- 2cm box
bool found = false;
double tolerance = 0.02;
double step = 0.005; // 5mm step
for (double dx = -tolerance; dx <= tolerance + 1e-5; dx += step) {
for (double dy = -tolerance; dy <= tolerance + 1e-5; dy += step) {
for (double dz = -tolerance; dz <= tolerance + 1e-5; dz += step) {
if (std::abs(dx) < 1e-5 && std::abs(dy) < 1e-5 && std::abs(dz) < 1e-5) continue;
tf2::Vector3 new_target = target_pos + tf2::Vector3(dx, dy, dz);
GraspResult new_result = create_grasp_matrix(new_target, approach_vec, palm_normal, safety_dist, finger_length);
new_result.name = name_suffix;
if (rm_arm_inverse_kinematics(new_result.grasp_pose.position, new_result.grasp_pose.orientation, q_out) == 0) {
result = new_result;
found = true;
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"Adjusted grasp target by [%.3f, %.3f, %.3f] to satisfy IK.", dx, dy, dz);
break;
}
}
if (found) break;
}
if (found) break;
}
if (!found) {
RCLCPP_WARN(rclcpp::get_logger("GraspPoseCalculator"), "Could not find valid IK within tolerance.");
}
}
#endif
return result;
}
@@ -240,62 +199,4 @@ std::vector<double> GraspPoseCalculator::find_optimal_grasp_angles(
return result;
}
void GraspPoseCalculator::rm_arm_algo_init(void)
{
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
rm_arm_algo_init_done_ = true;
}
int GraspPoseCalculator::rm_arm_inverse_kinematics(
const tf2::Vector3& target_pos,
const tf2::Quaternion& target_quat,
std::vector<double>& joint_angles
) {
if (!rm_arm_algo_init_done_) {
rm_arm_algo_init();
}
float joints[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
if (joint_angles.size() < USED_ARM_DOF) {
joint_angles.resize(USED_ARM_DOF, 0.0);
}
for (int i = 0; i < USED_ARM_DOF; i++) {
joints[i] = static_cast<float>(joint_angles[i]);
}
rm_inverse_kinematics_params_t ik_params;
memcpy(ik_params.q_in, joints, sizeof(joints));
ik_params.q_pose.position.x = target_pos.x();
ik_params.q_pose.position.y = target_pos.y();
ik_params.q_pose.position.z = target_pos.z();
ik_params.q_pose.quaternion.x = target_quat.x();
ik_params.q_pose.quaternion.y = target_quat.y();
ik_params.q_pose.quaternion.z = target_quat.z();
ik_params.q_pose.quaternion.w = target_quat.w();
ik_params.flag = 0;
#ifdef RM_ALGO_IK_DEBUG
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"IK Target Pos: [%.4f, %.4f, %.4f], Quat: [%.4f, %.4f, %.4f, %.4f]",
ik_params.q_pose.position.x, ik_params.q_pose.position.y, ik_params.q_pose.position.z,
ik_params.q_pose.quaternion.x, ik_params.q_pose.quaternion.y,
ik_params.q_pose.quaternion.z, ik_params.q_pose.quaternion.w);
RCLCPP_INFO(rclcpp::get_logger("GraspPoseCalculator"),
"IK Input Joints: [%.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]);
#endif
int result = rm_algo_inverse_kinematics(NULL, ik_params, joints);
if (result != 0) {
// Downgrade to DEBUG to avoid spamming during search
RCLCPP_DEBUG(rclcpp::get_logger("GraspPoseCalculator"), "Inverse kinematics failed with error code %d", result);
return ARM_AIM_CANNOT_REACH;
}
for (int i = 0; i < USED_ARM_DOF; i++) {
joint_angles[i] = static_cast<double>(joints[i]);
}
return 0;
}
} // namespace brain

View File

@@ -699,20 +699,12 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// return false;
// }
std::vector<double> joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if(target_arm == "right_arm") {
joint_angles = node->right_arm_joint_angles_;
} else if (target_arm == "left_arm") {
joint_angles = node->left_arm_joint_angles_;
}
}
std::vector<double> joint_angles(7, 0.0);
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(target_pos, target_quat, joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "%s take_photo_pose rm arm inverse kinematics failed.", target_arm.c_str());
int arm_id = (target_arm == "left_arm") ? 0 : 1;
if (node->CallInverseKinematics(target_pos, target_quat, joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d take_photo_pose rm arm inverse kinematics failed.", arm_id);
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
@@ -737,16 +729,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
//left & right cam
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if (node->camera_position_ == "right") {
current_joint_angles = node->right_arm_joint_angles_;
node->arm_target_frame_ = "base_link_rightarm";
} else if (node->camera_position_ == "left") {
current_joint_angles = node->left_arm_joint_angles_;
node->arm_target_frame_ = "base_link_leftarm";
}
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if (node->camera_position_ == "right") {
node->arm_target_frame_ = "base_link_rightarm";
} else if (node->camera_position_ == "left") {
node->arm_target_frame_ = "base_link_leftarm";
}
geometry_msgs::msg::PoseStamped pose_in_arm;
@@ -774,6 +761,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// best_angles.clear();
std::vector<double> best_angles = node->grasp_best_angles_;
int arm_id = ((node->camera_position_ == "left") ? 0 : 1);
{
std::vector<std::string> palm_facings = node->grasp_palm_facings_;
@@ -806,30 +794,30 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
std::vector<double> pre_grasp_joint_angles(7, 0.0);
if (node->CallInverseKinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//grasp
std::vector<double> grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
std::vector<double> grasp_joint_angles(7, 0.0);
if (node->CallInverseKinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}
//take object
auto take_object_pose = result.grasp_pose;
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(node->take_object_arm_x_); //向上拿起物体
std::vector<double> take_object_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
std::vector<double> take_object_joint_angles(7, 0.0);
if (node->CallInverseKinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles, arm_id) != 0) {
RCLCPP_ERROR(node->get_logger(), "arm_id %d, [%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
arm_id, skill_name.c_str(), angle, palm.c_str());
continue;
}

View File

@@ -74,6 +74,8 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
share_directory_(ament_index_cpp::get_package_share_directory("brain"))
{
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
ik_client_ = this->create_client<interfaces::srv::InverseKinematics>("inverse_kinematics");
// Use parameter-based path if available, otherwise default to package share directory
std::string config_path = robot_config_path_.empty()
@@ -1431,4 +1433,46 @@ void CerebellumNode::JointStateCallback(const sensor_msgs::msg::JointState::Shar
}
}
int CerebellumNode::CallInverseKinematics(const tf2::Vector3& pos, const tf2::Quaternion& quat, std::vector<double>& angles, int arm_id)
{
if (!ik_client_->wait_for_service(std::chrono::seconds(1))) {
RCLCPP_ERROR(this->get_logger(), "Service inverse_kinematics not available");
return -1;
}
auto request = std::make_shared<interfaces::srv::InverseKinematics::Request>();
request->arm_id = arm_id;
request->pose.position.x = pos.x();
request->pose.position.y = pos.y();
request->pose.position.z = pos.z();
request->pose.orientation.x = quat.x();
request->pose.orientation.y = quat.y();
request->pose.orientation.z = quat.z();
request->pose.orientation.w = quat.w();
auto result_future = ik_client_->async_send_request(request);
// Wait for result. Warning: potentially blocking if single-threaded executor.
if (result_future.wait_for(std::chrono::seconds(2)) == std::future_status::ready) {
auto result = result_future.get();
if (result->result == 0) {
if (angles.size() < result->joint_angles.size()) {
angles.resize(result->joint_angles.size());
}
for (size_t i = 0; i < result->joint_angles.size(); ++i) {
angles[i] = result->joint_angles[i];
}
RCLCPP_INFO(this->get_logger(), "arm_id %d Inverse kinematics service SUCCESS returned angles: [%f, %f, %f, %f, %f, %f]", arm_id,
angles[0], angles[1], angles[2], angles[3], angles[4], angles[5]);
return 0;
} else {
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service returned failure: %d", arm_id, result->result);
return -1;
}
} else {
RCLCPP_ERROR(this->get_logger(), "arm_id %d Inverse kinematics service call timed out", arm_id);
return -1;
}
}
} // namespace brain

View File

@@ -1,65 +0,0 @@
#ifndef ARM_DEFINE_H
#define ARM_DEFINE_H
#include "arm_action_define.h"
#define MAX_ARM_GOAL_COUNT 16
#define MAX_TRAJECTORY_HISTORY 64
#define ARM_FOLLOWING_PERIOD (5e6) //ns == 5ms
#define ARM_FOLLOWING_CHECKING_STEP (4)
#define ARM_FOLLOWING_CHECKING (ARM_FOLLOWING_PERIOD * ARM_FOLLOWING_CHECKING_STEP) //ns == 20ms
#define GET_FUNC_LINE_STR() \
(std::string(__func__ ) + ":" + std::to_string(__LINE__))
#define LEFT_ARM_IP_ADDRESS "192.168.2.2"
#define RIGHT_ARM_IP_ADDRESS "192.168.2.18"
#define ARM_IP_PORT (8080)
#define TRAJECTORY_COMPLETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1
/*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;
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 {
GOAL_TYPE_MOVING = 0,
GOAL_TYPE_STEPPING,
GOAL_TYPE_POSE_STEPPING,
GOAL_TYPE_ERROR
} GoalTypeE;
#endif // ARM_DEFINE_H

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@@ -1,16 +0,0 @@
#ifndef RM_INTERFACE_GLOBAL_H
#define RM_INTERFACE_GLOBAL_H
#ifdef __linux
#define RM_INTERFACE_EXPORT
#endif
#if _WIN32
#if defined(RM_INTERFACE_LIBRARY)
# define RM_INTERFACE_EXPORT __declspec(dllexport)
#else
# define RM_INTERFACE_EXPORT __declspec(dllexport)
#endif
#endif
#endif // RM_INTERFACE_GLOBAL_H

File diff suppressed because it is too large Load Diff

View File

@@ -1,16 +0,0 @@
#ifndef RM_VERSION_H
#define RM_VERSION_H
#ifdef __cplusplus
extern "C" {
#endif
#define SDK_VERSION ("1.1.1")
#ifdef __cplusplus
}
#endif
#endif

Binary file not shown.

Binary file not shown.

Binary file not shown.