From 3afbc5b05eeb57b7b91dab1fbaaa1cc7016d3b09 Mon Sep 17 00:00:00 2001 From: zj Date: Fri, 14 Nov 2025 10:24:40 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E7=A2=B0=E6=92=9E=E6=A3=80?= =?UTF-8?q?=E6=B5=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- arm_keyboard_input/src/arm_keyboard_node.cpp | 48 +- .../include/create_robot_describe.hpp | 21 +- .../src/create_robot_describe.cpp | 242 ++++-- gjk_interface/CMakeLists.txt | 2 +- gjk_interface/include/gjk_interface.hpp | 53 +- gjk_interface/src/gjk_interface.cpp | 555 ++++++++++++-- rm_arm_control/CMakeLists.txt | 9 +- rm_arm_control/include/col_simulator.hpp | 31 +- rm_arm_control/include/rm_arm_simulator.hpp | 2 +- .../launch/rm_arm_control.launch.py | 15 +- rm_arm_control/simulator/col_simulator.cpp | 714 +++++++++++------- rm_arm_control/simulator/rm_arm_simulator.cpp | 13 +- .../simulator/rm_driver_kinematics.cpp | 3 +- rm_arm_control/src/rm_arm_handler.cpp | 8 +- rm_arm_control/src/rm_arm_node.cpp | 71 +- rm_arm_control/urdf/FHrobot.urdf | 2 +- rm_arm_control/urdf/FHrobot.xml | 161 ++++ 17 files changed, 1448 insertions(+), 502 deletions(-) create mode 100644 rm_arm_control/urdf/FHrobot.xml diff --git a/arm_keyboard_input/src/arm_keyboard_node.cpp b/arm_keyboard_input/src/arm_keyboard_node.cpp index 23dcf1d..26fa4ea 100644 --- a/arm_keyboard_input/src/arm_keyboard_node.cpp +++ b/arm_keyboard_input/src/arm_keyboard_node.cpp @@ -78,7 +78,7 @@ bool is_keyboard_device(const std::string& event_path) { // 检查名称是否包含键盘相关关键词 std::string lower_name = to_lower(name); - const std::vector keywords = {"keyboard"}; + const std::vector keywords = {"at translated set 2 keyboard"}; for (const auto& kw : keywords) { if (lower_name.find(kw) != std::string::npos) { return true; @@ -819,31 +819,41 @@ void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) { int main(int argc, char *argv[]) { - /*std::vector polyA = {Vector3d(2,0,0), Vector3d(4,0,0), Vector3d(2,2,0), Vector3d(4,2,0), - Vector3d(2,0,2), Vector3d(4,0,2), Vector3d(2,2,2), Vector3d(4,2,2)}; - std::vector polyB = {Vector3d(3,1,1), Vector3d(5,1,1), Vector3d(3,3,1), Vector3d(5,3,1), - Vector3d(3,1,3), Vector3d(5,1,3), Vector3d(3,3,3), Vector3d(5,3,3)}; - OBB obbA = gjk_interface::createOBBFromVertices(polyA, 2, 2, 2); - OBB obbB = gjk_interface::createOBBFromVertices(polyB, 2, 2, 2); - bool inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB); + /* +obb1: center(-0.1883, 0.0863, 0.6624), axis1(-0.7700, 0.1477, -0.6207), axis2(-0.6119, 0.1045, 0.7840), axis3(0.1806, 0.9835, 0.0099), half_lengths(0.0316, 0.0330, 0.0145) +obb2: center(0.0095, 0.0001, 0.4868), axis1(-1.0000, -0.0071, -0.0000), axis2(0.0071, -1.0000, -0.0000), axis3(-0.0000, -0.0000, 1.0000), half_lengths(0.1505, 0.1795, 0.3083) +*/ + OBB obbA; + OBB obbB; + obbA.center = Vector3d(-0.1883, 0.0863, 0.6624); + obbA.axis[0] = Vector3d(-0.7700, 0.1477, -0.6207); + obbA.axis[1] = Vector3d(-0.6119, 0.1045, 0.7840); + obbA.axis[2] = Vector3d(0.1806, 0.9835, 0.0099); + obbA.halfExtent[0] = 0.0316; + obbA.halfExtent[1] = 0.0330; + obbA.halfExtent[2] = 0.0145; + obbB.center = Vector3d(0.0095, 0.0001, 0.4868); + obbB.axis[0] = Vector3d(-1.0000, -0.0071, -0.0000); + obbB.axis[1] = Vector3d(0.0071, -1.0000, -0.0000); + obbB.axis[2] = Vector3d(-0.0000, -0.0000, 1.0000); + obbB.halfExtent[0] = 0.1505; + obbB.halfExtent[1] = 0.1795; + obbB.halfExtent[2] = 0.3083; + + AxisStats stats; + bool inCollisionOBB = gjk_interface::checkOBBCollisionNew(obbA, obbB, &stats); if (inCollisionOBB) { - printf("dect poly A and B collision\n"); + printf("dect obb A and B collision\n"); } else { - printf("dect poly A and B safe\n"); + printf("dect obb A and B safe\n"); } - polyA = {Vector3d(2,0,0), Vector3d(4,0,0), Vector3d(2,2,0), Vector3d(4,2,0), - Vector3d(2,0,2), Vector3d(4,0,2), Vector3d(2,2,2), Vector3d(4,2,2)}; - polyB = {Vector3d(3.99,0,0), Vector3d(5.99,0,0), Vector3d(3.99,2,0), Vector3d(5.99,2,0), - Vector3d(3.99,0,2), Vector3d(5.99,0,2), Vector3d(3.99,2,2), Vector3d(5.99,2,2)}; - obbA = gjk_interface::createOBBFromVertices(polyA, 2, 2, 2); - obbB = gjk_interface::createOBBFromVertices(polyB, 2, 2, 2); inCollisionOBB = gjk_interface::checkOBBCollision(obbA, obbB); if (inCollisionOBB) { - printf("dect poly A and B collision\n"); + printf("dect obb A and B collision\n"); } else { - printf("dect poly A and B safe\n"); - }*/ + printf("dect obb A and B safe\n"); + } rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = std::make_shared(); diff --git a/create_robot_describer/include/create_robot_describe.hpp b/create_robot_describer/include/create_robot_describe.hpp index fbbdd82..62cec17 100644 --- a/create_robot_describer/include/create_robot_describe.hpp +++ b/create_robot_describer/include/create_robot_describe.hpp @@ -25,11 +25,11 @@ struct JointsModel { struct CollisionModel { std::string link_name; // 所属链接名称 - std::string geometry_type; // 几何类型 + int geometry_type; // 几何类型 std::string mesh_filename; // mesh文件路径（如STL） std::vector obbModelList; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z] - std::vector joint_list; - std::vector obbLinkIndexList; // 网格边界盒所属链接索引 + std::vector jointIndexList; // 网格边界盒所属链接索引 + std::vector obbObbIndexList; // 网格边界盒所属链接索引 int joint_count; int link_index; int parent_link_index; @@ -45,14 +45,6 @@ public: ~RobotDescription(); // 其他成员函数和变量 -private: - int InitJointModel(); - int InitJoint(const urdf::Model &urdf_model); - int GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct); - int InitCollisionStructureList(const urdf::Model &urdf_model); - void UpdatePolyhedrons(); - int InitLinkCollisionDetectMatrix(const std::string& srdf_str); - std::vector collision_structures_; std::vector jointMap_; std::map linkCollisionMap_; @@ -61,6 +53,13 @@ private: int maxObbCnt; bool *g_linkCollisionDetectMatrix; +private: + int InitJointModel(); + int InitJoint(const urdf::Model &urdf_model); + int GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct); + int InitCollisionStructureList(const urdf::Model &urdf_model); + void UpdatePolyhedrons(); + int InitLinkCollisionDetectMatrix(const std::string& srdf_str); }; #endif // CREATE_ROBOT_DESCRIBE_HPP \ No newline at end of file diff --git a/create_robot_describer/src/create_robot_describe.cpp b/create_robot_describer/src/create_robot_describe.cpp index 60590e0..7f52820 100644 --- a/create_robot_describer/src/create_robot_describe.cpp +++ b/create_robot_describer/src/create_robot_describe.cpp @@ -20,35 +20,38 @@ int RobotDescription::InitJointModel() { int linkIndex = rootLinkIndex_; CollisionModel *colStruct = &collision_structures_[linkIndex]; - std::vector jointList; + std::vector jointList; int index = 0; int maxJointCount = 0; Quaterniond rotationAll = Quaterniond::Identity(); while (colStruct->joint_count > 0 || index < maxJointCount) { for (int i = 0; i < colStruct->joint_count; i++) { - jointList.push_back(&colStruct->joint_list[i]); + jointList.push_back(colStruct->jointIndexList[i]); maxJointCount++; - printf("链接 %s 包含关节 %s, 原offset: (%.4f, %.4f, %.4f)\n", colStruct->link_name.c_str(), colStruct->joint_list[i].name.c_str(), - colStruct->joint_list[i].nextLinkOffset.x(), - colStruct->joint_list[i].nextLinkOffset.y(), - colStruct->joint_list[i].nextLinkOffset.z()); - colStruct->joint_list[i].rotation = rotationAll * colStruct->joint_list[i].rotation; - colStruct->joint_list[i].axis = colStruct->joint_list[i].rotation * colStruct->joint_list[i].axis; - colStruct->joint_list[i].nextLinkOffset = rotationAll * colStruct->joint_list[i].nextLinkOffset; + int jointIndex = colStruct->jointIndexList[i]; + JointsModel *jointInfo = &jointMap_[jointIndex]; + printf("链接 %s 包含关节 %s, 原offset: (%.4f, %.4f, %.4f)\n", colStruct->link_name.c_str(), jointInfo->name.c_str(), + jointInfo->nextLinkOffset.x(), + jointInfo->nextLinkOffset.y(), + jointInfo->nextLinkOffset.z()); + jointInfo->rotation = rotationAll * jointInfo->rotation; + jointInfo->axis = jointInfo->rotation * jointInfo->axis; + jointInfo->nextLinkOffset = rotationAll * jointInfo->nextLinkOffset; printf("更新后关节 %s 旋转四元数: (%.4f, %.4f, %.4f, %.4f), 轴向: (%.4f, %.4f, %.4f), 偏移: (%.4f, %.4f, %.4f)\n", - colStruct->joint_list[i].name.c_str(), - colStruct->joint_list[i].rotation.w(), - colStruct->joint_list[i].rotation.x(), - colStruct->joint_list[i].rotation.y(), - colStruct->joint_list[i].rotation.z(), - colStruct->joint_list[i].axis.x(), - colStruct->joint_list[i].axis.y(), - colStruct->joint_list[i].axis.z(), - colStruct->joint_list[i].nextLinkOffset.x(), - colStruct->joint_list[i].nextLinkOffset.y(), - colStruct->joint_list[i].nextLinkOffset.z()); + jointInfo->name.c_str(), + jointInfo->rotation.w(), + jointInfo->rotation.x(), + jointInfo->rotation.y(), + jointInfo->rotation.z(), + jointInfo->axis.x(), + jointInfo->axis.y(), + jointInfo->axis.z(), + jointInfo->nextLinkOffset.x(), + jointInfo->nextLinkOffset.y(), + jointInfo->nextLinkOffset.z()); } - JointsModel *jointInfo = jointList[index]; + int jointIndex = jointList[index]; + JointsModel *jointInfo = &jointMap_[jointIndex]; rotationAll = jointInfo->rotation; index++; linkIndex = jointInfo->child_link; @@ -64,6 +67,7 @@ int RobotDescription::InitJointModel() int RobotDescription::InitJoint(const urdf::Model &urdf_model) { + int joint_index = 0; for (const auto& joint_pair : urdf_model.joints_) { urdf::JointConstSharedPtr joint = joint_pair.second; JointsModel joint_info; @@ -112,18 +116,71 @@ int RobotDescription::InitJoint(const urdf::Model &urdf_model) } jointMap_.push_back(joint_info); CollisionModel *parentColStruct = &collision_structures_[joint_info.parent_link]; - parentColStruct->joint_list.push_back(joint_info); + parentColStruct->jointIndexList.push_back(joint_index); parentColStruct->joint_count++; CollisionModel *childColStruct = &collision_structures_[joint_info.child_link]; childColStruct->parent_link_index = joint_info.parent_link; + + joint_index++; } InitJointModel(); return 0; } +static OBB GetMeshesBounds(const std::string& filename) +{ + OBB obb; + shapes::Mesh* mesh = shapes::createMeshFromResource(filename); + if (!mesh) { + printf( "无法加载网格文件: %s", filename.c_str()); + return obb; + } + // 计算边界盒 + double min_x = mesh->vertices[0], max_x = mesh->vertices[0]; + double min_y = mesh->vertices[1], max_y = mesh->vertices[1]; + double min_z = mesh->vertices[2], max_z = mesh->vertices[2]; + + for (unsigned int i = 1; i < mesh->vertex_count; ++i) { + if (min_x > mesh->vertices[3*i]) { + min_x = mesh->vertices[3*i]; + } + if (min_y > mesh->vertices[3*i + 1]) { + min_y = mesh->vertices[3*i + 1]; + } + if (min_z > mesh->vertices[3*i + 2]) { + min_z = mesh->vertices[3*i + 2]; + } + if (max_x < mesh->vertices[3*i]) { + max_x = mesh->vertices[3*i]; + } + if (max_y < mesh->vertices[3*i + 1]) { + max_y = mesh->vertices[3*i + 1]; + } + if (max_z < mesh->vertices[3*i + 2]) { + max_z = mesh->vertices[3*i + 2]; + } + } + + std::vector points = { + Vector3d(min_x, min_y, min_z), + Vector3d(max_x, min_y, min_z), + Vector3d(min_x, max_y, min_z), + Vector3d(max_x, max_y, min_z), + Vector3d(min_x, min_y, max_z), + Vector3d(max_x, min_y, max_z), + Vector3d(min_x, max_y, max_z), + Vector3d(max_x, max_y, max_z) + }; + obb = gjk_interface::createOBBFromPoints(points); + + // 释放网格资源 + delete mesh; + return obb; +} + int RobotDescription::GetMeshLinkdimensions(const std::string& filename, CollisionModel& col_struct) { - /*shapes::Mesh* mesh = shapes::createMeshFromResource(filename); + shapes::Mesh* mesh = shapes::createMeshFromResource(filename); if (!mesh) { printf("无法加载网格文件: %s\n", filename.c_str()); return false; @@ -142,9 +199,9 @@ int RobotDescription::GetMeshLinkdimensions(const std::string& filename, Collisi for (const auto& pt : obb_points) { eigen_points.push_back(Eigen::Vector3d(pt.x(), pt.y(), pt.z())); printf("(%.4f, %.4f, %.4f)\n", pt.x(), pt.y(), pt.z()); - }*/ + } - UNUSED(filename); + /*UNUSED(filename); static std::map> pointMap = { {"LeftLink1", {Vector3d(0.0411, 0.0456, -0.1191), Vector3d(-0.0409, 0.0458, -0.1191), Vector3d(-0.0411, -0.0444, -0.1013), Vector3d(0.0409, -0.0446, -0.1013), Vector3d(0.0410, -0.0137, 0.0545), Vector3d(0.0412, 0.0765, 0.0366), Vector3d(-0.0408, 0.0767, 0.0366), Vector3d(-0.0410, -0.0135, 0.0545)}}, @@ -213,9 +270,15 @@ int RobotDescription::GetMeshLinkdimensions(const std::string& filename, Collisi }; std::array eigen_points_arr = pointMap.at(col_struct.link_name); - std::vector eigen_points(eigen_points_arr.begin(), eigen_points_arr.end()); + std::vector eigen_points(eigen_points_arr.begin(), eigen_points_arr.end());*/ + OBB boundsObb = GetMeshesBounds(filename); OBB obb = gjk_interface::createOBBFromPoints(eigen_points); + if (boundsObb.halfExtent[0] > 0) { + double v1 = boundsObb.halfExtent[0] * boundsObb.halfExtent[1] * boundsObb.halfExtent[2]; + double v2 = obb.halfExtent[0] * obb.halfExtent[1] * obb.halfExtent[2]; + printf("网格文件: %s 计算得到的 边界 体积: %.6f, 计算 OBB 体积: %.6f\n", filename.c_str(), v1 * 8, v2 * 8); + } col_struct.obbModelList.push_back(obb); return 0; } @@ -224,6 +287,7 @@ int RobotDescription::InitCollisionStructureList(const urdf::Model &urdf_model) { int result = 0; int linkIndex = 0; + maxObbCnt = 0; for (const auto& link_pair : urdf_model.links_) { // link_pair是一个键值对: 键为链接名称，值为链接指针 urdf::LinkConstSharedPtr link = link_pair.second; @@ -257,7 +321,7 @@ int RobotDescription::InitCollisionStructureList(const urdf::Model &urdf_model) } } for (int i = 0; i < obbCnt; i++) { - colStruct.obbLinkIndexList.push_back(maxObbCnt); + colStruct.obbObbIndexList.push_back(maxObbCnt); maxObbCnt++; } colStruct.link_index = linkIndex; @@ -281,15 +345,16 @@ void RobotDescription::UpdatePolyhedrons() { int linkIndex = rootLinkIndex_; CollisionModel *colStruct = &collision_structures_[linkIndex]; - std::vector jointList; + std::vector jointList; int index = 0; int maxJointCount = 0; while (colStruct->joint_count > 0 || index < maxJointCount) { for (int i = 0; i < colStruct->joint_count; i++) { - jointList.push_back(&colStruct->joint_list[i]); + jointList.push_back(colStruct->jointIndexList[i]); maxJointCount++; } - JointsModel *jointInfo = jointList[index]; + int jointIndex = jointList[index]; + JointsModel *jointInfo = &jointMap_[jointIndex]; Quaterniond rotation = collision_structures_[jointInfo->parent_link].rotation; Vector3d offset = collision_structures_[jointInfo->parent_link].offset; @@ -349,8 +414,8 @@ int RobotDescription::InitLinkCollisionDetectMatrix(const std::string& srdf_str) if (link1Index != -1 && link2Index != -1) { CollisionModel* colStruct1 = &collision_structures_[link1Index]; CollisionModel* colStruct2 = &collision_structures_[link2Index]; - for (int obb1Idx : colStruct1->obbLinkIndexList) { - for (int obb2Idx : colStruct2->obbLinkIndexList) { + for (int obb1Idx : colStruct1->obbObbIndexList) { + for (int obb2Idx : colStruct2->obbObbIndexList) { g_linkCollisionDetectMatrix[obb1Idx * maxObbCnt + obb2Idx] = false; g_linkCollisionDetectMatrix[obb2Idx * maxObbCnt + obb1Idx] = false; } @@ -359,7 +424,7 @@ int RobotDescription::InitLinkCollisionDetectMatrix(const std::string& srdf_str) disable_count++; } printf("Initialized link collision detect matrix, disabled %d collision pairs. total %d\n", disable_count * 2, maxObbCnt * maxObbCnt); - for (int i = 0; i < maxObbCnt; ++i) { + /*for (int i = 0; i < maxObbCnt; ++i) { for (int j = 0; j < maxObbCnt; ++j) { if (g_linkCollisionDetectMatrix[i * maxObbCnt + j]) { printf("true "); @@ -368,7 +433,7 @@ int RobotDescription::InitLinkCollisionDetectMatrix(const std::string& srdf_str) } } printf("\n"); - } + }*/ return 0; } @@ -500,40 +565,101 @@ void WriteXmlFile(const std::string& file_path, RobotDescription& robot_descript XMLDeclaration* decl = doc.NewDeclaration(); doc.InsertFirstChild(decl); - // 创建根节点 - XMLElement* root = doc.NewElement("root"); + // 创建根节点 + XMLElement* root = doc.NewElement("robot"); + root->SetAttribute("name", "FHrobot"); + root->SetAttribute("rootLinkIndex_", robot_description.rootLinkIndex_); + root->SetAttribute("maxObbCnt", robot_description.maxObbCnt); + root->SetAttribute("collision_structure_count", static_cast(robot_description.collision_structures_.size())); + root->SetAttribute("joint_count", static_cast(robot_description.jointMap_.size())); doc.InsertEndChild(root); // 将根节点添加到文档 - // 向根节点添加子节点 - XMLElement* node1 = doc.NewElement("node"); - node1->SetAttribute("id", "1"); // 设置属性 - root->InsertEndChild(node1); + for (const auto& col_struct : robot_description.collision_structures_) { + XMLElement* collisionElem = doc.NewElement("collision_model"); + collisionElem->SetAttribute("link_name", col_struct.link_name.c_str()); + collisionElem->SetAttribute("link_index", col_struct.link_index); + collisionElem->SetAttribute("geometry_type", col_struct.geometry_type); + collisionElem->SetAttribute("mesh_filename", col_struct.mesh_filename.c_str()); + collisionElem->SetAttribute("parent_link_index", col_struct.parent_link_index); + collisionElem->SetAttribute("offset_x", col_struct.offset.x()); + collisionElem->SetAttribute("offset_y", col_struct.offset.y()); + collisionElem->SetAttribute("offset_z", col_struct.offset.z()); + collisionElem->SetAttribute("rotation_x", col_struct.rotation.x()); + collisionElem->SetAttribute("rotation_y", col_struct.rotation.y()); + collisionElem->SetAttribute("rotation_z", col_struct.rotation.z()); + collisionElem->SetAttribute("rotation_w", col_struct.rotation.w()); - // 向node1添加子节点 - XMLElement* param1 = doc.NewElement("param"); - param1->SetAttribute("name", "name"); - param1->SetAttribute("value", "example"); - node1->InsertEndChild(param1); + collisionElem->SetAttribute("obb_count", static_cast(col_struct.obbModelList.size())); + for (size_t i = 0; i < col_struct.obbModelList.size(); ++i) { + const OBB& obb = col_struct.obbModelList[i]; + XMLElement* obbElem = doc.NewElement("obb"); + obbElem->SetAttribute("index", col_struct.obbObbIndexList[i]); + obbElem->SetAttribute("center_x", obb.center.x()); + obbElem->SetAttribute("center_y", obb.center.y()); + obbElem->SetAttribute("center_z", obb.center.z()); + obbElem->SetAttribute("half_extent_x", obb.halfExtent[0]); + obbElem->SetAttribute("half_extent_y", obb.halfExtent[1]); + obbElem->SetAttribute("half_extent_z", obb.halfExtent[2]); + obbElem->SetAttribute("axis_0_x", obb.axis[0].x()); + obbElem->SetAttribute("axis_0_y", obb.axis[0].y()); + obbElem->SetAttribute("axis_0_z", obb.axis[0].z()); + obbElem->SetAttribute("axis_1_x", obb.axis[1].x()); + obbElem->SetAttribute("axis_1_y", obb.axis[1].y()); + obbElem->SetAttribute("axis_1_z", obb.axis[1].z()); + obbElem->SetAttribute("axis_2_x", obb.axis[2].x()); + obbElem->SetAttribute("axis_2_y", obb.axis[2].y()); + obbElem->SetAttribute("axis_2_z", obb.axis[2].z()); - XMLElement* param2 = doc.NewElement("param"); - param2->SetAttribute("name", "type"); - param2->SetAttribute("value", "xml"); - node1->InsertEndChild(param2); + obbElem->SetAttribute("max_radius", obb.maxRadius); + obbElem->SetAttribute("self_min_project_x", obb.selfMinProject[0]); + obbElem->SetAttribute("self_min_project_y", obb.selfMinProject[1]); + obbElem->SetAttribute("self_min_project_z", obb.selfMinProject[2]); + obbElem->SetAttribute("self_max_project_x", obb.selfMaxProject[0]); + obbElem->SetAttribute("self_max_project_y", obb.selfMaxProject[1]); + obbElem->SetAttribute("self_max_project_z", obb.selfMaxProject[2]); + // 可根据需要添加轴向信息 + collisionElem->InsertEndChild(obbElem); + } - // 向根节点添加另一个子节点 - XMLElement* node2 = doc.NewElement("node"); - node2->SetAttribute("id", "2"); - root->InsertEndChild(node2); + collisionElem->SetAttribute("joint_index_count", static_cast(col_struct.jointIndexList.size())); + for (size_t i = 0; i < col_struct.jointIndexList.size(); ++i) { + XMLElement* jointIndexElem = doc.NewElement("joint_index"); + jointIndexElem->SetAttribute("index", col_struct.jointIndexList[i]); + collisionElem->InsertEndChild(jointIndexElem); + } - // 向node2添加文本节点 - XMLElement* value = doc.NewElement("value"); - value->SetText("123.456"); // 设置文本内容 - node2->InsertEndChild(value); + root->InsertEndChild(collisionElem); + } + + for (size_t i = 0; i < robot_description.jointMap_.size(); ++i) { + const JointsModel& joint = robot_description.jointMap_[i]; + XMLElement* jointElem = doc.NewElement("joint_model"); + jointElem->SetAttribute("joint_name", joint.name.c_str()); + jointElem->SetAttribute("parent_link", joint.parent_link); + jointElem->SetAttribute("child_link", joint.child_link); + jointElem->SetAttribute("nextLinkOffset_x", joint.nextLinkOffset.x()); + jointElem->SetAttribute("nextLinkOffset_y", joint.nextLinkOffset.y()); + jointElem->SetAttribute("nextLinkOffset_z", joint.nextLinkOffset.z()); + jointElem->SetAttribute("joint_index", i); + jointElem->SetAttribute("type", joint.type); + jointElem->SetAttribute("axis_x", joint.axis.x()); + jointElem->SetAttribute("axis_y", joint.axis.y()); + jointElem->SetAttribute("axis_z", joint.axis.z()); + jointElem->SetAttribute("limit_lower", joint.limit_lower); + jointElem->SetAttribute("limit_upper", joint.limit_upper); + jointElem->SetAttribute("limit_effort", joint.limit_e); + jointElem->SetAttribute("limit_velocity", joint.limit_v); + jointElem->SetAttribute("rotation_x", joint.rotation.x()); + jointElem->SetAttribute("rotation_y", joint.rotation.y()); + jointElem->SetAttribute("rotation_z", joint.rotation.z()); + jointElem->SetAttribute("rotation_w", joint.rotation.w()); + root->InsertEndChild(jointElem); + } // 4. 将XML内容写入文件 XMLError error = doc.SaveFile(file_path.c_str()); if (error != XML_SUCCESS) { - throw std::runtime_error("写入XML文件失败：" + file_path + "（错误码：" + std::to_string(error) + "）"); + throw std::runtime_error("写入XML文件失败:" + file_path + "（错误码：" + std::to_string(error) + "）"); } printf("成功写入XML文件: %s\n", file_path.c_str()); diff --git a/gjk_interface/CMakeLists.txt b/gjk_interface/CMakeLists.txt index 347c954..43c1b6f 100644 --- a/gjk_interface/CMakeLists.txt +++ b/gjk_interface/CMakeLists.txt @@ -4,7 +4,7 @@ project(gjk_interface) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -add_compile_options(-g) +add_compile_options(-g -march=native -O3 -DEIGEN_VECTORIZE_SSE4_2) find_package(ament_cmake REQUIRED) find_package(eigen3_cmake_module REQUIRED) diff --git a/gjk_interface/include/gjk_interface.hpp b/gjk_interface/include/gjk_interface.hpp index 466a904..aea8722 100644 --- a/gjk_interface/include/gjk_interface.hpp +++ b/gjk_interface/include/gjk_interface.hpp @@ -3,7 +3,6 @@ #include #include -#include "gjk_interface.hpp" typedef enum { COLLISION_RESULT_NO_COLLISION = 0, @@ -14,8 +13,51 @@ typedef enum { struct OBB { Eigen::Vector3d center; // 中心 Eigen::Vector3d axis[3]; // 3条正交的棱边方向轴（单位向量） - Eigen::Vector3d axisVectors[3]; // 3条正交的棱边方向轴（非单位向量） double halfExtent[3]; // 沿各轴的半长（从中心到面的距离） + double selfMinProject[3]; + double selfMaxProject[3]; + double maxRadius; + + OBB() { + center = Eigen::Vector3d::Zero(); + for (int i = 0; i < 3; ++i) { + axis[i] = Eigen::Vector3d::Zero(); + halfExtent[i] = 0.0; + selfMinProject[i] = 0.0; + selfMaxProject[i] = 0.0; + } + } +}; + +typedef enum { + AXIS_DISTANCE_LONG = 0, + AXIS_OBB_A_AXIS_0 = 1, + AXIS_OBB_A_AXIS_1 = 2, + AXIS_OBB_A_AXIS_2 = 3, + AXIS_OBB_B_AXIS_0 = 4, + AXIS_OBB_B_AXIS_1 = 5, + AXIS_OBB_B_AXIS_2 = 6, + AXIS_A0_CROSS_B0 = 7, + AXIS_A0_CROSS_B1 = 8, + AXIS_A0_CROSS_B2 = 9, + AXIS_A1_CROSS_B0 = 10, + AXIS_A1_CROSS_B1 = 11, + AXIS_A1_CROSS_B2 = 12, + AXIS_A2_CROSS_B0 = 13, + AXIS_A2_CROSS_B1 = 14, + AXIS_A2_CROSS_B2 = 15, + AXIS_CNT = 16, +} AxisE; + +struct AxisStats { + int64_t hitCount[AXIS_CNT]; + int axisRank[AXIS_CNT]; + AxisStats() { + for (int i = 0; i < AXIS_CNT; ++i) { + hitCount[i] = 0; + axisRank[i] = i; + } + } }; namespace gjk_interface { @@ -25,7 +67,14 @@ namespace gjk_interface { OBB createOBBFromPoints(const std::vector& vertices); OBB updateOBBFromPoints(OBB originalOBB, Eigen::Vector3d translation, Eigen::Quaterniond rotation); bool checkOBBCollision(const OBB &obb1, const OBB &obb2); + bool checkOBBCollisionNew(const OBB &A, const OBB &B, AxisStats *stats); + bool checkOBBCollisionNewTemp(const OBB &A, const OBB &B, AxisStats *stats); + // Optimized SAT-based OBB-OBB collision (no axis normalization, scalar math) + bool checkOBBCollision2(const OBB &a, const OBB &b); + void getSelfProject(OBB &self); + void GetAxisLog(double &checkAxisAve); + void BubbleSortAxisByHitCount(AxisStats *stats); } // namespace gjk_interface #endif // GJK_INTERFACE_HPP \ No newline at end of file diff --git a/gjk_interface/src/gjk_interface.cpp b/gjk_interface/src/gjk_interface.cpp index 7f48ccc..27b00c6 100644 --- a/gjk_interface/src/gjk_interface.cpp +++ b/gjk_interface/src/gjk_interface.cpp @@ -1,6 +1,8 @@ #include "math.h" +#include #include "gjk_interface.hpp" +#define UNUSED(x) (void)(x) #define POLY_SIMPLIFY_MAX_SIZE 4 using namespace Eigen; @@ -163,6 +165,26 @@ namespace gjk_interface { return false; } + // 计算OBB在指定轴上的投影范围（min, max） + std::pair projectOBB(const OBB& obb, const Vector3d& axis) { + // OBB中心在轴上的投影 + double centerProj = obb.center.dot(axis); + // OBB沿轴的总投影长度（半长在轴上的投影之和） + double totalExtent = 0.0; + for (int i = 0; i < 3; ++i) { + totalExtent += obb.halfExtent[i] * fabs(obb.axis[i].dot(axis)); + } + return { centerProj - totalExtent, centerProj + totalExtent }; + } + + void getSelfProject(OBB &self) { + for (int i = 0; i < 3; ++i) { + auto [min, max] = projectOBB(self, self.axis[i]); + self.selfMinProject[i] = min; + self.selfMaxProject[i] = max; + } + } + // 从8个顶点提取OBB参数 OBB createOBBFromVertices(const std::vector& vertices, const float length, const float width, const float height) { @@ -182,6 +204,13 @@ namespace gjk_interface { obb.halfExtent[0] = length * (1.0 / 2.0); obb.halfExtent[1] = width * (1.0 / 2.0); obb.halfExtent[2] = height * (1.0 / 2.0); + obb.maxRadius = 0; + for (int i = 0; i < 3; ++i) { + obb.maxRadius += obb.halfExtent[i] * obb.halfExtent[i]; + } + obb.maxRadius = sqrt(obb.maxRadius); + + getSelfProject(obb); return obb; } @@ -204,9 +233,6 @@ namespace gjk_interface { obb.axis[0] = vec1.normalized(); obb.axis[1] = vec2.normalized(); obb.axis[2] = vec3.normalized(); - obb.axisVectors[0] = vec1; - obb.axisVectors[1] = vec2; - obb.axisVectors[2] = vec3; // 计算中心点 obb.center = (vertices[i] + vertices[j] + vertices[k] - vertices[0]) * (1.0 / 2.0); @@ -215,14 +241,19 @@ namespace gjk_interface { obb.halfExtent[0] = vec1.norm() * (1.0 / 2.0); obb.halfExtent[1] = vec2.norm() * (1.0 / 2.0); obb.halfExtent[2] = vec3.norm() * (1.0 / 2.0); - + obb.maxRadius = 0; + for (int i = 0; i < 3; ++i) { + obb.maxRadius += obb.halfExtent[i] * obb.halfExtent[i]; + } + obb.maxRadius = sqrt(obb.maxRadius); + getSelfProject(obb); return obb; } } } } } - + getSelfProject(obb); return obb; } @@ -235,70 +266,500 @@ namespace gjk_interface { // 更新轴方向 for (int i = 0; i < 3; ++i) { updatedOBB.axis[i] = rotation * originalOBB.axis[i]; - updatedOBB.axisVectors[i] = rotation * originalOBB.axisVectors[i]; updatedOBB.halfExtent[i] = originalOBB.halfExtent[i]; } + // getSelfProject(updatedOBB); + updatedOBB.maxRadius = originalOBB.maxRadius; return updatedOBB; } - // 计算OBB在指定轴上的投影范围（min, max） - std::pair projectOBB(const OBB& obb, const Vector3d& axis) { - // OBB中心在轴上的投影 - double centerProj = obb.center.dot(axis); - // OBB沿轴的总投影长度（半长在轴上的投影之和） - double totalExtent = 0.0; - for (int i = 0; i < 3; ++i) { - totalExtent += obb.halfExtent[i] * fabs(obb.axis[i].dot(axis)); - } - return { centerProj - totalExtent, centerProj + totalExtent }; - } - // 判断两个投影范围是否重叠 bool areProjectionsOverlapping(double aMin, double aMax, double bMin, double bMax) { return !(aMax < bMin - 1e-6 || bMax < aMin - 1e-6); // 1e-6为浮点误差容限 } + static int checkCnt = 0; + static int checkAxisCnt = 0; // OBB碰撞检测主函数（输入：两个长方体的8个顶点，共16个） - bool checkOBBCollision(const OBB &obb1, const OBB &obb2) { - - // 2. 定义所有需要检查的分离轴（共15条） - std::vector separationAxes; - - // 2.3 两个OBB轴的两两叉积（3×3=9条） + bool checkOBBCollision2(const OBB &obb1, const OBB &obb2) { + // 其次测试 9 条叉积轴 + checkCnt++; + checkAxisCnt++; + double distance = obb1.maxRadius + obb2.maxRadius; + if (distance * distance < (obb1.center - obb2.center).squaredNorm()) { + return false; + } for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { + checkAxisCnt++; Vector3d crossAxis = obb1.axis[i].cross(obb2.axis[j]); - if (crossAxis.dot(crossAxis) > 1e-6) { // 排除零向量（轴平行时叉积为零 - Vector3d crossAxisNormalized = crossAxis.normalized(); - auto [aMin, aMax] = projectOBB(obb1, crossAxisNormalized); - auto [bMin, bMax] = projectOBB(obb2, crossAxisNormalized); - if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) { + double lengthDot = crossAxis.dot(crossAxis); + if (lengthDot > 1e-6) { // 排除零向量（轴平行时叉积为零 + /*if (lengthDot < 25 * 1e-6) { + crossAxis = crossAxis.normalized(); + }*/ + auto [aMin, aMax] = projectOBB(obb1, crossAxis); + auto [bMin, bMax] = projectOBB(obb2, crossAxis); + if (aMax < bMin - 1e-6 || bMax < aMin - 1e-6) { // 找到分离轴，判定为不碰撞 return false; } } } } - // 2.1 第一个OBB的3条轴 - for (int i = 0; i < 3; ++i) { - auto [aMin, aMax] = projectOBB(obb1, obb1.axis[i]); - auto [bMin, bMax] = projectOBB(obb2, obb1.axis[i]); - if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) { - // 找到分离轴，判定为不碰撞 - return false; - } - } - // 2.2 第二个OBB的3条轴 - for (int i = 0; i < 3; ++i) { - auto [aMin, aMax] = projectOBB(obb1, obb2.axis[i]); - auto [bMin, bMax] = projectOBB(obb2, obb2.axis[i]); - if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) { - // 找到分离轴，判定为不碰撞 - return false; - } - } + // 2. 定义所有需要检查的分离轴（共15条） + // 先测试主轴（更便宜，能更早返回） + for (int i = 0; i < 3; ++i) { + checkAxisCnt++; + auto [bMin, bMax] = projectOBB(obb2, obb1.axis[i]); + if ((obb1.selfMaxProject[i] < bMin - 1e-6 || bMax < obb1.selfMinProject[i] - 1e-6)){ + // 找到分离轴，判定为不碰撞 + return false; + } + checkAxisCnt++; + auto [aMin, aMax] = projectOBB(obb1, obb2.axis[i]); + if ((aMax < obb2.selfMinProject[i] - 1e-6 || obb2.selfMaxProject[i] < aMin - 1e-6)){ + // 找到分离轴，判定为不碰撞 + return false; + } + } + // 所有轴均重叠，判定为碰撞 return true; } + + bool checkOBBCollision(const OBB &A, const OBB &B) { + checkCnt++; + /*double distance = A.maxRadius + B.maxRadius; + if (distance * distance < (A.center - B.center).squaredNorm()) { + checkAxisCnt += 1; + return false; + }*/ + // Broadphase: sphere check (optional, skipped for determinism) + const double eps = 1e-6; + + // Half extents + const double a0 = A.halfExtent[0], a1 = A.halfExtent[1], a2 = A.halfExtent[2]; + const double b0 = B.halfExtent[0], b1 = B.halfExtent[1], b2 = B.halfExtent[2]; + + // Rotation matrix expressing B in A's frame: R[i][j] = Ai · Bj + double R[3][3]; + double absR[3][3]; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R[i][j] = A.axis[i].dot(B.axis[j]); + absR[i][j] = std::abs(R[i][j]) + eps; + } + } + + // Translation vector from A to B in world, then expressed in A's frame + Eigen::Vector3d tWorld = B.center - A.center; + double tA[3] = { tWorld.dot(A.axis[0]), tWorld.dot(A.axis[1]), tWorld.dot(A.axis[2]) }; + + // Test axes L = Ai x Bj + // A0 x B0 + { + double ra = a1 * absR[2][0] + a2 * absR[1][0]; + double rb = b1 * absR[0][2] + b2 * absR[0][1]; + double t = std::abs(tA[2] * R[1][0] - tA[1] * R[2][0]); + checkAxisCnt ++; + printf("A0 x B0: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A0 x B1 + { + double ra = a1 * absR[2][1] + a2 * absR[1][1]; + double rb = b0 * absR[0][2] + b2 * absR[0][0]; + double t = std::abs(tA[2] * R[1][1] - tA[1] * R[2][1]); + checkAxisCnt ++; + printf("A0 x B1: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A0 x B2 + { + double ra = a1 * absR[2][2] + a2 * absR[1][2]; + double rb = b0 * absR[0][1] + b1 * absR[0][0]; + double t = std::abs(tA[2] * R[1][2] - tA[1] * R[2][2]); + checkAxisCnt ++; + printf("A0 x B2: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A1 x B0 + { + double ra = a0 * absR[2][0] + a2 * absR[0][0]; + double rb = b1 * absR[1][2] + b2 * absR[1][1]; + double t = std::abs(tA[0] * R[2][0] - tA[2] * R[0][0]); + checkAxisCnt ++; + printf("A1 x B0: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A1 x B1 + { + double ra = a0 * absR[2][1] + a2 * absR[0][1]; + double rb = b0 * absR[1][2] + b2 * absR[1][0]; + double t = std::abs(tA[0] * R[2][1] - tA[2] * R[0][1]); + checkAxisCnt ++; + printf("A1 x B1: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A1 x B2 + { + double ra = a0 * absR[2][2] + a2 * absR[0][2]; + double rb = b0 * absR[1][1] + b1 * absR[1][0]; + double t = std::abs(tA[0] * R[2][2] - tA[2] * R[0][2]); + checkAxisCnt ++; + printf("A1 x B2: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A2 x B0 + { + double ra = a0 * absR[1][0] + a1 * absR[0][0]; + double rb = b1 * absR[2][2] + b2 * absR[2][1]; + double t = std::abs(tA[1] * R[0][0] - tA[0] * R[1][0]); + checkAxisCnt ++; + printf("A2 x B0: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A2 x B1 + { + double ra = a0 * absR[1][1] + a1 * absR[0][1]; + double rb = b0 * absR[2][2] + b2 * absR[2][0]; + double t = std::abs(tA[1] * R[0][1] - tA[0] * R[1][1]); + checkAxisCnt ++; + printf("A2 x B1: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + // A2 x B2 + { + double ra = a0 * absR[1][2] + a1 * absR[0][2]; + double rb = b0 * absR[2][1] + b1 * absR[2][0]; + double t = std::abs(tA[1] * R[0][2] - tA[0] * R[1][2]); + checkAxisCnt ++; + printf("A2 x B2: t = %f, ra + rb = %f\n", t, ra + rb); + if (t > ra + rb) return false; + } + + // Test axes L = A0, A1, A2 + { + double ra = a0; + double rb = b0 * absR[0][0] + b1 * absR[0][1] + b2 * absR[0][2]; + checkAxisCnt ++; + printf("A0 x B0: t = %f, ra + rb = %f\n", tA[0], ra + rb); + if (std::abs(tA[0]) > ra + rb) return false; + } + { + double ra = a1; + double rb = b0 * absR[1][0] + b1 * absR[1][1] + b2 * absR[1][2]; + checkAxisCnt ++; + printf("A1 x B1: t = %f, ra + rb = %f\n", tA[1], ra + rb); + if (std::abs(tA[1]) > ra + rb) return false; + } + { + double ra = a2; + double rb = b0 * absR[2][0] + b1 * absR[2][1] + b2 * absR[2][2]; + checkAxisCnt ++; + printf("A2 x B2: t = %f, ra + rb = %f\n", tA[2], ra + rb); + if (std::abs(tA[2]) > ra + rb) return false; + } + + // Test axes L = B0, B1, B2 + { + double ra = a0 * absR[0][0] + a1 * absR[1][0] + a2 * absR[2][0]; + double rb = b0; + double tB = std::abs(tWorld.dot(B.axis[0])); + checkAxisCnt ++; + printf("B0: t = %f, ra + rb = %f\n", tB, ra + rb); + if (tB > ra + rb) return false; + } + { + double ra = a0 * absR[0][1] + a1 * absR[1][1] + a2 * absR[2][1]; + double rb = b1; + double tB = std::abs(tWorld.dot(B.axis[1])); + checkAxisCnt ++; + printf("B1: t = %f, ra + rb = %f\n", tB, ra + rb); + if (tB > ra + rb) return false; + } + { + double ra = a0 * absR[0][2] + a1 * absR[1][2] + a2 * absR[2][2]; + double rb = b2; + double tB = std::abs(tWorld.dot(B.axis[2])); + checkAxisCnt ++; + printf("B2: t = %f, ra + rb = %f\n", tB, ra + rb); + if (tB > ra + rb) return false; + } + + return true; + } + + void GetAxisLog(double &checkAxisAve) { + checkAxisAve = static_cast(checkAxisCnt) / static_cast(checkCnt); + checkAxisCnt = 0; + checkCnt = 0; + } + + void BubbleSortAxisByHitCount(AxisStats *stats) { + if (stats == NULL) { + return; + } + + for (int i = 0; i < AXIS_CNT; ++i) { + if (stats->axisRank[i] == AXIS_DISTANCE_LONG) { + std::swap(stats->axisRank[i], stats->axisRank[0]); + } + stats->hitCount[i] = stats->hitCount[i] / 2; + } + + for (int i = 1; i < AXIS_CNT - 1; ++i) { + bool swapped = false; + for (int j = 1; j < AXIS_CNT - 1 - i; ++j) { + int currAxis = stats->axisRank[j]; + int nextAxis = stats->axisRank[j + 1]; + if (stats->hitCount[currAxis] < stats->hitCount[nextAxis]) { + std::swap(stats->axisRank[j], stats->axisRank[j + 1]); + swapped = true; + } + } + if (!swapped) { + break; + } + } + } + + using CheckCollisionAxisFunc2 = bool(*) + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats); + + bool checkCollisionAxisDistance2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double distance = obb1.maxRadius + obb2.maxRadius; + return distance * distance > tWorld.squaredNorm(); + } + + bool checkCollisionAxisOBB2_A_Axis_0 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0])); + double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1])); + double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2])); + double ra = obb1.halfExtent[0]; + double rb = obb2.halfExtent[0] * r00 + obb2.halfExtent[1] * r01 + obb2.halfExtent[2] * r02; + double t = std::abs(tWorld.dot(obb1.axis[0])); + return t < ra + rb; + } + + bool checkCollisionAxisOBB2_A_Axis_1 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0])); + double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1])); + double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2])); + double ra = obb1.halfExtent[1]; + double rb = obb2.halfExtent[0] * r10 + obb2.halfExtent[1] * r11 + obb2.halfExtent[2] * r12; + double t = std::abs(tWorld.dot(obb1.axis[1])); + return t < ra + rb; + } + + bool checkCollisionAxisOBB2_A_Axis_2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0])); + double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1])); + double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2])); + double ra = obb1.halfExtent[2]; + double rb = obb2.halfExtent[0] * r20 + obb2.halfExtent[1] * r21 + obb2.halfExtent[2] * r22; + double t = std::abs(tWorld.dot(obb1.axis[2])); + return t < ra + rb; + } + + bool checkCollisionAxisOBB2_B_Axis_0 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r00 = std::abs(obb2.axis[0].dot(obb1.axis[0])); + double r10 = std::abs(obb2.axis[0].dot(obb1.axis[1])); + double r20 = std::abs(obb2.axis[0].dot(obb1.axis[2])); + double ra = obb1.halfExtent[0] * r00 + obb1.halfExtent[1] * r10 + obb1.halfExtent[2] * r20; + double rb = obb2.halfExtent[0]; + double t = std::abs(tWorld.dot(obb2.axis[0])); + return t < ra + rb; + } + + bool checkCollisionAxisOBB2_B_Axis_1 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r01 = std::abs(obb2.axis[1].dot(obb1.axis[0])); + double r11 = std::abs(obb2.axis[1].dot(obb1.axis[1])); + double r21 = std::abs(obb2.axis[1].dot(obb1.axis[2])); + double ra = obb1.halfExtent[0] * r01 + obb1.halfExtent[1] * r11 + obb1.halfExtent[2] * r21; + double rb = obb2.halfExtent[1]; + double t = std::abs(tWorld.dot(obb2.axis[1])); + return t < ra + rb; + } + + bool checkCollisionAxisOBB2_B_Axis_2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r02 = std::abs(obb2.axis[2].dot(obb1.axis[0])); + double r12 = std::abs(obb2.axis[2].dot(obb1.axis[1])); + double r22 = std::abs(obb2.axis[2].dot(obb1.axis[2])); + double ra = obb1.halfExtent[0] * r02 + obb1.halfExtent[1] * r12 + obb1.halfExtent[2] * r22; + double rb = obb2.halfExtent[2]; + double t = std::abs(tWorld.dot(obb2.axis[2])); + return t < ra + rb; + } + + bool checkCollisionAxis2A0_Cross_B0 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0])); + double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0])); + double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1])); + double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2])); + double ra = obb1.halfExtent[2] * r10 + obb1.halfExtent[1] * r20; + double rb = obb2.halfExtent[2] * r01 + obb2.halfExtent[1] * r02; + double t = std::abs(tWorld.dot(obb1.axis[2]) * r10 - tWorld.dot(obb1.axis[1]) * r20); + return t < ra + rb; + } + + bool checkCollisionAxis2A0_Cross_B1 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1])); + double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1])); + double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2])); + double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0])); + double ra = obb1.halfExtent[2] * r11 + obb1.halfExtent[1] * r21; + double rb = obb2.halfExtent[0] * r02 + obb2.halfExtent[2] * r00; + double t = std::abs(tWorld.dot(obb1.axis[2]) * r11 - tWorld.dot(obb1.axis[1]) * r21); + return t < ra + rb; + } + + bool checkCollisionAxis2A0_Cross_B2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2])); + double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2])); + double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1])); + double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0])); + double ra = obb1.halfExtent[2] * r12 + obb1.halfExtent[1] * r22; + double rb = obb2.halfExtent[0] * r01 + obb2.halfExtent[1] * r00; + double t = std::abs(tWorld.dot(obb1.axis[2]) * r12 - tWorld.dot(obb1.axis[1]) * r22); + return t < ra + rb; + } + + bool checkCollisionAxis2A1_Cross_B0 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0])); + double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0])); + double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1])); + double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2])); + double ra = obb1.halfExtent[0] * r20 + obb1.halfExtent[2] * r00; + double rb = obb2.halfExtent[2] * r11 + obb2.halfExtent[1] * r12; + double t = std::abs(tWorld.dot(obb1.axis[0]) * r20 - tWorld.dot(obb1.axis[2]) * r00); + return t < ra + rb; + } + + bool checkCollisionAxis2A1_Cross_B1 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1])); + double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1])); + double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2])); + double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0])); + double ra = obb1.halfExtent[0] * r21 + obb1.halfExtent[2] * r01; + double rb = obb2.halfExtent[0] * r12 + obb2.halfExtent[2] * r10; + double t = std::abs(tWorld.dot(obb1.axis[0]) * r21 - tWorld.dot(obb1.axis[2]) * r01); + return t < ra + rb; + } + + bool checkCollisionAxis2A1_Cross_B2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2])); + double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2])); + double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1])); + double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0])); + double ra = obb1.halfExtent[0] * r22 + obb1.halfExtent[2] * r02; + double rb = obb2.halfExtent[0] * r11 + obb2.halfExtent[2] * r10; + double t = std::abs(tWorld.dot(obb1.axis[0]) * r22 - tWorld.dot(obb1.axis[2]) * r02); + return t < ra + rb; + } + + bool checkCollisionAxis2A2_Cross_B0 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r00 = std::abs(obb1.axis[0].dot(obb2.axis[0])); + double r10 = std::abs(obb1.axis[1].dot(obb2.axis[0])); + double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1])); + double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2])); + double ra = obb1.halfExtent[1] * r00 + obb1.halfExtent[0] * r10; + double rb = obb2.halfExtent[2] * r21 + obb2.halfExtent[1] * r22; + double t = std::abs(tWorld.dot(obb1.axis[1]) * r00 - tWorld.dot(obb1.axis[0]) * r10); + return t < ra + rb; + } + + bool checkCollisionAxis2A2_Cross_B1 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r01 = std::abs(obb1.axis[0].dot(obb2.axis[1])); + double r11 = std::abs(obb1.axis[1].dot(obb2.axis[1])); + double r22 = std::abs(obb1.axis[2].dot(obb2.axis[2])); + double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0])); + double ra = obb1.halfExtent[1] * r01 + obb1.halfExtent[0] * r11; + double rb = obb2.halfExtent[0] * r22 + obb2.halfExtent[2] * r20; + double t = std::abs(tWorld.dot(obb1.axis[1]) * r01 - tWorld.dot(obb1.axis[0]) * r11); + return t < ra + rb; + } + + bool checkCollisionAxis2A2_Cross_B2 + (const OBB &obb1, const OBB &obb2, const Vector3d &tWorld, AxisStats *stats) { + UNUSED(stats); + double r02 = std::abs(obb1.axis[0].dot(obb2.axis[2])); + double r12 = std::abs(obb1.axis[1].dot(obb2.axis[2])); + double r20 = std::abs(obb1.axis[2].dot(obb2.axis[0])); + double r21 = std::abs(obb1.axis[2].dot(obb2.axis[1])); + double ra = obb1.halfExtent[1] * r02 + obb1.halfExtent[0] * r12; + double rb = obb2.halfExtent[1] * r20 + obb2.halfExtent[0] * r21; + double t = std::abs(tWorld.dot(obb1.axis[1]) * r02 - tWorld.dot(obb1.axis[0]) * r12); + return t < ra + rb; + } + + CheckCollisionAxisFunc2 checkCollisionAxisFunc2[AXIS_CNT] = { + checkCollisionAxisDistance2, + checkCollisionAxisOBB2_A_Axis_0, + checkCollisionAxisOBB2_A_Axis_1, + checkCollisionAxisOBB2_A_Axis_2, + checkCollisionAxisOBB2_B_Axis_0, + checkCollisionAxisOBB2_B_Axis_1, + checkCollisionAxisOBB2_B_Axis_2, + checkCollisionAxis2A0_Cross_B0, + checkCollisionAxis2A0_Cross_B1, + checkCollisionAxis2A0_Cross_B2, + checkCollisionAxis2A1_Cross_B0, + checkCollisionAxis2A1_Cross_B1, + checkCollisionAxis2A1_Cross_B2, + checkCollisionAxis2A2_Cross_B0, + checkCollisionAxis2A2_Cross_B1, + checkCollisionAxis2A2_Cross_B2, + }; + + bool checkOBBCollisionNew(const OBB &A, const OBB &B, AxisStats *stats) { + checkCnt++; + Vector3d tWorld = B.center - A.center; + + for (int axisIndex = 1; axisIndex < AXIS_CNT; ++axisIndex) { + checkAxisCnt++; + int axis = stats->axisRank[0]; + if (!checkCollisionAxisFunc2[axis](A, B, tWorld, stats)) { + stats->hitCount[axis]++; + return false; + } else { + stats->hitCount[axis]--; + std::swap(stats->axisRank[0], stats->axisRank[axisIndex]); + } + } + checkAxisCnt++; + return checkCollisionAxisFunc2[stats->axisRank[0]](A, B, tWorld, stats); + } } diff --git a/rm_arm_control/CMakeLists.txt b/rm_arm_control/CMakeLists.txt index 0b8cd9b..938c18e 100644 --- a/rm_arm_control/CMakeLists.txt +++ b/rm_arm_control/CMakeLists.txt @@ -8,14 +8,14 @@ set(CMAKE_C_STANDARD 11) if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID STREQUAL "Clang") add_compile_options(-Wno-define-redefinition) endif() +add_compile_options(-march=native -O3 -DEIGEN_VECTORIZE_SSE4_2) add_compile_options(-g) #set(CMAKE_BUILD_TYPE Release) -# 全局设置 C++ 编译选项 -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") -# 全局设置 C 编译选项 -# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") +# 全局设置 C++/C 编译选项 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3") set(ROS_DOMAIN_ID_STR $ENV{ROS_DOMAIN_ID}) if(NOT ROS_DOMAIN_ID_STR) @@ -68,6 +68,7 @@ set(SOURCES simulator/rm_arm_simulator.cpp simulator/trapezoidal_trajectory_kinematics.cpp simulator/rm_driver_kinematics.cpp + simulator/col_simulator.cpp src/goal_manager.cpp driver/arm_driver.cpp driver/rm_arm_driver.cpp diff --git a/rm_arm_control/include/col_simulator.hpp b/rm_arm_control/include/col_simulator.hpp index 23f6ca8..7e5c2e2 100644 --- a/rm_arm_control/include/col_simulator.hpp +++ b/rm_arm_control/include/col_simulator.hpp @@ -47,15 +47,15 @@ struct JointsInfo { struct CollisionStructureInfo { std::string link_name; // 所属链接名称 - std::string geometry_type; // 几何类型 + int geometry_type; // 几何类型 std::string mesh_filename; // mesh文件路径（如STL） std::vector obbModelList; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z] std::vector obbLinkIndexList; // 网格边界盒所属链接索引 + std::vector jointIndexList; // 网格边界盒所属关节索引 Eigen::Quaterniond rotation[COLLISION_CHECK_SIMPLE_CNT]; // 链接旋转 Eigen::Vector3d offset[COLLISION_CHECK_SIMPLE_CNT]; // 链接偏移 - bool rotation_add_flag; - std::vector joint_list; + int joint_count; int link_index; int parent_link_index; @@ -64,30 +64,30 @@ struct CollisionStructureInfo { class CollisionSimulator { public: - CollisionSimulator(std::string urdf_string, std::string srdf_string, float **jointsList, KinematicsManager **trajectory); + CollisionSimulator(std::string robot_string, std::string srdf_string, float **jointsList, KinematicsManager **trajectory); ~CollisionSimulator(); - int CheckCollision(); + int CheckCollision(rclcpp::Node *node); int UpdateCollisionForNewGoal(int bodyType); int AddPolyhedron(int id, const Polyhedron& poly); int RemovePolyhedron(int id); + int CheckArmJointsAngleLimit(float *joints); private: - void InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory); - int CheckJointsAngleLimit(int simple, int bodyType); - int InitPolyhedrons(const urdf::Model &urdf_model); + int CheckBodyAngleLimit(int simple, int bodyType); int CheckCollisionInner(std::vector *addedOBBs, std::vector *addedOBBsNext_); + int CheckCollisionInnerOld(std::vector *addedOBBs, std::vector *addedOBBsNext_); void UpdatePolyhedrons(int bodyType, int simple); - int InitCollisionStructureList(const urdf::Model &urdf_model); - int GetMeshLinkdimensions(const std::string& filename, CollisionStructureInfo& col_struct); - int InitJoint(const urdf::Model &urdf_model); - int InitJointModel(); - void InitPolyhedronsList(); int InitLinkCollisionDetectMatrix(const std::string& srdf_str); + void InitPolyhedronsList(); + + int InitXmlStrFromFile(const std::string& robot_string); + int InitJoint(); KinematicsManager *trajectory_[BODY_CNT]; std::vector polyhedrons_[COLLISION_CHECK_SIMPLE_CNT]; std::vector addedOBBs_[COLLISION_CHECK_SIMPLE_CNT]; + std::map obbNameMap_; std::vector collision_structures_; std::vector jointMap_; @@ -99,6 +99,11 @@ private: std::unordered_map addedPolyhedrons_; std::map linkCollisionMap_; + + // Collision pair enable matrix (flattened upper-triangle including i linkCollisionDetectMatrix_; + // Precomputed list of enabled OBB pairs to check + std::vector> tuplesToCheck_; }; #endif // COL_SIMULATOR_HPP \ No newline at end of file diff --git a/rm_arm_control/include/rm_arm_simulator.hpp b/rm_arm_control/include/rm_arm_simulator.hpp index 80ce902..3d08309 100644 --- a/rm_arm_control/include/rm_arm_simulator.hpp +++ b/rm_arm_control/include/rm_arm_simulator.hpp @@ -63,7 +63,7 @@ public: ~ArmSimulator(); int CheckCollision(); - int CheckJointsAngleLimit(float *joints); + int CheckArmJointsAngleLimit(float *joints); int UpdateCollisionForNewGoal(int arm); int AddPolyhedron(int id, const Polyhedron& poly); int RemovePolyhedron(int id); diff --git a/rm_arm_control/launch/rm_arm_control.launch.py b/rm_arm_control/launch/rm_arm_control.launch.py index ef91260..f2fdf2e 100644 --- a/rm_arm_control/launch/rm_arm_control.launch.py +++ b/rm_arm_control/launch/rm_arm_control.launch.py @@ -78,19 +78,9 @@ def open_file(file_name, context): def load_urdf(context, *args, **kwargs): nodeList = [] - # 获取包的共享目录路径（解析为实际字符串） - package_share = FindPackageShare(package='rm_arm_control').perform(context) - - # 构建URDF文件的实际路径 - urdf_path = PathJoinSubstitution( - [package_share, 'urdf', 'FHrobot.urdf'] # 替换为你的URDF路径 - ).perform(context) - - # 读取URDF文件内容 - with open(urdf_path, 'r') as f: - urdf_content = f.read() - srdf_content = open_file('FHrobot.srdf', context) + urdf_content = open_file('FHrobot.urdf', context) + xml_content = open_file('FHrobot.xml', context) robot_controller = Node( package='rm_arm_control', @@ -100,6 +90,7 @@ def load_urdf(context, *args, **kwargs): parameters=[{ 'robot_description': urdf_content, 'robot_description_semantic': srdf_content, + 'robot_description_xml': xml_content, 'use_sim_time': False }] ) diff --git a/rm_arm_control/simulator/col_simulator.cpp b/rm_arm_control/simulator/col_simulator.cpp index 449a71b..4a9dae5 100644 --- a/rm_arm_control/simulator/col_simulator.cpp +++ b/rm_arm_control/simulator/col_simulator.cpp @@ -1,27 +1,19 @@ -#include #include -#include #include #include #include -#include -#include -#include #include "col_simulator.hpp" -typedef CGAL::Exact_predicates_inexact_constructions_kernel CgalK; -typedef CgalK::Point_3 CgalPoint; - 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}; +const int g_bodyJointCnt[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}; const std::string g_bodyStartName[BODY_CNT] = {"LeftLink1", "RightLink1", "base_link"}; -bool *g_linkCollisionDetectMatrix = nullptr; +static float zeroFixedAngle = 0; int g_linkStartIndex[BODY_CNT] = {0}; @@ -31,37 +23,7 @@ static double toRadians(float degrees) { return (static_cast(degrees)) * (M_PI / 180.0); } -int CollisionSimulator::InitJointModel() -{ - int linkIndex = rootLinkIndex_; - CollisionStructureInfo *colStruct = &collision_structures_[linkIndex]; - std::vector jointList; - int index = 0; - int maxJointCount = 0; - Quaterniond rotationAll = Quaterniond::Identity(); - while (colStruct->joint_count > 0 || index < maxJointCount) { - for (int i = 0; i < colStruct->joint_count; i++) { - jointList.push_back(&colStruct->joint_list[i]); - maxJointCount++; - colStruct->joint_list[i].rotation = rotationAll * colStruct->joint_list[i].rotation; - colStruct->joint_list[i].axis = colStruct->joint_list[i].rotation * colStruct->joint_list[i].axis; - colStruct->joint_list[i].nextLinkOffset = rotationAll * colStruct->joint_list[i].nextLinkOffset; - } - JointsInfo *jointInfo = jointList[index]; - rotationAll = jointInfo->rotation; - index++; - linkIndex = jointInfo->child_link; - colStruct = &collision_structures_[linkIndex]; - for (int i = 0; i < colStruct->obbModelList.size(); i++) { - colStruct->obbModelList[i] = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i], - Vector3d::Zero(), - rotationAll); - } - } - return 0; -} - -int CollisionSimulator::InitJoint(const urdf::Model &urdf_model) +int CollisionSimulator::InitJoint() { const std::map jointNameToIndexMap = { {"left_joint1", 0}, {"left_joint2", 1}, {"left_joint3", 2}, {"left_joint4", 3}, {"left_joint5", 4}, {"left_joint6", 5}, @@ -83,168 +45,32 @@ int CollisionSimulator::InitJoint(const urdf::Model &urdf_model) {"right_front_roll_joint", BODY_TYPE_OTHERS}, {"right_front_hip_joint", BODY_TYPE_OTHERS}, {"right_front_knee_joint", BODY_TYPE_OTHERS}, {"right_front_wheel_joint", BODY_TYPE_OTHERS} }; - for (const auto& joint_pair : urdf_model.joints_) { - urdf::JointConstSharedPtr joint = joint_pair.second; - JointsInfo joint_info; - joint_info.name = joint->name; - joint_info.type = joint->type; - joint_info.parent_link = -1; - joint_info.child_link = -1; - if (joint->parent_link_name != "") { - if (linkCollisionMap_.find(joint->parent_link_name) != linkCollisionMap_.end()) { - joint_info.parent_link = linkCollisionMap_[joint->parent_link_name]; - } else { - RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s 的父链接 %s 未找到！", joint->name.c_str(), joint->parent_link_name.c_str()); - return -1; - } - } - if (joint->child_link_name != "") { - if (linkCollisionMap_.find(joint->child_link_name) != linkCollisionMap_.end()) { - joint_info.child_link = linkCollisionMap_[joint->child_link_name]; - } else { - RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s 的子链接 %s 未找到！", joint->name.c_str(), joint->child_link_name.c_str()); - return -1; - } - } - joint_info.nextLinkOffset = 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); - joint_info.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->type != urdf::Joint::FIXED) { - int jointAngleIndex = jointNameToIndexMap.at(joint->name); - int bodyType = jointNameToBodyType.at(joint->name); + for (JointsInfo &joint : jointMap_) { + if (joint.type != urdf::Joint::FIXED) { + int jointAngleIndex = jointNameToIndexMap.at(joint.name); + int bodyType = jointNameToBodyType.at(joint.name); for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { - joint_info.angle[i] = &joints_[i][bodyType][jointAngleIndex]; - } - joint_info.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z); - if (joint->limits != NULL) { - joint_info.limit_lower = joint->limits->lower; - joint_info.limit_upper = joint->limits->upper; - joint_info.limit_v = joint->limits->velocity; - joint_info.limit_e = joint->limits->effort; + joint.angle[i] = &joints_[i][bodyType][jointAngleIndex]; } } else { - joint_info.axis = Vector3d::Zero(); - joint_info.limit_lower = 0.0; - joint_info.limit_upper = 0.0; - joint_info.limit_v = 0.0; - joint_info.limit_e = 0.0; - } - jointMap_.push_back(joint_info); - CollisionStructureInfo *parentColStruct = &collision_structures_[joint_info.parent_link]; - parentColStruct->joint_list.push_back(joint_info); - parentColStruct->joint_count++; - CollisionStructureInfo *childColStruct = &collision_structures_[joint_info.child_link]; - childColStruct->parent_link_index = joint_info.parent_link; - } - InitJointModel(); - return 0; -} - -int CollisionSimulator::GetMeshLinkdimensions(const std::string& filename, CollisionStructureInfo& col_struct) -{ - shapes::Mesh* mesh = shapes::createMeshFromResource(filename); - if (!mesh) { - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法加载网格文件: %s", filename.c_str()); - return false; - } - - std::vector eigen_points; - - std::vector points; - - for (unsigned int i = 0; i < mesh->vertex_count; ++i) { - points.emplace_back(mesh->vertices[3*i], mesh->vertices[3*i + 1], mesh->vertices[3*i + 2]); - } - std::array obb_points; - CGAL::oriented_bounding_box(points, obb_points, CGAL::parameters::use_convex_hull(true)); - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "计算%s得到的 OBB 顶点:", col_struct.link_name.c_str()); - for (const auto& pt : obb_points) { - eigen_points.push_back(Eigen::Vector3d(pt.x(), pt.y(), pt.z())); - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "(%.4f, %.4f, %.4f)", pt.x(), pt.y(), pt.z()); - } - - OBB obb = gjk_interface::createOBBFromPoints(eigen_points); - col_struct.obbModelList.push_back(obb); - return 0; -} - -int CollisionSimulator::InitCollisionStructureList(const urdf::Model &urdf_model) -{ - int result = OK; - int linkIndex = 0; - for (const auto& link_pair : urdf_model.links_) { - // link_pair是一个键值对: 键为链接名称，值为链接指针 - urdf::LinkConstSharedPtr link = link_pair.second; - for (const auto& collision : link->collision_array) { - CollisionStructureInfo colStruct; - colStruct.link_name = link->name; - colStruct.geometry_type = collision->geometry->type; - linkCollisionMap_[link->name] = linkIndex; - int obbCnt = 1; - if (collision->geometry->type == urdf::Geometry::BOX) { - urdf::Box* box = dynamic_cast(collision->geometry.get()); - std::vector obb_points = { - Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2), - Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, -box->dim.z / 2), - Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2), - Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, -box->dim.z / 2), - Eigen::Vector3d(-box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2), - Eigen::Vector3d(box->dim.x / 2, -box->dim.y / 2, box->dim.z / 2), - Eigen::Vector3d(-box->dim.x / 2, box->dim.y / 2, box->dim.z / 2), - Eigen::Vector3d(box->dim.x / 2, box->dim.y / 2, box->dim.z / 2) - }; - OBB obb = gjk_interface::createOBBFromPoints(obb_points); - colStruct.obbModelList.push_back(obb); - } else if (collision->geometry->type == urdf::Geometry::MESH) { - urdf::Mesh* mesh = dynamic_cast(collision->geometry.get()); - colStruct.mesh_filename = mesh->filename; - result = GetMeshLinkdimensions(mesh->filename, colStruct); - if (result != OK) { - RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to get mesh dimensions for %s", mesh->filename.c_str()); - return result; - } + for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { + joint.angle[i] = &zeroFixedAngle; } - colStruct.link_index = linkIndex; - for (int i = 0; i < obbCnt; i++) { - colStruct.obbLinkIndexList.push_back(maxObbCnt); - for (int j = 0; j < COLLISION_CHECK_SIMPLE_CNT; j++) { - addedOBBs_[j].push_back(colStruct.obbModelList[i]); - } - maxObbCnt++; - } - colStruct.joint_count = 0; - for (int i = 0; i < BODY_CNT; i++) { - if (link->name == g_bodyStartName[i]) { - g_linkStartIndex[i] = linkIndex; - if (i == BODY_TYPE_OTHERS) { - rootLinkIndex_ = linkIndex; - colStruct.parent_link_index = -1; - for (int j = 0; j < COLLISION_CHECK_SIMPLE_CNT; j++) { - colStruct.rotation[j] = Quaterniond::Identity(); - colStruct.offset[j] = Vector3d::Zero(); - } - } - break; - } - } - collision_structures_.push_back(colStruct); - linkIndex++; } } - return result; + return 0; } int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_str) { - g_linkCollisionDetectMatrix = new bool[maxObbCnt * maxObbCnt]; + if (maxObbCnt <= 0) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InitLinkCollisionDetectMatrix: invalid maxObbCnt=%d", maxObbCnt); + return -1; + } + linkCollisionDetectMatrix_.assign(static_cast(maxObbCnt) * static_cast(maxObbCnt), 0); for (int i = 0; i < maxObbCnt; ++i) { for (int j = i + 1; j < maxObbCnt; ++j) { - g_linkCollisionDetectMatrix[i * maxObbCnt + j] = true; + linkCollisionDetectMatrix_[static_cast(i) * static_cast(maxObbCnt) + static_cast(j)] = 1; } } tinyxml2::XMLDocument doc; @@ -265,8 +91,14 @@ int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_st for (tinyxml2::XMLElement* dc_elem = robot_root->FirstChildElement("disable_collisions"); dc_elem; dc_elem = dc_elem->NextSiblingElement("disable_collisions")) { - std::string link1 = dc_elem->Attribute("link1"); - std::string link2 = dc_elem->Attribute("link2"); + const char* link1_c = dc_elem->Attribute("link1"); + const char* link2_c = dc_elem->Attribute("link2"); + if (!link1_c || !link2_c) { + RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "disable_collisions missing link1/link2 attribute, skipping."); + continue; + } + std::string link1 = link1_c; + std::string link2 = link2_c; int link1Index = linkCollisionMap_.find(link1) != linkCollisionMap_.end() ? linkCollisionMap_[link1] : -1; int link2Index = linkCollisionMap_.find(link2) != linkCollisionMap_.end() ? linkCollisionMap_[link2] : -1; if (link1Index != -1 && link2Index != -1) { @@ -274,126 +106,377 @@ int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_st CollisionStructureInfo* colStruct2 = &collision_structures_[link2Index]; for (int obb1Idx : colStruct1->obbLinkIndexList) { for (int obb2Idx : colStruct2->obbLinkIndexList) { - g_linkCollisionDetectMatrix[obb1Idx * maxObbCnt + obb2Idx] = false; - g_linkCollisionDetectMatrix[obb2Idx * maxObbCnt + obb1Idx] = false; + if (obb1Idx >= 0 && obb1Idx < maxObbCnt && obb2Idx >= 0 && obb2Idx < maxObbCnt) { + linkCollisionDetectMatrix_[ + static_cast(obb1Idx) * static_cast(maxObbCnt) + static_cast(obb2Idx)] = 0; + linkCollisionDetectMatrix_[ + static_cast(obb2Idx) * static_cast(maxObbCnt) + static_cast(obb1Idx)] = 0; + } else { + RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), + "disable_collisions references out-of-range OBB index (%d,%d) with maxObbCnt=%d", + obb1Idx, obb2Idx, maxObbCnt); + } } } } disable_count++; } - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initialized link collision detect matrix, disabled %d collision pairs.", disable_count); + // Build tuplesToCheck_ list + tuplesToCheck_.clear(); + tuplesToCheck_.reserve(static_cast(maxObbCnt) * (static_cast(maxObbCnt) - 1) / 2); + for (int i = 0; i < maxObbCnt; ++i) { + for (int j = i + 1; j < maxObbCnt; ++j) { + if (linkCollisionDetectMatrix_[static_cast(i) * static_cast(maxObbCnt) + static_cast(j)]) { + AxisStats *stats = new AxisStats(); + tuplesToCheck_.emplace_back(i, j, stats); + } + } + } + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), + "Initialized link collision detect matrix, disabled %d collision pairs, enabled pairs: %zu.", + disable_count, tuplesToCheck_.size()); return 0; } -int CollisionSimulator::InitPolyhedrons(const urdf::Model &urdf_model) +int CollisionSimulator::InitXmlStrFromFile(const std::string& robot_string) { + tinyxml2::XMLDocument doc; + tinyxml2::XMLError error = doc.Parse(robot_string.c_str()); + if (error != tinyxml2::XML_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to parse SRDF: %s, length: %ld", doc.ErrorStr(), robot_string.length()); + return -1; + } - InitCollisionStructureList(urdf_model); - InitJoint(urdf_model); + tinyxml2::XMLElement* robot_root = doc.FirstChildElement("robot"); + if (!robot_root) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF has no root node"); + return -1; + } + if (!robot_root->Attribute("maxObbCnt")) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF missing required attribute maxObbCnt"); + return -1; + } + rootLinkIndex_ = robot_root->IntAttribute("rootLinkIndex_", 0); + maxObbCnt = robot_root->IntAttribute("maxObbCnt", 0); + if (maxObbCnt <= 0) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid maxObbCnt=%d parsed from SRDF", maxObbCnt); + return -1; + } + for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { + addedOBBs_[i].resize(maxObbCnt); + } + int collision_structure_count = robot_root->IntAttribute("collision_structure_count", 0); + int joint_count = robot_root->IntAttribute("joint_count", 0); + if (collision_structure_count <= 0 || joint_count <= 0) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Invalid counts: collision_structure_count=%d, joint_count=%d", collision_structure_count, joint_count); + return -1; + } + collision_structures_.resize(collision_structure_count); + jointMap_.resize(joint_count); + + int obbCnt = 0; + for (tinyxml2::XMLElement* link_elem = robot_root->FirstChildElement("collision_model"); + link_elem; + link_elem = link_elem->NextSiblingElement("collision_model")) { + CollisionStructureInfo col_struct; + int linkIndex = col_struct.link_index = link_elem->IntAttribute("link_index", -1); + const char* link_name_c = link_elem->Attribute("link_name"); + if (!link_name_c || linkIndex < 0 || linkIndex >= collision_structure_count) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "collision_model missing/invalid link_name or link_index=%d", linkIndex); + return -1; + } + std::string linkName = col_struct.link_name = link_name_c; + col_struct.geometry_type = (link_elem->IntAttribute("geometry_type", 0)); + col_struct.mesh_filename = link_elem->Attribute("mesh_filename") ? link_elem->Attribute("mesh_filename") : ""; + col_struct.joint_count = 0; + col_struct.parent_link_index = link_elem->IntAttribute("parent_link_index", -1); + + for (tinyxml2::XMLElement* obb_elem = link_elem->FirstChildElement("obb"); + obb_elem; + obb_elem = obb_elem->NextSiblingElement("obb")) { + OBB obb; + obb.center = Vector3d( + obb_elem->DoubleAttribute("center_x", 0.0), + obb_elem->DoubleAttribute("center_y", 0.0), + obb_elem->DoubleAttribute("center_z", 0.0)); + obb.axis[0] = Vector3d( + obb_elem->DoubleAttribute("axis_0_x", 1.0), + obb_elem->DoubleAttribute("axis_0_y", 0.0), + obb_elem->DoubleAttribute("axis_0_z", 0.0)); + obb.axis[1] = Vector3d( + obb_elem->DoubleAttribute("axis_1_x", 0.0), + obb_elem->DoubleAttribute("axis_1_y", 1.0), + obb_elem->DoubleAttribute("axis_1_z", 0.0)); + obb.axis[2] = Vector3d( + obb_elem->DoubleAttribute("axis_2_x", 0.0), + obb_elem->DoubleAttribute("axis_2_y", 0.0), + obb_elem->DoubleAttribute("axis_2_z", 1.0)); + obb.halfExtent[0] = obb_elem->DoubleAttribute("half_extent_x", 0.5); + obb.halfExtent[1] = obb_elem->DoubleAttribute("half_extent_y", 0.5); + obb.halfExtent[2] = obb_elem->DoubleAttribute("half_extent_z", 0.5); + obb.maxRadius = obb_elem->DoubleAttribute("max_radius", 0.0); + obb.selfMinProject[0] = obb_elem->DoubleAttribute("self_min_project_x", 0.0); + obb.selfMinProject[1] = obb_elem->DoubleAttribute("self_min_project_y", 0.0); + obb.selfMinProject[2] = obb_elem->DoubleAttribute("self_min_project_z", 0.0); + obb.selfMaxProject[0] = obb_elem->DoubleAttribute("self_max_project_x", 0.0); + obb.selfMaxProject[1] = obb_elem->DoubleAttribute("self_max_project_y", 0.0); + obb.selfMaxProject[2] = obb_elem->DoubleAttribute("self_max_project_z", 0.0); + gjk_interface::getSelfProject(obb); + col_struct.obbModelList.push_back(obb); + obbCnt++; + int obbIndex = obb_elem->IntAttribute("index", -1); + if (obbIndex < 0 || obbIndex >= maxObbCnt) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB index out of range: %d (max %d) for link %s", obbIndex, maxObbCnt, linkName.c_str()); + return -1; + } + col_struct.obbLinkIndexList.push_back(obbIndex); + obbNameMap_[obbIndex] = linkName; + + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded OBB for link %s: center(%.2f, %.2f, %.2f), halfExtent(%.2f, %.2f, %.2f).", + linkName.c_str(), + obb.center.x(), obb.center.y(), obb.center.z(), + obb.halfExtent[0], obb.halfExtent[1], obb.halfExtent[2]); + } + + for (tinyxml2::XMLElement* joint_elem = link_elem->FirstChildElement("joint_index"); + joint_elem; + joint_elem = joint_elem->NextSiblingElement("joint_index")) { + int idx = joint_elem->IntAttribute("index", -1); + if (idx < 0 || idx >= joint_count) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_index out of range: %d (joint_count %d) for link %s", idx, joint_count, linkName.c_str()); + return -1; + } + col_struct.jointIndexList.push_back(idx); + col_struct.joint_count++; + } + + collision_structures_[linkIndex] = col_struct; + linkCollisionMap_[linkName] = linkIndex; + for (int i = 0; i < BODY_CNT; i++) { + if (g_bodyStartName[i] == linkName) { + g_linkStartIndex[i] = linkIndex; + break; + } + } + } + + for (tinyxml2::XMLElement* joint_elem = robot_root->FirstChildElement("joint_model"); + joint_elem; + joint_elem = joint_elem->NextSiblingElement("joint_model")) { + JointsInfo joint_info; + int joint_index = joint_elem->IntAttribute("joint_index", -1); + const char* joint_name_c = joint_elem->Attribute("joint_name"); + if (!joint_name_c || joint_index < 0 || joint_index >= joint_count) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model missing/invalid joint_name or joint_index=%d", joint_index); + return -1; + } + joint_info.name = joint_name_c; + joint_info.type = (joint_elem->IntAttribute("type", 0)); + joint_info.parent_link = joint_elem->IntAttribute("parent_link", -1); + joint_info.child_link = joint_elem->IntAttribute("child_link", -1); + joint_info.limit_lower = joint_elem->DoubleAttribute("limit_lower", 0.0); + joint_info.limit_upper = joint_elem->DoubleAttribute("limit_upper", 0.0); + joint_info.limit_v = joint_elem->DoubleAttribute("limit_v", 0.0); + joint_info.limit_e = joint_elem->DoubleAttribute("limit_e", 0.0); + joint_info.nextLinkOffset = Vector3d( + joint_elem->DoubleAttribute("nextLinkOffset_x", 0.0), + joint_elem->DoubleAttribute("nextLinkOffset_y", 0.0), + joint_elem->DoubleAttribute("nextLinkOffset_z", 0.0)); + joint_info.rotation = Quaterniond( + joint_elem->DoubleAttribute("rotation_w", 1.0), + joint_elem->DoubleAttribute("rotation_x", 0.0), + joint_elem->DoubleAttribute("rotation_y", 0.0), + joint_elem->DoubleAttribute("rotation_z", 0.0)); + joint_info.axis = Vector3d( + joint_elem->DoubleAttribute("axis_x", 1.0), + joint_elem->DoubleAttribute("axis_y", 0.0), + joint_elem->DoubleAttribute("axis_z", 0.0)); + if (joint_info.child_link < 0 || joint_info.child_link >= collision_structure_count || + joint_info.parent_link < 0 || joint_info.parent_link >= collision_structure_count) { + RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "joint_model link indices out of range for joint %s", joint_info.name.c_str()); + return -1; + } + jointMap_[joint_index] = joint_info; + + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Loaded joint %s: axis(%.2f, %.2f, %.2f), offset(%.2f, %.2f, %.2f).", + joint_info.name.c_str(), joint_info.axis.x(), joint_info.axis.y(), joint_info.axis.z(), + joint_info.nextLinkOffset.x(), joint_info.nextLinkOffset.y(), joint_info.nextLinkOffset.z()); + } + + for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) { + collision_structures_[rootLinkIndex_].rotation[i] = Quaterniond::Identity(); + collision_structures_[rootLinkIndex_].offset[i] = Vector3d::Zero(); + } + + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Initialized collision structures from XML, total links: %ld, total OBBs: %d.", + collision_structures_.size(), obbCnt); return 0; } -void CollisionSimulator::InitCollisionSimulator(const urdf::Model &urdf_model, float **jointsList, KinematicsManager **trajectory) +CollisionSimulator::CollisionSimulator(std::string robot_string, std::string srdf_string, float **jointsList, KinematicsManager **trajectory) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CollisionSimulator: 初始化机械臂碰撞检测模块..."); + 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_TYPE_OTHERS; j++) { - for (int k = 0; k < g_jointLinkCnt[j]; k++) { + for (int k = 0; k < g_bodyJointCnt[j]; k++) { joints_[i][j][k] = jointsList[j][k]; } } - for (int j = 0; j < g_jointLinkCnt[BODY_TYPE_OTHERS]; j++) { + for (int j = 0; j < g_bodyJointCnt[BODY_TYPE_OTHERS]; j++) { joints_[i][BODY_TYPE_OTHERS][j] = 0; } } - - InitPolyhedrons(urdf_model); - InitPolyhedronsList(); -} - -CollisionSimulator::CollisionSimulator(std::string urdf_string, std::string srdf_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; + for (int i = 0; i < BODY_TYPE_OTHERS; ++i) { + trajectory_[i] = trajectory[i]; } - InitCollisionSimulator(urdf_model, jointsList, trajectory); + simpleUpdateIndex_ = 0; + InitXmlStrFromFile(robot_string); + + InitJoint(); + InitPolyhedronsList(); InitLinkCollisionDetectMatrix(srdf_string); } CollisionSimulator::~CollisionSimulator() { - if (g_linkCollisionDetectMatrix != nullptr) { - delete[] g_linkCollisionDetectMatrix; - g_linkCollisionDetectMatrix = nullptr; + for (const auto& p : tuplesToCheck_) { + delete std::get<2>(p); } + tuplesToCheck_.clear(); } +int flashStateIndex = 0; int CollisionSimulator::CheckCollisionInner(std::vector *addedOBBs, std::vector *addedOBBsNext_) { - int result = OK; - for (int i = 0; i < maxObbCnt; ++i) { - for (int j = i + 1; j < maxObbCnt; ++j) { - if (!g_linkCollisionDetectMatrix[i * maxObbCnt + 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; - } + bool inCollision = false; + OBB *obb1 = NULL; + OBB *obb2 = NULL; + for (const auto& p : tuplesToCheck_) { + const int i = std::get<0>(p); + const int j = std::get<1>(p); + AxisStats *stats = std::get<2>(p); + inCollision = gjk_interface::checkOBBCollisionNew((*addedOBBsNext_)[i], (*addedOBBsNext_)[j], stats); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision b", obbNameMap_[i].c_str(), obbNameMap_[j].c_str()); + obb1 = &(*addedOBBsNext_)[i]; + obb2 = &(*addedOBBsNext_)[j]; + break; + } + inCollision = gjk_interface::checkOBBCollisionNew((*addedOBBs)[i], (*addedOBBsNext_)[j], stats); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision c", obbNameMap_[i].c_str(), obbNameMap_[j].c_str()); + obb1 = &(*addedOBBs)[i]; + obb2 = &(*addedOBBsNext_)[j]; + break; } } - return result; + AxisStats *stats = std::get<2>(tuplesToCheck_[flashStateIndex]); + gjk_interface::BubbleSortAxisByHitCount(stats); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb1: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)", + obb1->center.x(), obb1->center.y(), obb1->center.z(), + obb1->axis[0].x(), obb1->axis[0].y(), obb1->axis[0].z(), + obb1->axis[1].x(), obb1->axis[1].y(), obb1->axis[1].z(), + obb1->axis[2].x(), obb1->axis[2].y(), obb1->axis[2].z(), + obb1->halfExtent[0], obb1->halfExtent[1], obb1->halfExtent[2]); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb2: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)", + obb2->center.x(), obb2->center.y(), obb2->center.z(), + obb2->axis[0].x(), obb2->axis[0].y(), obb2->axis[0].z(), + obb2->axis[1].x(), obb2->axis[1].y(), obb2->axis[1].z(), + obb2->axis[2].x(), obb2->axis[2].y(), obb2->axis[2].z(), + obb2->halfExtent[0], obb2->halfExtent[1], obb2->halfExtent[2]); + return ARM_COLLISION; + } + return OK; +} + +int CollisionSimulator::CheckCollisionInnerOld(std::vector *addedOBBs, std::vector *addedOBBsNext_) +{ + bool inCollision = false; + OBB *obb1 = NULL; + OBB *obb2 = NULL; + for (const auto& p : tuplesToCheck_) { + const int i = std::get<0>(p); + const int j = std::get<1>(p); + inCollision = gjk_interface::checkOBBCollision((*addedOBBsNext_)[i], (*addedOBBsNext_)[j]); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision b", obbNameMap_[i].c_str(), obbNameMap_[j].c_str()); + obb1 = &(*addedOBBsNext_)[i]; + obb2 = &(*addedOBBsNext_)[j]; + break; + } + inCollision = gjk_interface::checkOBBCollision((*addedOBBs)[i], (*addedOBBsNext_)[j]); + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "dect %s and %s collision c", obbNameMap_[i].c_str(), obbNameMap_[j].c_str()); + obb1 = &(*addedOBBs)[i]; + obb2 = &(*addedOBBsNext_)[j]; + break; + } + } + if (inCollision) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb1: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)", + obb1->center.x(), obb1->center.y(), obb1->center.z(), + obb1->axis[0].x(), obb1->axis[0].y(), obb1->axis[0].z(), + obb1->axis[1].x(), obb1->axis[1].y(), obb1->axis[1].z(), + obb1->axis[2].x(), obb1->axis[2].y(), obb1->axis[2].z(), + obb1->halfExtent[0], obb1->halfExtent[1], obb1->halfExtent[2]); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "obb2: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), half_lengths(%.4f, %.4f, %.4f)", + obb2->center.x(), obb2->center.y(), obb2->center.z(), + obb2->axis[0].x(), obb2->axis[0].y(), obb2->axis[0].z(), + obb2->axis[1].x(), obb2->axis[1].y(), obb2->axis[1].z(), + obb2->axis[2].x(), obb2->axis[2].y(), obb2->axis[2].z(), + obb2->halfExtent[0], obb2->halfExtent[1], obb2->halfExtent[2]); + return ARM_COLLISION; + } + return OK; } void CollisionSimulator::UpdatePolyhedrons(int bodyType, int simple) { int linkIndex = g_linkStartIndex[bodyType]; CollisionStructureInfo *colStruct = &collision_structures_[linkIndex]; - std::vector jointList; - int index = 0; - int maxJointCount = 0; - while (colStruct->joint_count > 0 || index < maxJointCount) { + std::vector child = {colStruct}; + int childIndex = 0; + int childCnt = 1; + while (childIndex < childCnt) { + colStruct = child[childIndex]; + childIndex++; for (int i = 0; i < colStruct->joint_count; i++) { - jointList.push_back(&colStruct->joint_list[i]); - maxJointCount++; - } - JointsInfo *jointInfo = jointList[index]; - Quaterniond rotation = colStruct->rotation[simple]; - Vector3d offset = colStruct->offset[simple]; - - index++; - linkIndex = jointInfo->child_link; - colStruct = &collision_structures_[linkIndex]; + int jointIndex = colStruct->jointIndexList[i]; + JointsInfo *jointInfo = &jointMap_[jointIndex]; + Quaterniond rotation = colStruct->rotation[simple]; + Vector3d offset = colStruct->offset[simple]; - if (jointInfo->type == urdf::Joint::REVOLUTE) { - double angleRad = toRadians(jointInfo->angle[simple][0]); - rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation; - } else if (jointInfo->type == urdf::Joint::PRISMATIC) { - double displacement = jointInfo->angle[simple][0]; - offset = offset + rotation * jointInfo->axis * displacement; - } + linkIndex = jointInfo->child_link; + CollisionStructureInfo *nowChild = &collision_structures_[linkIndex]; - offset = offset + rotation * jointInfo->nextLinkOffset; - colStruct->rotation[simple] = rotation; - colStruct->offset[simple] = offset; + offset = offset + rotation * jointInfo->nextLinkOffset; + if (jointInfo->type == urdf::Joint::REVOLUTE) { + double angleRad = toRadians(jointInfo->angle[simple][0]); + rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation; + } else if (jointInfo->type == urdf::Joint::PRISMATIC) { + double displacement = jointInfo->angle[simple][0]; + offset = offset + rotation * jointInfo->axis * displacement; + } - for (int i = 0; i < colStruct->obbModelList.size(); i++) { - int obbIndex = colStruct->obbLinkIndexList[i]; - addedOBBs_[simple][obbIndex] = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i], - offset, - rotation); + nowChild->rotation[simple] = rotation; + nowChild->offset[simple] = offset; + + for (int i = 0; i < nowChild->obbModelList.size(); i++) { + int obbIndex = nowChild->obbLinkIndexList[i]; + addedOBBs_[simple][obbIndex] = gjk_interface::updateOBBFromPoints(nowChild->obbModelList[i], + offset, + rotation); + } + + if (nowChild->joint_count > 0) { + childCnt++; + child.push_back(nowChild); + } } } } @@ -414,22 +497,43 @@ void CollisionSimulator::InitPolyhedronsList() } } -int CollisionSimulator::CheckCollision() +int checkCnt = 0; +int64_t prepareTime = 0; +int64_t checkTime = 0; +int64_t checkTime2 = 0; +int CollisionSimulator::CheckCollision(rclcpp::Node *node) { - for (int bodyType = 0; bodyType < BODY_CNT; bodyType++) { + int64_t currentTimeNs = node->get_clock()->now().nanoseconds(); + for (int bodyType = 0; bodyType < BODY_TYPE_OTHERS; bodyType++) { float *jointList = joints_[simpleUpdateIndex_][bodyType]; trajectory_[bodyType]->getAngleForChecking(jointList); } - if (CheckJointsAngleLimit(simpleUpdateIndex_, BODY_TYPE_OTHERS) != 0) { + if (CheckBodyAngleLimit(simpleUpdateIndex_, BODY_TYPE_OTHERS) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckCollision joints limit error."); return ARM_AIM_CANNOT_REACH; } UpdatePolyhedrons(BODY_TYPE_OTHERS, simpleUpdateIndex_); + int64_t updateNs = node->get_clock()->now().nanoseconds(); + prepareTime += updateNs - currentTimeNs; int lastSimpleIndex = (simpleUpdateIndex_ + COLLISION_CHECK_SIMPLE_CNT - 1) % COLLISION_CHECK_SIMPLE_CNT; std::vector *addedOBBsNext_ = &(addedOBBs_[simpleUpdateIndex_]); std::vector *addedOBBs = &(addedOBBs_[lastSimpleIndex]); int result = CheckCollisionInner(addedOBBs, addedOBBsNext_); simpleUpdateIndex_ = (simpleUpdateIndex_ + 1) % COLLISION_CHECK_SIMPLE_CNT; + + int64_t time1 = node->get_clock()->now().nanoseconds(); + checkTime += time1 - updateNs; + checkCnt++; + if (checkCnt == 2000) { + double checkAxisAve; + gjk_interface::GetAxisLog(checkAxisAve); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), + "Update time:%ld, check time1:%ld, check axis ave:%.4f.", prepareTime/checkCnt/1000, + checkTime/checkCnt/1000, checkAxisAve); + checkTime = 0; + prepareTime = 0; + checkCnt = 0; + } return result; } @@ -452,28 +556,66 @@ int CollisionSimulator::RemovePolyhedron(int id) return -1; // 未找到对应ID的多面体 } -int CollisionSimulator::CheckJointsAngleLimit(int simple, int bodyType) +int CollisionSimulator::CheckBodyAngleLimit(int simple, int bodyType) { CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[bodyType]]; - std::vector jointList; - int index = 0; - int maxJointCount = 0; - while (colStruct->joint_count > 0 || index < maxJointCount) { + + std::vector child = {colStruct}; + int childIndex = 0; + int childCnt = 1; + while (childIndex < childCnt) { + colStruct = child[childIndex]; + childIndex++; for (int i = 0; i < colStruct->joint_count; i++) { - jointList.push_back(&colStruct->joint_list[i]); - maxJointCount++; + int jointIndex = colStruct->jointIndexList[i]; + JointsInfo *jointInfo = &jointMap_[jointIndex]; + float angles = toRadians(jointInfo->angle[simple][0]); + if (angles < jointInfo->limit_lower || angles > jointInfo->limit_upper) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s angle limit error: %.2f not in [%.2f, %.2f]", + jointInfo->name.c_str(), + jointInfo->angle[simple][0], + jointInfo->limit_lower, + jointInfo->limit_upper); + return ARM_AIM_CANNOT_REACH; + } + CollisionStructureInfo *nowChild = &collision_structures_[jointInfo->child_link]; + if (nowChild->joint_count > 0) { + childCnt++; + child.push_back(nowChild); + } } - JointsInfo *jointInfo = jointList[index]; - if (jointInfo->angle[simple][0] < jointInfo->limit_lower || jointInfo->angle[simple][0] > jointInfo->limit_upper) { - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s angle limit error: %.2f not in [%.2f, %.2f]", - jointInfo->name.c_str(), - jointInfo->angle[simple][0], - jointInfo->limit_lower, - jointInfo->limit_upper); - return ARM_AIM_CANNOT_REACH; + } + return OK; +} + +int CollisionSimulator::CheckArmJointsAngleLimit(float *joints) +{ + CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[BODY_TYPE_LEFT_ARM]]; + + std::vector child = {colStruct}; + int childIndex = 0; + int childCnt = 1; + while (childIndex < childCnt) { + colStruct = child[childIndex]; + for (int i = 0; i < colStruct->joint_count; i++) { + int jointIndex = colStruct->jointIndexList[i]; + JointsInfo *jointInfo = &jointMap_[jointIndex]; + float angles = toRadians(joints[childIndex]); + if (angles < jointInfo->limit_lower || angles > jointInfo->limit_upper) { + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s angle limit error: %.2f not in [%.2f, %.2f]", + jointInfo->name.c_str(), + joints[childIndex], + jointInfo->limit_lower, + jointInfo->limit_upper); + return ARM_AIM_CANNOT_REACH; + } + CollisionStructureInfo *nowChild = &collision_structures_[jointInfo->child_link]; + if (nowChild->joint_count > 0) { + childCnt++; + child.push_back(nowChild); + } } - index++; - colStruct = &collision_structures_[jointInfo->child_link]; + childIndex++; } return OK; } @@ -490,7 +632,7 @@ int CollisionSimulator::UpdateCollisionForNewGoal(int bodyType) trajectory->getAngleForStarting(jointList20); trajectory->getAngleForChecking(jointList40); - if (CheckJointsAngleLimit(simple20, bodyType) != 0 || CheckJointsAngleLimit(simple40, bodyType) != 0) { + if (CheckBodyAngleLimit(simple20, bodyType) != 0 || CheckBodyAngleLimit(simple40, bodyType) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); return ARM_AIM_CANNOT_REACH; } diff --git a/rm_arm_control/simulator/rm_arm_simulator.cpp b/rm_arm_control/simulator/rm_arm_simulator.cpp index d6fbdb5..48e4a49 100644 --- a/rm_arm_control/simulator/rm_arm_simulator.cpp +++ b/rm_arm_control/simulator/rm_arm_simulator.cpp @@ -1,4 +1,3 @@ -#include #include #include #include @@ -255,11 +254,11 @@ static int UpdateJointAngles(ArmSimulator *simulator) (simulator->leftArmJoints_[simple]); simulator->rightTrajectory_->getAngleForChecking (simulator->rightArmJoints_[simple]); - if (simulator->CheckJointsAngleLimit(simulator->leftArmJoints_[simple]) != 0) { + if (simulator->CheckArmJointsAngleLimit(simulator->leftArmJoints_[simple]) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); return ARM_AIM_CANNOT_REACH; } - if (simulator->CheckJointsAngleLimit(simulator->rightArmJoints_[simple]) != 0) { + if (simulator->CheckArmJointsAngleLimit(simulator->rightArmJoints_[simple]) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); return ARM_AIM_CANNOT_REACH; } @@ -884,7 +883,7 @@ int ArmSimulator::UpdateCollisionForNewGoal(int arm) jointList40 = rightArmJoints_[simple40]; } - if (CheckJointsAngleLimit(jointList20) != 0 || CheckJointsAngleLimit(jointList40) != 0) { + if (CheckArmJointsAngleLimit(jointList20) != 0 || CheckArmJointsAngleLimit(jointList40) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); return ARM_AIM_CANNOT_REACH; } @@ -1117,7 +1116,7 @@ int ArmSimulator::RemovePolyhedron(int id) return -1; // 未找到对应ID的多面体 } -int ArmSimulator::CheckJointsAngleLimit(float *joints) +int ArmSimulator::CheckArmJointsAngleLimit(float *joints) { for (int i = 0; i < USED_ARM_DOF; ++i) { auto joint = jointMap_.find(((i) << 8) | (i + 1)); @@ -1125,12 +1124,12 @@ int ArmSimulator::CheckJointsAngleLimit(float *joints) float angle = toRadians(joints[i]); if (angle < joint->second.limit_lower || angle > joint->second.limit_upper) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), - "CheckJointsAngleLimit: joint out of limit [%.4f, %.4f], current: %.4f(%.2f)", + "CheckArmJointsAngleLimit: joint out of limit [%.4f, %.4f], current: %.4f(%.2f)", joint->second.limit_lower, joint->second.limit_upper, angle, joints[i]); return ARM_AIM_CANNOT_REACH; } } else { - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckJointsAngleLimit: no joint"); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckArmJointsAngleLimit: no joint"); return ARM_AIM_CANNOT_REACH; } } diff --git a/rm_arm_control/simulator/rm_driver_kinematics.cpp b/rm_arm_control/simulator/rm_driver_kinematics.cpp index 602d2d6..2331940 100644 --- a/rm_arm_control/simulator/rm_driver_kinematics.cpp +++ b/rm_arm_control/simulator/rm_driver_kinematics.cpp @@ -240,8 +240,7 @@ int RmArmDriverAndKinematics::computeMovingTrajectory(NodataTrajectory *newTraje for (int i = 0; i < USED_ARM_DOF; ++i) { joints[i] = end[i]; } - rm_movej(robotHandle, joints, 5, 1, 0, 0); - return 0; + return rm_movej(robotHandle, joints, 5, 1, 0, 0); } int RmArmDriverAndKinematics::computeSteppingTrajectory(NodataTrajectory *newTrajectory, const float *start, const float *speed, int armId) diff --git a/rm_arm_control/src/rm_arm_handler.cpp b/rm_arm_control/src/rm_arm_handler.cpp index 375ca9d..6a91db8 100644 --- a/rm_arm_control/src/rm_arm_handler.cpp +++ b/rm_arm_control/src/rm_arm_handler.cpp @@ -28,9 +28,9 @@ RmArmHandler::~RmArmHandler() void PrintData(ArmStatusCallbackData *data) { - RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK=================="); + /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK=================="); RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode); - /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:"); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:"); for (size_t i = 0; i < USED_ARM_DOF; i++){ RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]); } @@ -44,14 +44,14 @@ void PrintData(ArmStatusCallbackData *data) for (size_t i = 0; i < USED_ARM_DOF; i++){ RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]); } - */ + std::string posStr = "joints position: "; for (size_t i = 0; i < USED_ARM_DOF; i++){ posStr += std::to_string(data->jointAngle[i]) + " "; } RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str()); - /*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:"); + RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:"); for (size_t i = 0; i < USED_ARM_DOF; i++){ RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]); } diff --git a/rm_arm_control/src/rm_arm_node.cpp b/rm_arm_control/src/rm_arm_node.cpp index cdbc4e6..cd1b97e 100644 --- a/rm_arm_control/src/rm_arm_node.cpp +++ b/rm_arm_control/src/rm_arm_node.cpp @@ -1,9 +1,9 @@ #include "arm_define.h" -#include "rm_arm_simulator.hpp" +// #include "rm_arm_simulator.hpp" #include "rm_arm_node.hpp" #include "col_simulator.hpp" -static ArmSimulator *armSimulator_; +// static ArmSimulator *armSimulator_; static CollisionSimulator *colSimulator_; using ArmGoalSharedPtr = @@ -40,7 +40,7 @@ namespace ArmCommandHash for (int i = 0; i < USED_ARM_DOF; i++) { points[i] = goal->data_array[i]; } - if (armSimulator_->CheckJointsAngleLimit(points) != 0) { + if (colSimulator_->CheckArmJointsAngleLimit(points) != 0) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error."); return ARM_AIM_CANNOT_REACH; } @@ -134,39 +134,38 @@ RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_ar std::bind(&RmArmNode::ArmActionCancel, this, std::placeholders::_1), std::bind(&RmArmNode::ArmActionAccepted, this, std::placeholders::_1)); + this->declare_parameter("robot_description", ""); + std::string urdf_string = this->get_parameter("robot_description").as_string(); + this->declare_parameter("robot_description_semantic", ""); std::string srdf_string = this->get_parameter("robot_description_semantic").as_string(); if (srdf_string == "") { throw std::runtime_error("robot_description_semantic parameter is not valid."); } - this->declare_parameter("robot_description", ""); - std::string urdf_string = this->get_parameter("robot_description").as_string(); - /*leftArmHandler_->nowJoints_[0] = 180.0; - leftArmHandler_->nowJoints_[1] = 180.0;*/ - if (urdf_string != "") { - armStates.name = { - "left_behind_hip_joint","left_behind_leg_joint","left_behind_wheel_joint","right_behind_hip_joint", - "right_behind_leg_joint","right_behind_wheel_joint","body_joint","body_2_joint","head_joint", - "left_joint1","left_joint2","left_joint3","left_joint4","left_joint5", "left_joint6", - "right_joint1","right_joint2","right_joint3","right_joint4","right_joint5", "right_joint6", - "left_front_roll_joint","left_front_hip_joint","left_front_knee_joint", - "left_front_wheel_joint","right_front_roll_joint","right_front_hip_joint","right_front_knee_joint","right_front_wheel_joint"}; - armStates.position.resize(JOINT_ALL_CNT, 0.0); - armStates.header.frame_id = "base_link"; - armSimulator_ = new ArmSimulator(urdf_string, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_, - leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_); - robotDescriptionSub_ = NULL; - - /*colSimulator_ = new CollisionSimulator(urdf_string, srdf_string, - (float**)(std::vector{leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_}.data()), - (KinematicsManager**)(std::vector{leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_}.data()));*/ - } else { - armSimulator_ = NULL; - robotDescriptionSub_ = this->create_subscription - ("robot_description", 10, std::bind(&RmArmNode::CallbackGetRobotDescription, this, std::placeholders::_1)); + this->declare_parameter("robot_description_xml", ""); + std::string xml_string = this->get_parameter("robot_description_xml").as_string(); + if (xml_string == "") { + throw std::runtime_error("robot_description_xml parameter is not valid."); } + /*rightArmHandler_->nowJoints_[0] = 92.56; + rightArmHandler_->nowJoints_[1] = 105.44; + rightArmHandler_->nowJoints_[2] = 27.5;*/ + + armStates.name = { + "left_behind_hip_joint","left_behind_leg_joint","left_behind_wheel_joint","right_behind_hip_joint", + "right_behind_leg_joint","right_behind_wheel_joint","body_joint","body_2_joint","head_joint", + "left_joint1","left_joint2","left_joint3","left_joint4","left_joint5", "left_joint6", + "right_joint1","right_joint2","right_joint3","right_joint4","right_joint5", "right_joint6", + "left_front_roll_joint","left_front_hip_joint","left_front_knee_joint", + "left_front_wheel_joint","right_front_roll_joint","right_front_hip_joint","right_front_knee_joint","right_front_wheel_joint"}; + armStates.position.resize(JOINT_ALL_CNT, 0.0); + armStates.header.frame_id = "base_link"; + colSimulator_ = new CollisionSimulator(xml_string, srdf_string, + (float**)(std::vector{leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_}.data()), + (KinematicsManager**)(std::vector{leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_}.data())); + std::thread{std::bind(&RmArmNode::ArmActionCheck, this)}.detach(); std::thread{std::bind(&RmArmNode::ArmActionExecute, this)}.detach(); @@ -178,13 +177,13 @@ RmArmNode::~RmArmNode() armStatesPub_.reset(); delete leftArmHandler_; delete rightArmHandler_; - delete armSimulator_; + // delete armSimulator_; } void RmArmNode::CallbackGetRobotDescription(const std_msgs::msg::String::SharedPtr msg) { RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received robot_description message."); - if (armSimulator_ == NULL) { + /*if (armSimulator_ == NULL) { armStates.name = { "left_behind_hip_joint","left_behind_leg_joint","left_behind_wheel_joint","right_behind_hip_joint", "right_behind_leg_joint","right_behind_wheel_joint","body_joint","body_2_joint","head_joint", @@ -196,7 +195,7 @@ void RmArmNode::CallbackGetRobotDescription(const std_msgs::msg::String::SharedP armStates.header.frame_id = "base_link"; armSimulator_ = new ArmSimulator(msg, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_, leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_); - } + }*/ } rclcpp_action::GoalResponse RmArmNode::ArmActionGoal( @@ -347,6 +346,7 @@ void RmArmNode::StartNewGoal(RmArmHandler *handle) ret = handle->StartNewTrajectory(); if (ret == 0) { // ret = armSimulator_->UpdateCollisionForNewGoal(handle->armType_); + ret = colSimulator_->UpdateCollisionForNewGoal(handle->armType_); if (ret == SAFE) { handle->ArmMove(); } else { @@ -388,6 +388,9 @@ void RmArmNode::ArmActionCheck() while (rclcpp::ok()) { if (!busy) { int result = sem_timedwait(&sem_, &timeout); + if (!rclcpp::ok()) { + break; + } if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) { StartNewGoal(leftArmHandler_); @@ -401,7 +404,7 @@ void RmArmNode::ArmActionCheck() if (currentTimeNs >= nextCheckPoint) { step++; nextCheckPoint += ARM_FOLLOWING_CHECKING; - int coll = 0; //armSimulator_->CheckCollision(); + int coll = colSimulator_->CheckCollision(this); //armSimulator_->CheckCollision(); g_checkCnt++; int64_t durationCol = (get_clock()->now().nanoseconds() - currentTimeNs); @@ -456,7 +459,7 @@ void RmArmNode::ArmActionExecute() int64_t startTimeNs = startTime.nanoseconds(); // Execute the goal while (rclcpp::ok()) { - if (armSimulator_ != NULL) { + if (colSimulator_ != NULL) { if (leftArmHandler_ != NULL) { leftArmHandler_->ArmRun(startTimeNs); } @@ -494,7 +497,7 @@ void RmArmNode::CallbackRealtimeArmJointState(ArmStatusCallbackData *data) else if (data->armId == RIGHT_ARM && rightArmHandler_ != NULL) { rightArmHandler_->ArmDealCallBackInfo(data); } - if (armStatesPub_ != NULL && armSimulator_ != NULL) { + if (armStatesPub_ != NULL && colSimulator_ != NULL) { for (int i = LEFT_ARM_JOINT_START_INDEX; i < LEFT_ARM_JOINT_START_INDEX + USED_ARM_DOF; i++) { armStates.position[i] = leftArmHandler_->nowJoints_[i - LEFT_ARM_JOINT_START_INDEX] * M_PI / 180.0f; } diff --git a/rm_arm_control/urdf/FHrobot.urdf b/rm_arm_control/urdf/FHrobot.urdf index 18b425a..86c6f43 100644 --- a/rm_arm_control/urdf/FHrobot.urdf +++ b/rm_arm_control/urdf/FHrobot.urdf @@ -1800,4 +1800,4 @@ effort="80" velocity="1" /> - + \ No newline at end of file diff --git a/rm_arm_control/urdf/FHrobot.xml b/rm_arm_control/urdf/FHrobot.xml new file mode 100644 index 0000000..4bf3aac --- /dev/null +++ b/rm_arm_control/urdf/FHrobot.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +