diff --git a/arm_keyboard_input/include/arm_keyboard_node.hpp b/arm_keyboard_input/include/arm_keyboard_node.hpp index da0c4f1..5c96ae8 100644 --- a/arm_keyboard_input/include/arm_keyboard_node.hpp +++ b/arm_keyboard_input/include/arm_keyboard_node.hpp @@ -5,7 +5,7 @@ #include "std_msgs/msg/float64_multi_array.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "interfaces/action/arm.hpp" -#include "arm_define.h" +#include "arm_action_define.h" #include "rm_define.h" using ArmAction = interfaces::action::Arm; diff --git a/arm_keyboard_input/src/arm_keyboard_node.cpp b/arm_keyboard_input/src/arm_keyboard_node.cpp index 241cb8d..23dcf1d 100644 --- a/arm_keyboard_input/src/arm_keyboard_node.cpp +++ b/arm_keyboard_input/src/arm_keyboard_node.cpp @@ -365,9 +365,9 @@ void ArmKeyboardNode::SendDirectSteppingGoal2(const float *values, int armId) int64_t timeStamp = get_clock()->now().nanoseconds(); auto goal_msg = ArmAction::Goal(); commandId++; - goal_msg.body_id.body_id = armId; - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制 - goal_msg.data_length.data_length = POSE_DIMENSION; + goal_msg.body_id = armId; + goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制 + goal_msg.data_length = POSE_DIMENSION; goal_msg.frame_time_stamp = timeStamp; goal_msg.command_id = commandId; for (int i = 0; i < POSE_DIMENSION; ++i) { @@ -389,9 +389,9 @@ void ArmKeyboardNode::SendDirectMovingGoal(const float *values, int armId) int64_t timeStamp = get_clock()->now().nanoseconds(); auto goal_msg = ArmAction::Goal(); commandId++; - goal_msg.body_id.body_id = armId; - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制 - goal_msg.data_length.data_length = POSE_DIMENSION; + goal_msg.body_id = armId; + goal_msg.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制 + goal_msg.data_length = POSE_DIMENSION; goal_msg.frame_time_stamp = timeStamp; goal_msg.command_id = commandId; for (int i = 0; i < POSE_DIMENSION; ++i) { @@ -414,9 +414,9 @@ void ArmKeyboardNode::SendAngleMovingGoal(const float *values, int armId) auto goal_msg = ArmAction::Goal(); commandId++; RCLCPP_INFO(this->get_logger(), "SendAngleMovingGoal id=%ld", commandId); - goal_msg.body_id.body_id = armId; - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制 - goal_msg.data_length.data_length = USED_ARM_DOF; + goal_msg.body_id = armId; + goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制 + goal_msg.data_length = USED_ARM_DOF; goal_msg.frame_time_stamp = timeStamp; goal_msg.command_id = commandId; for (int i = 0; i < USED_ARM_DOF; ++i) { @@ -648,8 +648,8 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command) speedadd = -0.01; } - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制 - goal_msg.data_length.data_length = POSE_DIMENSION; + goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制 + goal_msg.data_length = POSE_DIMENSION; goal_msg.command_id = commandId; for (int i = 0; i < POSE_DIMENSION; ++i) { if (command[directCommandMap[i]]) { @@ -662,7 +662,7 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command) if (command[12] && leftDirectCommand_!= commandValueBitMap) { commandId++; - goal_msg.body_id.body_id = LEFT_ARM; + goal_msg.body_id = LEFT_ARM; leftDirectCommand_ = commandValueBitMap; if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) { RCLCPP_INFO(this->get_logger(), "cancel left arm goal. command id: %ld", leftCommandId_); @@ -682,7 +682,7 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command) } } else if (command[13] && rightDirectCommand_!= commandValueBitMap) { commandId++; - goal_msg.body_id.body_id = RIGHT_ARM; + goal_msg.body_id = RIGHT_ARM; rightDirectCommand_ = commandValueBitMap; if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) { RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_); @@ -780,9 +780,9 @@ void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) { auto goal_msg = ArmAction::Goal(); commandId++; leftCommandId_ = commandId; - goal_msg.body_id.body_id = LEFT_ARM; - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制 - goal_msg.data_length.data_length = USED_ARM_DOF; + goal_msg.body_id = LEFT_ARM; + goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制 + goal_msg.data_length = USED_ARM_DOF; goal_msg.command_id = commandId; for (int i = 0; i < USED_ARM_DOF; ++i) { goal_msg.data_array.push_back(leftArmAngleAim_[i]); @@ -802,9 +802,9 @@ void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) { auto goal_msg = ArmAction::Goal(); commandId++; rightCommandId_ = commandId; - goal_msg.body_id.body_id = RIGHT_ARM; - goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制 - goal_msg.data_length.data_length = USED_ARM_DOF; + goal_msg.body_id = RIGHT_ARM; + goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制 + goal_msg.data_length = USED_ARM_DOF; goal_msg.command_id = commandId; for (int i = 0; i < USED_ARM_DOF; ++i) { diff --git a/include/arm_define.h b/include/arm_define.h index 73f2855..353fd45 100644 --- a/include/arm_define.h +++ b/include/arm_define.h @@ -1,9 +1,7 @@ #ifndef ARM_DEFINE_H #define ARM_DEFINE_H -#define USED_ARM_DOF (6) -#define GOAL_DATA_LENGTH (7) -#define USED_OTHER_DOF (17) +#include "arm_action_define.h" #define MAX_ARM_GOAL_COUNT 16 #define MAX_TRAJECTORY_HISTORY 64 @@ -21,7 +19,7 @@ #define TRAJECTORY_ONGOING 0 #define TRAJECTORY_ERROR -1 -typedef enum { +/*typedef enum { ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0, ARM_COMMAND_TYPE_POSE_STEP_ON, ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE, @@ -45,13 +43,7 @@ typedef enum { RIGHT_ARM, ARM_ID_ERR } ArmIdE; - -typedef enum { - GOAL_TYPE_MOVING = 0, - GOAL_TYPE_STEPPING, - GOAL_TYPE_POSE_STEPPING, - GOAL_TYPE_ERROR -} GoalTypeE; + typedef enum { OK = 0, @@ -61,6 +53,13 @@ typedef enum { ARM_AIM_CANNOT_REACH = -4, ARM_NOW_NO_GOAL = -5, ARM_GOAL_CANCELLED = -6, -} ErrCodeE; +} ErrCodeE;*/ + +typedef enum { + GOAL_TYPE_MOVING = 0, + GOAL_TYPE_STEPPING, + GOAL_TYPE_POSE_STEPPING, + GOAL_TYPE_ERROR +} GoalTypeE; #endif // ARM_DEFINE_H \ No newline at end of file diff --git a/rm_arm_control/CMakeLists.txt b/rm_arm_control/CMakeLists.txt index 41b9aa0..1375ceb 100644 --- a/rm_arm_control/CMakeLists.txt +++ b/rm_arm_control/CMakeLists.txt @@ -10,6 +10,11 @@ if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID STREQUAL "Clang") endif() add_compile_options(-g) +# 全局设置 C++ 编译选项 +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") +# 全局设置 C 编译选项 +# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") + set(ROS_DOMAIN_ID_STR $ENV{ROS_DOMAIN_ID}) if(NOT ROS_DOMAIN_ID_STR) set(ROS_DOMAIN_ID_STR "0") # 未设置时默认 0 @@ -54,7 +59,6 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径 ) - # 设置源文件 set(SOURCES src/rm_arm_handler.cpp diff --git a/rm_arm_control/include/col_simulator.hpp b/rm_arm_control/include/col_simulator.hpp new file mode 100644 index 0000000..b1b3643 --- /dev/null +++ b/rm_arm_control/include/col_simulator.hpp @@ -0,0 +1,97 @@ +#ifndef COL_SIMULATOR_HPP +#define COL_SIMULATOR_HPP + +#include +#include +#include +#include +#include "arm_define.h" +#include "arm_driver_define.hpp" +#include "gjk_interface.hpp" + +#define COLLISION_CHECK_SIMPLE_CNT 2 + +typedef enum { + BODY_TYPE_LEFT_ARM = 0, + BODY_TYPE_RIGHT_ARM, + BODY_TYPE_OTHERS, + BODY_CNT +} BodyListE; + +typedef enum { + FRAME_CHANGE_TYPE_ARM_BASE_TO_ORI, + FRAME_CHANGE_TYPE_ORI_TO_ARM_BASE, + FRAME_CHANGE_TYPE_ARM_END_TO_ORI, + FRAME_CHANGE_TYPE_ORI_TO_ARM_END, + FRAME_CHANGE_TYPE_ERR +} FrameChangeTypeE; + +typedef std::vector Polyhedron; + +struct JointInfo { + std::string name; // 关节名称 + int type; // 关节类型 + int parent_link; // 父连杆 + int child_link; // 子连杆 + Eigen::Quaterniond rotationAll; + Eigen::Vector3d axis; // 关节轴 + Eigen::Vector3d offset; // 关节偏移 + Eigen::Quaterniond rotation; // 关节旋转 + double limit_lower; + double limit_upper; + double limit_v; + double limit_e; + double angle; // 关节角度 +}; + +struct CollisionStructure { + std::string link_name; // 所属链接名称 + std::string geometry_type; // 几何类型 + std::vector dimensions; // 几何尺寸/缩放因子 + std::vector origin_xyz; // 原点坐标 + std::vector origin_rpy; // 原点姿态 + std::string mesh_filename; // mesh文件路径（如STL） + std::vector mesh_bounds; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z] + Polyhedron origin_points; + JointInfo *joint_list; + int joint_count; +}; + +class CollisionSimulator +{ +public: + CollisionSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float **jointsList, KinematicsManager **trajectory); + CollisionSimulator(std::string urdf_string, float **jointsList, KinematicsManager **trajectory); + ~CollisionSimulator(); + + int CheckCollision(); + int UpdateCollisionForNewGoal(int bodyType); + int AddPolyhedron(int id, const Polyhedron& poly); + int RemovePolyhedron(int id); + +private: + void InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory); + int CheckJointsAngleLimit(float *joints, int bodyType); + int InitPolyhedrons(const urdf::Model &urdf_model); + void InitArmSinglePolyhedron(int index, Polyhedron *poly, Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation); + void UpdatePolyhedrons(int startLinkIndex, int endLinkIndex, int simple, float *jointList); + + std::pair GetBodyTransform(int startIndex); + + void InitArmPolyhedronsList(); + void UpdateArmPolyhedrons(); + + KinematicsManager *trajectory_[BODY_CNT]; + std::vector polyhedrons_[COLLISION_CHECK_SIMPLE_CNT]; + std::vector addedOBBs_[COLLISION_CHECK_SIMPLE_CNT]; + + std::vector collision_structures_; + std::vector jointMap_; + + int simpleUpdateIndex_; + float *joints_[COLLISION_CHECK_SIMPLE_CNT][BODY_CNT]; + + std::unordered_map addedPolyhedrons_; +}; + +#endif // COL_SIMULATOR_HPP \ No newline at end of file diff --git a/rm_arm_control/simulator/col_simulator.cpp b/rm_arm_control/simulator/col_simulator.cpp new file mode 100644 index 0000000..ca8600e --- /dev/null +++ b/rm_arm_control/simulator/col_simulator.cpp @@ -0,0 +1,544 @@ +#include +#include +#include +#include +#include +#include +#include "col_simulator.hpp" + +using namespace Eigen; +const int g_bodyLinkCnt[BODY_CNT] = {USED_ARM_DOF + 1, USED_ARM_DOF + 1, USED_OTHER_DOF + 1}; +const int g_jointLinkCnt[BODY_CNT] = {USED_ARM_DOF, USED_ARM_DOF, USED_OTHER_DOF}; +float g_leftArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0}; +float g_rightArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0}; +float g_otherJoints[COLLISION_CHECK_SIMPLE_CNT][USED_OTHER_DOF] = {0}; + +#define POLYHEDRONS_POINTS_CNT (8) + +enum OriginBoxIndex { + LEFT_ARM_ORIGIN = 0, + LEFT_ARM_UPDATE_ORIGIN, + RIGHT_ARM_ORIGIN = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM], + RIGHT_ARM_UPDATE_ORIGIN, + OTHER_ORIGIN = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM] + g_bodyLinkCnt[BODY_TYPE_RIGHT_ARM], + BASE = g_bodyLinkCnt[BODY_TYPE_LEFT_ARM] + g_bodyLinkCnt[BODY_TYPE_RIGHT_ARM] + g_bodyLinkCnt[BODY_TYPE_OTHERS], +}; + +const int g_linkStartIndex[BODY_CNT + 1] = { + LEFT_ARM_UPDATE_ORIGIN, + RIGHT_ARM_UPDATE_ORIGIN, + OTHER_ORIGIN, + BASE +}; + +#define ROOT (BASE - 1) + +bool g_linkCollisionDetecMatrix[OTHER_ORIGIN][BASE] = {false}; + +Vector3d g_origin[BASE] = {Vector3d::Zero()}; +Vector3d g_offset[BASE] = {Vector3d::Zero()}; +Vector3d g_dir[BASE] = {Vector3d::Zero()}; + +static double toRadians(float degrees) { + return (static_cast(degrees)) * (M_PI / 180.0); +} + +static int UpdateJointInfo(int i, int parent, Quaterniond &rotation, std::vector &jointMap) +{ + int key = (parent << 8) | (i % 256); + auto jointPair = jointMap.find(key); + if (jointPair == jointMap.end()) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "lose joint %d -> %d", parent, i); + return -1; + } + + Vector3d position = rotation * jointPair->second.offset; + g_origin[i] = position; + + rotation = rotation * jointPair->second.rotation; + jointPair->second.rotationAll = rotation; + Eigen::Vector3d axis = jointPair->second.axis; + g_dir[i] = rotation * axis; + return 0; +} + +static int InitJoint(const urdf::Model &urdf_model, std::vector &jointMap) +{ + for (const auto& joint_pair : urdf_model.joints_) { + JointInfo jointInfo; + // joint_pair是一个键值对: 键为关节名称，值为关节指针 + jointInfo.name = joint_pair.first; + urdf::JointConstSharedPtr joint = joint_pair.second; + + jointInfo.type = joint->type; + jointInfo.parent_link = -1; + jointInfo.child_link = -1; + auto link = g_linkMap.find(joint->parent_link_name); + if (link != g_linkMap.end()) { + jointInfo.parent_link = link->second; + } else { + continue; + } + link = g_linkMap.find(joint->child_link_name); + if (link != g_linkMap.end()) { + jointInfo.child_link = link->second; + } else { + continue; + } + jointInfo.offset = Vector3d(joint->parent_to_joint_origin_transform.position.x, + joint->parent_to_joint_origin_transform.position.y, + joint->parent_to_joint_origin_transform.position.z); + jointInfo.rotation = Quaterniond( + joint->parent_to_joint_origin_transform.rotation.w, + joint->parent_to_joint_origin_transform.rotation.x, + joint->parent_to_joint_origin_transform.rotation.y, + joint->parent_to_joint_origin_transform.rotation.z); + if (joint->limits != NULL) { + jointInfo.limit_lower = joint->limits->lower; + jointInfo.limit_upper = joint->limits->upper; + jointInfo.limit_v = joint->limits->velocity; + jointInfo.limit_e = joint->limits->effort; + } + jointInfo.angle = 0; + jointInfo.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z); + + int key = ((jointInfo.parent_link % 256) << 8) | (jointInfo.child_link % 256); + jointMap[key] = jointInfo; + } + Quaterniond rotation = Quaterniond::Identity(); + int result = UpdateJointInfo(LEFT, BASE, rotation, jointMap); + if (result != 0) return -1; + for (int i = LEFT + 1; i < RIGHT; ++i) { + result = UpdateJointInfo(i, i - 1, rotation, jointMap); + if (result != 0) return -1; + } + rotation = Quaterniond::Identity(); + result = UpdateJointInfo(RIGHT, BASE, rotation, jointMap); + if (result != 0) return -1; + for (int i = RIGHT + 1; i < BASE; ++i) { + result = UpdateJointInfo(i, i - 1, rotation, jointMap); + if (result != 0) return -1; + } + return 0; +} + +int CollisionSimulator::InitPolyhedrons(const urdf::Model &urdf_model) +{ + std::vector collision_structures_temp; + InitJoint(urdf_model, jointMap_); + InitCollisionStructure(urdf_model, collision_structures_temp); + return RemakeCollisionStructureList(collision_structures_temp); +} + +void CollisionSimulator::InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory) +{ + for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { + joints_[i][BODY_TYPE_LEFT_ARM] = g_leftArmJoints[i]; + joints_[i][BODY_TYPE_RIGHT_ARM] = g_rightArmJoints[i]; + joints_[i][BODY_TYPE_OTHERS] = g_otherJoints[i]; + for (int j = 0; j < BODY_CNT; j++) { + for (int k = 0; k < g_jointLinkCnt[j]; k++) { + joints_[i][j][k] = jointsList[j][k]; + } + } + } + + InitPolyhedrons(urdf_model); + InitArmPolyhedronsList(); + UpdateArmPolyhedrons(); +} + +CollisionSimulator::CollisionSimulator(std::string urdf_string, float **jointsList, KinematicsManager **trajectory) +{ + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块..."); + urdf::Model urdf_model; + simpleUpdateIndex_ = 0; + if (!urdf_model.initString(urdf_string)) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法解析URDF模型"); + return; + } + InitCollisionSimulator(urdf_model, jointsList, trajectory); +} + +CollisionSimulator::CollisionSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float **jointsList, KinematicsManager **trajectory) +{ + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块..."); + std::string urdf_string = urdf_string_ptr->data; + + // 解析URDF模型 + urdf::Model urdf_model; + simpleUpdateIndex_ = 0; + if (!urdf_model.initString(urdf_string)) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法解析URDF模型"); + return; + } + + InitCollisionSimulator(urdf_model, jointsList, trajectory); +} + +void CollisionSimulator::InitArmSinglePolyhedron(int index, Polyhedron *poly, + Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation) +{ + float length = collision_structures_[index].dimensions[0]; // 长度（沿u方向） + float width = collision_structures_[index].dimensions[1]; // 宽度（沿v方向） + float height = collision_structures_[index].dimensions[2]; // 高度（沿法向量方向） + + Vector3d lenVec = rotation * Vector3d(length, 0, 0); // 长度方向 + Vector3d widVec = rotation * Vector3d(0, width, 0); // 宽度方向 + Vector3d heiVec = rotation * Vector3d(0, 0, height); // 高度方向 + + Vector3d bottomLeft = rotation * offset + origin; + + (*poly)[0] = bottomLeft; + (*poly)[1] = bottomLeft + lenVec; + (*poly)[2] = bottomLeft + widVec; + (*poly)[3] = (*poly)[1] + widVec; + (*poly)[4] = bottomLeft + heiVec; + (*poly)[5] = (*poly)[1] + heiVec; + (*poly)[6] = (*poly)[2] + heiVec; + (*poly)[7] = (*poly)[3] + heiVec; + if (index < BASE - 1) { + origin = origin + rotation * g_origin[index + 1]; + } +} + +static int CheckCollisionInner(std::vector *addedOBBs, std::vector *addedOBBsNext_) +{ + for (int i = g_linkStartIndex[LEFT_ARM_ORIGIN]; i < g_linkStartIndex[OTHER_ORIGIN]; ++i) { + for (int j = i + 1; j < BASE; ++j) { + if (!g_linkCollisionDetecMatrix[i][j]) { + continue; + } + bool inCollision = gjk_interface::checkOBBCollision((*addedOBBsNext_)[i], (*addedOBBsNext_)[j]); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect left %d and %d collision b", i, j); + return ARM_COLLISION; + } + inCollision = gjk_interface::checkOBBCollision((*addedOBBs)[i], (*addedOBBsNext_)[j]); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect left %d and %d collision c", i, j); + return ARM_COLLISION; + } + } + } + return OK; +} + +std::pair CollisionSimulator::GetBodyTransform(int startIndex) +{ + Vector3d offset_vec; + Matrix3d rotation = Matrix3d::Identity(); + + std::stack indexStack; + int bodyIndex = startIndex; + + while (bodyIndex != ROOT) { + indexStack.push(bodyIndex); + bodyIndex = jointMap_[bodyIndex].parent_link; + } + + while (!indexStack.empty()) { + int currentIndex = indexStack.top(); + indexStack.pop(); + if (jointMap_[currentIndex].type == urdf::Joint::FIXED) { + offset_vec = offset_vec + rotation * g_origin[currentIndex]; + } else if (jointMap_[currentIndex].type == urdf::Joint::REVOLUTE) { + double angleRad = toRadians(jointMap_[currentIndex].angle); + rotation = AngleAxisd(angleRad, rotation * g_dir[currentIndex]) * rotation; + offset_vec = offset_vec + rotation * g_origin[currentIndex]; + } else if (jointMap_[currentIndex].type == urdf::Joint::PRISMATIC) { + double displacement = jointMap_[currentIndex].angle; + offset_vec = offset_vec + rotation * (g_origin[currentIndex] + g_dir[currentIndex] * displacement); + } + } + return { offset_vec, rotation }; +} + +void CollisionSimulator::UpdatePolyhedrons(int startLinkIndex, int endLinkIndex, int simple, float *jointList) +{ + std::vector *armPolyhedrons = &(polyhedrons_[simple]); + auto [offset_vec, rotation] = GetBodyTransform(startLinkIndex); + for (int i = startLinkIndex + 1; i < endLinkIndex; i++) { + double angleRad = toRadians(jointList[i - startLinkIndex - 1]); + rotation = AngleAxisd(angleRad, rotation * g_dir[i]) * rotation; + InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i], + offset_vec, + g_offset[i], + rotation); + addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i], + collision_structures_[i].dimensions[0], + collision_structures_[i].dimensions[1], + collision_structures_[i].dimensions[2]); + } +} + +void CollisionSimulator::InitArmPolyhedronsList() +{ + for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { + polyhedrons_[i].resize(BASE); + addedOBBs_[i].resize(BASE); + polyhedrons_[i][BASE].clear(); + } + + /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "origin:"); + for (int i = 0; i < BASE; ++i) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i, g_origin[i].x(), g_origin[i].y(), g_origin[i].z()); + } + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "offset:"); + for (int i = 0; i < BASE; ++i) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i, g_offset[i].x(), g_offset[i].y(), g_offset[i].z()); + }*/ + + for (int simple = 0; simple < COLLISION_CHECK_SIMPLE_CNT; simple++) { + for (int i = 0; i < BASE; i++) { + for (int j = 0; j < POLYHEDRONS_POINTS_CNT; j++) { + polyhedrons_[simple][i].push_back(Vector3d(0, 0, 0)); + } + } + } + for (int simple = 0; simple < COLLISION_CHECK_SIMPLE_CNT; simple++) { + std::vector *armPolyhedrons = &(polyhedrons_[simple]); + Vector3d offset_temp = {0, 0 ,0}; + Vector3d origin_point_offset = {0, 0 ,0}; + if (collision_structures_[ROOT].geometry_type == "box") { + origin_point_offset = Vector3d(-collision_structures_[ROOT].dimensions[0] / 2, + -collision_structures_[ROOT].dimensions[1] / 2, + -collision_structures_[ROOT].dimensions[2] / 2); + } else if (collision_structures_[ROOT].geometry_type == "mesh") { + origin_point_offset = Vector3d(collision_structures_[ROOT].mesh_bounds[0], + collision_structures_[ROOT].mesh_bounds[2], + collision_structures_[ROOT].mesh_bounds[4]); + } + Matrix3d rotation = Matrix3d::Identity(); + InitArmSinglePolyhedron(ROOT, &(*armPolyhedrons)[ROOT], + offset_temp, + origin_point_offset, + rotation); + addedOBBs_[simple][ROOT] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[ROOT], + collision_structures_[ROOT].dimensions[0], + collision_structures_[ROOT].dimensions[1], + collision_structures_[ROOT].dimensions[2]); + + Vector3d offset_vec = g_origin[LEFT]; + for (int i = LEFT; i < RIGHT; i++) { + InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i], + offset_vec, + g_offset[i], + rotation); + addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i], + collision_structures_[i].dimensions[0], + collision_structures_[i].dimensions[1], + collision_structures_[i].dimensions[2]); + } + + offset_vec = g_origin[RIGHT]; + for (int i = RIGHT; i < BASE; i++) { + InitArmSinglePolyhedron(i, &(*armPolyhedrons)[i], + offset_vec, + g_offset[i], + rotation); + addedOBBs_[simple][i] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[i], + collision_structures_[i].dimensions[0], + collision_structures_[i].dimensions[1], + collision_structures_[i].dimensions[2]); + } + } + for (int i = LEFT_ARM_ORIGIN; i < BASE; ++i) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i); + for (int j = 0; j < POLYHEDRONS_POINTS_CNT; ++j) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j, + polyhedrons_[0][i][j].x(), + polyhedrons_[0][i][j].y(), + polyhedrons_[0][i][j].z()); + } + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------"); + } +} + +void CollisionSimulator::UpdateArmPolyhedrons() +{ + std::vector *armPolyhedrons = &(polyhedrons_[simpleUpdateIndex_]); + + UpdatePolyhedrons(LEFT_ARM_UPDATE_ORIGIN, RIGHT_ARM_ORIGIN, simpleUpdateIndex_, + joints_[simpleUpdateIndex_][BODY_TYPE_LEFT_ARM]); + UpdatePolyhedrons(RIGHT_ARM_UPDATE_ORIGIN, OTHER_ORIGIN, simpleUpdateIndex_, + joints_[simpleUpdateIndex_][BODY_TYPE_RIGHT_ARM]); + + /*for (int i = LEFT; i <= BASE; ++i) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i); + for (int j = 0; j < POLYHEDRONS_POINTS_CNT; ++j) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j, + polyhedrons_[0][i][j].x(), + polyhedrons_[0][i][j].y(), + polyhedrons_[0][i][j].z()); + } + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------"); + } + for (int i = LEFT; i <= BASE; i++) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), " + "half_length(%.4f, %.4f, %.4f)", + addedOBBs_[simple][i].center.x(), + addedOBBs_[simple][i].center.y(), + addedOBBs_[simple][i].center.z(), + addedOBBs_[simple][i].axis[0].x(), + addedOBBs_[simple][i].axis[0].y(), + addedOBBs_[simple][i].axis[0].z(), + addedOBBs_[simple][i].axis[1].x(), + addedOBBs_[simple][i].axis[1].y(), + addedOBBs_[simple][i].axis[1].z(), + addedOBBs_[simple][i].axis[2].x(), + addedOBBs_[simple][i].axis[2].y(), + addedOBBs_[simple][i].axis[2].z(), + addedOBBs_[simple][i].halfExtent[0], + addedOBBs_[simple][i].halfExtent[1], + addedOBBs_[simple][i].halfExtent[2]); + } + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "left joints: %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", + leftArmJoints_[simple][0], + leftArmJoints_[simple][1], + leftArmJoints_[simple][2], + leftArmJoints_[simple][3], + leftArmJoints_[simple][4], + leftArmJoints_[simple][5]);*/ +} + +int CollisionSimulator::AddPolyhedron(int id, const Polyhedron& poly) +{ + if (addedPolyhedrons_.find(id) != addedPolyhedrons_.end()) { + return -1; // 已存在相同ID的多面体 + } + addedPolyhedrons_[id] = poly; + return 0; // 成功添加 +} + +int CollisionSimulator::RemovePolyhedron(int id) +{ + auto it = addedPolyhedrons_.find(id); + if (it != addedPolyhedrons_.end()) { + addedPolyhedrons_.erase(it); + return 0; // 成功移除 + } + return -1; // 未找到对应ID的多面体 +} + +int CollisionSimulator::CheckJointsAngleLimit(float *joints, int bodyType) +{ + for (int i = 0; i < g_jointLinkCnt[bodyType]; ++i) { + JointInfo *joint = &(jointMap_[g_linkStartIndex[bodyType] + i]); + float angle = toRadians(joints[i]); + if (angle < joint->limit_lower || angle > joint->limit_upper) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), + "CheckJointsAngleLimit: joint out of limit [%.4f, %.4f], current: %.4f(%.2f)", + joint->limit_lower, joint->limit_upper, angle, joints[i]); + return ARM_AIM_CANNOT_REACH; + } + } + return 0; +} + +int CollisionSimulator::UpdateCollisionForNewGoal(int bodyType) +{ + if (bodyType >= ARM_ID_ERR) return -1; + int simple20 = (simpleUpdateIndex_ + 1) % COLLISION_CHECK_SIMPLE_CNT; + int simple40 = (simple20 + 1) % COLLISION_CHECK_SIMPLE_CNT; + + int startLinkIndex = g_linkStartIndex[bodyType], endLinkIndex = startLinkIndex + g_jointLinkCnt[bodyType]; + KinematicsManager *trajectory = trajectory_[bodyType]; + float *jointList20 = joints_[simple20][bodyType]; + float *jointList40 = joints_[simple40][bodyType]; + trajectory->getAngleForStarting(jointList20); + trajectory->getAngleForChecking(jointList40); + + if (CheckJointsAngleLimit(jointList20, bodyType) != 0 || CheckJointsAngleLimit(jointList40, bodyType) != 0) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); + return ARM_AIM_CANNOT_REACH; + } + + UpdatePolyhedrons(startLinkIndex, endLinkIndex, simple20, jointList20); + std::vector *addedOBBsNext_ = &(addedOBBs_[simple20]); + std::vector *addedOBBs = &(addedOBBs_[simpleUpdateIndex_]); + int result = CheckCollisionInner(addedOBBs, addedOBBsNext_); + if (result != OK) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: bodyType %d collision", bodyType); + return result; + } + + UpdatePolyhedrons(startLinkIndex, endLinkIndex, simple40, jointList40); + addedOBBsNext_ = &(addedOBBs_[simple40]); + addedOBBs = &(addedOBBs_[simple20]); + result = CheckCollisionInner(addedOBBs, addedOBBsNext_); + if (result != OK) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "UpdateCollisionForNewGoal: bodyType %d collision", bodyType); + return result; + } + return 0; +} + + +#include +#include +#include +#include + +int main(int argc, char**argv) { + // 初始化 ROS 节点 + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("moveit_collision_demo"); + + // -------------------------- + // 步骤1：加载机器人模型，创建 PlanningScene + // -------------------------- + // 加载机器人模型（需在 launch 文件中加载 URDF 和 SRDF） + robot_model_loader::RobotModelLoader robot_model_loader(node, "robot_description"); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); + + // 创建 PlanningScene（管理碰撞环境） + planning_scene::PlanningScene planning_scene(robot_model); + + // -------------------------- + // 步骤3：定义机器人姿态（待检测的状态） + // -------------------------- + moveit::core::RobotState& robot_state = planning_scene.getCurrentStateNonConst(); + const moveit::core::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm"); // 机械臂组名称（需替换为你的机器人） + + // 设置关节角度（示例：Panda 机械臂的一个姿态） + std::vector joint_values = {0.0, 0.5, 0.0, -1.5, 0.0, 1.0, 0.0}; + robot_state.setJointGroupPositions(joint_model_group, joint_values); + robot_state.update(); // 更新机器人状态 + + // -------------------------- + // 步骤4：调用碰撞检测接口 + // -------------------------- + // 定义碰撞检测请求（设置需要的结果类型） + collision_detection::CollisionRequest collision_request; + collision_request.contacts = true; // 是否返回接触信息（如碰撞点、法向量） + collision_request.max_contacts = 10; // 最大返回接触对数量 + collision_request.max_contacts_per_pair = 3; // 每对碰撞体的最大接触点数量 + collision_request.verbose = false; // 是否输出详细信息 + + // 存储碰撞检测结果 + collision_detection::CollisionResult collision_result; + + // 执行碰撞检测（检测机器人当前状态是否碰撞） + planning_scene.checkCollision(collision_request, collision_result); + + // -------------------------- + // 步骤5：处理碰撞结果 + // -------------------------- + if (collision_result.collision) { + RCLCPP_WARN(node->get_logger(), "检测到碰撞！"); + + // 输出碰撞对信息（机器人关节/链接与障碍物的碰撞） + for (const auto& contact : collision_result.contacts) { + RCLCPP_INFO(node->get_logger(), "碰撞对: %s 与 %s", + contact.first.first.c_str(), // 第一个碰撞体ID + contact.first.second.c_str()); // 第二个碰撞体ID + } + } else { + RCLCPP_INFO(node->get_logger(), "未检测到碰撞。"); + } + + // 关闭节点 + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/rm_arm_control/simulator/rm_arm_simulator.cpp b/rm_arm_control/simulator/rm_arm_simulator.cpp index 2b463f5..d764359 100644 --- a/rm_arm_control/simulator/rm_arm_simulator.cpp +++ b/rm_arm_control/simulator/rm_arm_simulator.cpp @@ -48,7 +48,8 @@ static std::map g_linkMap = { {"RightLink4", 11}, {"RightLink5", 12}, {"RightLink6", 13}, - {"body_2_link", 14} + {"body_2_link", 14}, + {"left_behind_hip_link", 15} }; enum OriginBoxIndex { diff --git a/rm_arm_control/src/rm_arm_node.cpp b/rm_arm_control/src/rm_arm_node.cpp index 9827547..57b1c74 100644 --- a/rm_arm_control/src/rm_arm_node.cpp +++ b/rm_arm_control/src/rm_arm_node.cpp @@ -13,7 +13,7 @@ namespace ArmCommandHash static int AngleStepOn(RmArmHandler *handle, const ArmGoalSharedPtr goal) { - if (goal->data_length.data_length != USED_ARM_DOF) { + if (goal->data_length != USED_ARM_DOF) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleStepOn data length error."); return -1; } @@ -30,7 +30,7 @@ namespace ArmCommandHash static int AngleDirectMove(RmArmHandler *handle, std::shared_ptr goal) { - if (goal->data_length.data_length != USED_ARM_DOF) { + if (goal->data_length != USED_ARM_DOF) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove data length error."); return -1; } @@ -48,7 +48,7 @@ namespace ArmCommandHash static int PoseStepOn(RmArmHandler *handle, std::shared_ptr goal) { - if (goal->data_length.data_length != POSE_DIMENSION) { + if (goal->data_length != POSE_DIMENSION) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseStepOn data length error."); return -1; } @@ -61,7 +61,7 @@ namespace ArmCommandHash static int PoseDirectMove(RmArmHandler *handle, std::shared_ptr goal) { - if (goal->data_length.data_length != POSE_DIMENSION) { + if (goal->data_length != POSE_DIMENSION) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseDirectMove data length error."); return -1; } @@ -193,11 +193,11 @@ rclcpp_action::GoalResponse RmArmNode::ArmActionGoal( { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal request."); // Accept the goal - if (ArmCommandHash::armCommandMap.find(goal->data_type.action_type) == ArmCommandHash::armCommandMap.end()) { - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type.action_type); + if (ArmCommandHash::armCommandMap.find(goal->data_type) == ArmCommandHash::armCommandMap.end()) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type); return rclcpp_action::GoalResponse::REJECT; } - RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id); + RmArmHandler *handle = GetArmHandler(this, goal->body_id); if (handle == NULL) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error."); return rclcpp_action::GoalResponse::REJECT; @@ -206,7 +206,7 @@ rclcpp_action::GoalResponse RmArmNode::ArmActionGoal( RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status."); return rclcpp_action::GoalResponse::REJECT; } - int result = ArmCommandHash::armCommandMap.at(goal->data_type.action_type)(handle, goal); + int result = ArmCommandHash::armCommandMap.at(goal->data_type)(handle, goal); if (result != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to process the goal."); return rclcpp_action::GoalResponse::REJECT; @@ -221,7 +221,7 @@ rclcpp_action::CancelResponse RmArmNode::ArmActionCancel( int64_t index = goal->command_id; RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received request to cancel goal. Index: %ld", index); int result = 0; - RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id); + RmArmHandler *handle = GetArmHandler(this, goal->body_id); if (handle == NULL) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error."); return rclcpp_action::CancelResponse::REJECT; @@ -250,7 +250,7 @@ void RmArmNode::ArmActionFeedback(int coll) for (auto it = goal_handles_.begin(); it != goal_handles_.end(); ++it) { auto goal_handle = it->second; ArmGoalSharedPtr goal = goal_handle->get_goal(); - RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id); + RmArmHandler *handle = GetArmHandler(this, goal->body_id); if (handle == NULL) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error."); continue; @@ -259,12 +259,12 @@ void RmArmNode::ArmActionFeedback(int coll) RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status."); continue; } - float *pose = (goal->body_id.body_id == LEFT_ARM) ? leftPose : rightPose; + float *pose = (goal->body_id == LEFT_ARM) ? leftPose : rightPose; auto feedback = std::make_shared(); feedback->command_id = goal->command_id; feedback->int_lenth = 1; - feedback->float_length.data_length = POSE_DIMENSION; + feedback->float_length = POSE_DIMENSION; feedback->int_data_array = {coll}; feedback->float_data_array = {pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]}; RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f, %f", @@ -283,7 +283,7 @@ void RmArmNode::ArmActionFinish(int64_t index, int result) auto goal_handle = goal_handle_pair->second; auto goalResult = std::make_shared(); float pose[POSE_DIMENSION] = {0}; - RmArmHandler *handle = GetArmHandler(this, goal_handle->get_goal()->body_id.body_id); + RmArmHandler *handle = GetArmHandler(this, goal_handle->get_goal()->body_id); RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Finishing goal joints: %f, %f, %f, %f, %f, %f", handle->nowJoints_[0], handle->nowJoints_[1], handle->nowJoints_[2], handle->nowJoints_[3], handle->nowJoints_[4], handle->nowJoints_[5]); @@ -291,7 +291,7 @@ void RmArmNode::ArmActionFinish(int64_t index, int result) handle->trapezoidalTrajectory_->armKinematics_->ForwardKinematics(handle->nowJoints_, pose); } goalResult->end_time = endColTime.nanoseconds(); - goalResult->result.result = result; + goalResult->result = result; goalResult->command_id = index; goalResult->pose.position.x = pose[POSE_POSITION_X]; goalResult->pose.position.y = pose[POSE_POSITION_Y]; diff --git a/rm_arm_control/urdf/FHrobot.srdf b/rm_arm_control/urdf/FHrobot.srdf new file mode 100644 index 0000000..b24dc3c --- /dev/null +++ b/rm_arm_control/urdf/FHrobot.srdf @@ -0,0 +1,460 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srdf/.setup_assistant b/srdf/.setup_assistant new file mode 100644 index 0000000..bd49cde --- /dev/null +++ b/srdf/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: rm_arm_control + relative_path: urdf/FHrobot.urdf + srdf: + relative_path: config/FHrobot.srdf + package_settings: + author_name: zj + author_email: zj@zj.com + generated_timestamp: 1761287950 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/srdf/CMakeLists.txt b/srdf/CMakeLists.txt new file mode 100644 index 0000000..311bbd2 --- /dev/null +++ b/srdf/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.22) +project(srdf) + +find_package(ament_cmake REQUIRED) + +ament_package() + +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/launch") + install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +endif() + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/srdf/config/FHrobot.ros2_control.xacro b/srdf/config/FHrobot.ros2_control.xacro new file mode 100644 index 0000000..f5eeb45 --- /dev/null +++ b/srdf/config/FHrobot.ros2_control.xacro @@ -0,0 +1,105 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['body_2_joint']} + + + + + + + ${initial_positions['left_joint1']} + + + + + + + ${initial_positions['left_joint2']} + + + + + + + ${initial_positions['left_joint3']} + + + + + + + ${initial_positions['left_joint4']} + + + + + + + ${initial_positions['left_joint5']} + + + + + + + ${initial_positions['left_joint6']} + + + + + + + ${initial_positions['right_joint1']} + + + + + + + ${initial_positions['right_joint2']} + + + + + + + ${initial_positions['right_joint3']} + + + + + + + ${initial_positions['right_joint4']} + + + + + + + ${initial_positions['right_joint5']} + + + + + + + ${initial_positions['right_joint6']} + + + + + + + diff --git a/srdf/config/FHrobot.srdf b/srdf/config/FHrobot.srdf new file mode 100644 index 0000000..b24dc3c --- /dev/null +++ b/srdf/config/FHrobot.srdf @@ -0,0 +1,460 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srdf/config/FHrobot.urdf.xacro b/srdf/config/FHrobot.urdf.xacro new file mode 100644 index 0000000..b05f16b --- /dev/null +++ b/srdf/config/FHrobot.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/srdf/config/initial_positions.yaml b/srdf/config/initial_positions.yaml new file mode 100644 index 0000000..ebbd3db --- /dev/null +++ b/srdf/config/initial_positions.yaml @@ -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 \ No newline at end of file diff --git a/srdf/config/joint_limits.yaml b/srdf/config/joint_limits.yaml new file mode 100644 index 0000000..ba19840 --- /dev/null +++ b/srdf/config/joint_limits.yaml @@ -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 \ No newline at end of file diff --git a/srdf/config/kinematics.yaml b/srdf/config/kinematics.yaml new file mode 100644 index 0000000..865eebc --- /dev/null +++ b/srdf/config/kinematics.yaml @@ -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 \ No newline at end of file diff --git a/srdf/config/moveit.rviz b/srdf/config/moveit.rviz new file mode 100644 index 0000000..99bb740 --- /dev/null +++ b/srdf/config/moveit.rviz @@ -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 diff --git a/srdf/config/moveit_controllers.yaml b/srdf/config/moveit_controllers.yaml new file mode 100644 index 0000000..7870985 --- /dev/null +++ b/srdf/config/moveit_controllers.yaml @@ -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 \ No newline at end of file diff --git a/srdf/config/pilz_cartesian_limits.yaml b/srdf/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/srdf/config/pilz_cartesian_limits.yaml @@ -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 diff --git a/srdf/config/ros2_controllers.yaml b/srdf/config/ros2_controllers.yaml new file mode 100644 index 0000000..12f6154 --- /dev/null +++ b/srdf/config/ros2_controllers.yaml @@ -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 \ No newline at end of file diff --git a/srdf/launch/demo.launch.py b/srdf/launch/demo.launch.py new file mode 100644 index 0000000..d093ae8 --- /dev/null +++ b/srdf/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/srdf/launch/move_group.launch.py b/srdf/launch/move_group.launch.py new file mode 100644 index 0000000..6f6f032 --- /dev/null +++ b/srdf/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/srdf/launch/moveit_rviz.launch.py b/srdf/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..f89ad45 --- /dev/null +++ b/srdf/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/srdf/launch/rsp.launch.py b/srdf/launch/rsp.launch.py new file mode 100644 index 0000000..924b2ac --- /dev/null +++ b/srdf/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/srdf/launch/setup_assistant.launch.py b/srdf/launch/setup_assistant.launch.py new file mode 100644 index 0000000..2a5e388 --- /dev/null +++ b/srdf/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/srdf/launch/spawn_controllers.launch.py b/srdf/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..a214d1b --- /dev/null +++ b/srdf/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/srdf/launch/static_virtual_joint_tfs.launch.py b/srdf/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..7828ac0 --- /dev/null +++ b/srdf/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/srdf/launch/warehouse_db.launch.py b/srdf/launch/warehouse_db.launch.py new file mode 100644 index 0000000..a9adbec --- /dev/null +++ b/srdf/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("FHrobot", package_name="srdf").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/srdf/package.xml b/srdf/package.xml new file mode 100644 index 0000000..da91250 --- /dev/null +++ b/srdf/package.xml @@ -0,0 +1,52 @@ + + + + srdf + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the FHrobot with the MoveIt Motion Planning Framework + + zj + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + zj + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + rm_arm_control + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + +