Refactor IK client: move from GraspPoseCalculator to CerebellumNode
This commit is contained in:
@@ -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()
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
65
src/thirdparty/rm_arm/include/arm_define.h
vendored
65
src/thirdparty/rm_arm/include/arm_define.h
vendored
@@ -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
|
||||
1035
src/thirdparty/rm_arm/include/rm_define.h
vendored
1035
src/thirdparty/rm_arm/include/rm_define.h
vendored
File diff suppressed because it is too large
Load Diff
4533
src/thirdparty/rm_arm/include/rm_interface.h
vendored
4533
src/thirdparty/rm_arm/include/rm_interface.h
vendored
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
4553
src/thirdparty/rm_arm/include/rm_service.h
vendored
4553
src/thirdparty/rm_arm/include/rm_service.h
vendored
File diff suppressed because it is too large
Load Diff
16
src/thirdparty/rm_arm/include/rm_version.h
vendored
16
src/thirdparty/rm_arm/include/rm_version.h
vendored
@@ -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
|
||||
|
||||
BIN
src/thirdparty/rm_arm/lib/api_cpp.dll
vendored
BIN
src/thirdparty/rm_arm/lib/api_cpp.dll
vendored
Binary file not shown.
BIN
src/thirdparty/rm_arm/lib/api_cpp.lib
vendored
BIN
src/thirdparty/rm_arm/lib/api_cpp.lib
vendored
Binary file not shown.
BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so
vendored
BIN
src/thirdparty/rm_arm/lib/libapi_cpp.so
vendored
Binary file not shown.
Reference in New Issue
Block a user