修正初始链接，去掉注释

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<std::string> keywords = {"keyboard"};
+    const std::vector<std::string> 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<Eigen::Vector3d> 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<Eigen::Vector3d> 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<ArmKeyboardNode>();

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<OBB> obbModelList;  // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
-    std::vector<JointsModel> joint_list;
-    std::vector<int> obbLinkIndexList; // 网格边界盒所属链接索引
+    std::vector<int> jointIndexList; // 网格边界盒所属链接索引
+    std::vector<int> 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<CollisionModel> collision_structures_;
     std::vector<JointsModel> jointMap_;
     std::map<std::string, int> 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

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<JointsModel *> jointList;
+    std::vector<int> 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<Vector3d> 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<std::string, std::array<Vector3d, 8>> 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<Vector3d, 8> eigen_points_arr = pointMap.at(col_struct.link_name);
-    std::vector<Eigen::Vector3d> eigen_points(eigen_points_arr.begin(), eigen_points_arr.end());
+    std::vector<Eigen::Vector3d> 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<JointsModel *> jointList;
+    std::vector<int> 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);
 
-    // 创建根节点 <root>
-    XMLElement* root = doc.NewElement("root");
+    // 创建根节点 <robot>
+    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<int>(robot_description.collision_structures_.size()));
+    root->SetAttribute("joint_count", static_cast<int>(robot_description.jointMap_.size()));
     doc.InsertEndChild(root);  // 将根节点添加到文档
 
-    // 向根节点添加子节点 <node id="1">
-    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添加子节点 <param>
-    XMLElement* param1 = doc.NewElement("param");
-    param1->SetAttribute("name", "name");
-    param1->SetAttribute("value", "example");
-    node1->InsertEndChild(param1);
+        collisionElem->SetAttribute("obb_count", static_cast<int>(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);
+        }
 
-    // 向根节点添加另一个子节点 <node id="2">
-    XMLElement* node2 = doc.NewElement("node");
-    node2->SetAttribute("id", "2");
-    root->InsertEndChild(node2);
+        collisionElem->SetAttribute("joint_index_count", static_cast<int>(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添加文本节点 <value>
-    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());

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)

gjk_interface/include/gjk_interface.hpp

@@ -3,7 +3,6 @@
 
 #include <vector>
 #include <Eigen/Dense>
-#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<Eigen::Vector3d>& 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

gjk_interface/src/gjk_interface.cpp

@@ -1,6 +1,8 @@
 #include "math.h"
+#include <cmath>
 #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<double, double> 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<Eigen::Vector3d>& 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<double, double> 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<Vector3d> 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<double>(checkAxisCnt) / static_cast<double>(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);
+    }
 }

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)
@@ -52,22 +52,23 @@ find_package(trajectory_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(gjk_interface REQUIRED)
 find_package(interfaces REQUIRED)
-find_package(moveit_core REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
 
 include_directories(
   include
   ${CMAKE_CURRENT_SOURCE_DIR}/../include
   ${EIGEN3_INCLUDE_DIRS}  # 引入Eigen3的头文件路径
+  ${TINYXML2_INCLUDE_DIR}
 )
 
 # 设置源文件
 set(SOURCES
     src/rm_arm_handler.cpp
     src/rm_arm_node.cpp
-    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
@@ -87,8 +88,6 @@ ament_target_dependencies(rm_arm_control
   control_msgs
   gjk_interface
   interfaces
-  moveit_core
-  moveit_ros_planning_interface
 )
 
 # 查找库
@@ -96,6 +95,7 @@ find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/Robot
 
 # 链接库
 target_link_libraries(rm_arm_control ${RMAN_API_LIB})
+target_link_libraries(rm_arm_control ${TINYXML2_LIBRARIES})
 
 # 如果是 UNIX 平台且不是 ARM 平台，确保动态库文件可以在运行时被找到（这一步通常不是必须的，但在某些配置下可能需要）
 if(UNIX)

rm_arm_control/include/col_simulator.hpp

@@ -3,9 +3,9 @@
 
 #include <unordered_map>
 #include <map>
+#include <rclcpp/rclcpp.hpp>
 #include <urdf_model/pose.h>
 #include <std_msgs/msg/string.hpp>
-#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
 #include "arm_define.h"
 #include "arm_driver_define.hpp"
 #include "gjk_interface.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<OBB> obbModelList;  // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
     std::vector<int> obbLinkIndexList; // 网格边界盒所属链接索引
+    std::vector<int> jointIndexList; // 网格边界盒所属关节索引
 
     Eigen::Quaterniond rotation[COLLISION_CHECK_SIMPLE_CNT];      // 链接旋转
     Eigen::Vector3d offset[COLLISION_CHECK_SIMPLE_CNT];          // 链接偏移
-    bool rotation_add_flag;
-    std::vector<JointsInfo> 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<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_);
+    int CheckCollisionInnerOld(std::vector<OBB> *addedOBBs, std::vector<OBB> *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<Polyhedron> polyhedrons_[COLLISION_CHECK_SIMPLE_CNT];
     std::vector<OBB> addedOBBs_[COLLISION_CHECK_SIMPLE_CNT];
+    std::map<int, std::string> obbNameMap_;
 
     std::vector<CollisionStructureInfo> collision_structures_;
     std::vector<JointsInfo> jointMap_;
@@ -99,6 +99,11 @@ private:
 
     std::unordered_map<int, Polyhedron> addedPolyhedrons_;
     std::map<std::string, int> linkCollisionMap_;
+
+    // Collision pair enable matrix (flattened upper-triangle including i<j)
+    std::vector<uint8_t> linkCollisionDetectMatrix_;
+    // Precomputed list of enabled OBB pairs to check
+    std::vector<std::tuple<int, int, AxisStats*>> tuplesToCheck_;
 };
 
 #endif // COL_SIMULATOR_HPP

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);

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
         }]
     )

rm_arm_control/package.xml

@@ -12,8 +12,6 @@
   <depend>gjk_interface</depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>
-  <exec_depend>moveit_core</exec_depend>
-  <exec_depend>moveit_ros_planning_interface</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>

rm_arm_control/simulator/col_simulator.cpp

@@ -1,67 +1,45 @@
-#include <memory>
 #include <vector>
-#include <stack>
 #include <string>
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/robot_model/robot_model.h>
-#include <CGAL/optimal_bounding_box.h>
-#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
-#include <CGAL/Surface_mesh.h>
+#include <tinyxml2.h>
 #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"};
+const std::string g_bodyStartName[BODY_CNT] = {"base_link_leftarm", "base_link_rightarm", "base_link"};
 
-bool *g_linkCollisionDetectMatrix = nullptr;
+static float zeroFixedAngle = 0;
 
 int g_linkStartIndex[BODY_CNT] = {0};
 
 #define POLYHEDRONS_POINTS_CNT   (8)
 
+double truncate_to_4_decimal(double num) {
+    double int_part; // 存储整数部分
+    double frac_part = std::modf(num, &int_part); // 分离小数部分
+
+    // 截断小数部分到前 4 位
+    double truncated_frac = std::trunc(frac_part * 10000.0) * 0.0001;
+
+    // 合并整数部分和截断后的小数部分
+    return int_part + truncated_frac;
+}
+
 static double toRadians(float degrees) {
     return (static_cast<double>(degrees)) * (M_PI / 180.0);
 }
 
-int CollisionSimulator::InitJointModel()
-{
-    int linkIndex = rootLinkIndex_;
-    CollisionStructureInfo *colStruct = &collision_structures_[linkIndex];
-    std::vector<JointsInfo *> 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;
-}
+enum
+  {
+    UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
+  } JointTypeE;
 
-int CollisionSimulator::InitJoint(const urdf::Model &urdf_model)
+int CollisionSimulator::InitJoint()
 {
     const std::map<std::string, int> jointNameToIndexMap = {
         {"left_joint1", 0}, {"left_joint2", 1}, {"left_joint3", 2}, {"left_joint4", 3}, {"left_joint5", 4}, {"left_joint6", 5}, 
@@ -83,168 +61,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 != 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::Vector3d> eigen_points;
-
-    std::vector<CgalPoint> 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<CgalPoint, 8> 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<urdf::Box*>(collision->geometry.get());
-                std::vector<Eigen::Vector3d> 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<urdf::Mesh*>(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<size_t>(maxObbCnt) * static_cast<size_t>(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<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(j)] = 1;
         }
     }
     tinyxml2::XMLDocument doc;
@@ -265,8 +107,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 +122,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<size_t>(obb1Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(obb2Idx)] = 0;
+                        linkCollisionDetectMatrix_[
+                            static_cast<size_t>(obb2Idx) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(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<size_t>(maxObbCnt) * (static_cast<size_t>(maxObbCnt) - 1) / 2);
+    for (int i = 0; i < maxObbCnt; ++i) {
+        for (int j = i + 1; j < maxObbCnt; ++j) {
+            if (linkCollisionDetectMatrix_[static_cast<size_t>(i) * static_cast<size_t>(maxObbCnt) + static_cast<size_t>(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 <robot> root node");
+        return -1;
+    }
+    if (!robot_root->Attribute("maxObbCnt")) {
+        RCLCPP_ERROR(rclcpp::get_logger(GET_FUNC_LINE_STR()), "SRDF <robot> 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<OBB> *addedOBBs, std::vector<OBB> *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<OBB> *addedOBBs, std::vector<OBB> *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<JointsInfo *> jointList;
-    int index = 0;
-    int maxJointCount = 0;
-    while (colStruct->joint_count > 0 || index < maxJointCount) {
+    std::vector<CollisionStructureInfo *> 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 == REVOLUTE) {
+                double angleRad = toRadians(jointInfo->angle[simple][0]);
+                rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation;
+            } else if (jointInfo->type == 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 +513,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<OBB> *addedOBBsNext_ = &(addedOBBs_[simpleUpdateIndex_]);
     std::vector<OBB> *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 +572,65 @@ 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<JointsInfo *> jointList;
-    int index = 0;
-    int maxJointCount = 0;
-    while (colStruct->joint_count > 0 || index < maxJointCount) {
+
+    std::vector<CollisionStructureInfo *> 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<CollisionStructureInfo *> 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 +647,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;
     }

rm_arm_control/simulator/rm_arm_simulator.cpp

@@ -1,4 +1,3 @@
-#include <memory>
 #include <vector>
 #include <string>
 #include <rclcpp/rclcpp.hpp>
@@ -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;
         }
     }

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, 20, 1, 0, 0);
-    return 0;
+    return rm_movej(robotHandle, joints, 20, 1, 0, 0);
 }
 
 int RmArmDriverAndKinematics::computeSteppingTrajectory(NodataTrajectory *newTrajectory, const float *start, const float *speed, int armId)

rm_arm_control/src/rm_arm_handler.cpp

@@ -28,10 +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:");
-    std::string current = "current: ";
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
     for (size_t i = 0; i < USED_ARM_DOF; i++){
         current += std::to_string(data->jointCurrent[i]) + " ";
         //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
@@ -46,7 +45,7 @@ void PrintData(ArmStatusCallbackData *data)
     RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "error code:");
     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++){
@@ -54,10 +53,10 @@ void PrintData(ArmStatusCallbackData *data)
     }
     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]);
-    }*/
+    }
     
     posStr = "temperature: ";
     //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "temperature:");
@@ -73,7 +72,7 @@ void PrintData(ArmStatusCallbackData *data)
         //RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointVoltage[i]);
         posStr += std::to_string(data->jointVoltage[i]) + " ";
     }
-    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
+    RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());*/
 }
 
 void RmArmHandler::ArmDealCallBackInfo(ArmStatusCallbackData *data)

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<std::string>("robot_description", "");
+    std::string urdf_string = this->get_parameter("robot_description").as_string();
+
     this->declare_parameter<std::string>("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<std::string>("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<float*>{leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_}.data()),
-            (KinematicsManager**)(std::vector<KinematicsManager*>{leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_}.data()));*/
-    } else {
-        armSimulator_ = NULL;
-        robotDescriptionSub_ = this->create_subscription<std_msgs::msg::String>
-            ("robot_description", 10, std::bind(&RmArmNode::CallbackGetRobotDescription, this, std::placeholders::_1));
+    this->declare_parameter<std::string>("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<float*>{leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_}.data()),
+        (KinematicsManager**)(std::vector<KinematicsManager*>{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;
         }

rm_arm_control/urdf/FHrobot.urdf

@@ -1800,4 +1800,4 @@
       effort="80"
       velocity="1" />
   </joint>
-</robot>
+</robot>

rm_arm_control/urdf/FHrobot.xml

@@ -0,0 +1,161 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="FHrobot" rootLinkIndex_="12" maxObbCnt="32" collision_structure_count="32" joint_count="31">
+    <collision_model link_name="LeftLink1" link_index="0" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link1.STL" parent_link_index="13" offset_x="-0.0012343334058273648" offset_y="0.2594999999967551" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="0" center_x="-0.016051056200847666" center_y="-0.032326459587917543" center_z="-3.9525419148360441e-05" half_extent_x="0.040999921166100871" half_extent_y="0.079396313069021993" half_extent_z="0.046002154469577221" axis_0_x="-0.0024475571032369636" axis_0_y="1.0493825749080532e-05" axis_0_z="0.99999700467256725" axis_1_x="0.19432167128890609" axis_1_y="-0.98093774112340471" axis_1_z="0.00048590863205766333" axis_2_x="0.98093480799271926" axis_2_y="0.19432227852099693" axis_2_z="0.0023988619583620308" max_radius="0.10050356352101891" self_min_project_x="-0.041000499818472834" self_min_project_y="-0.050805156094976392" self_min_project_z="-0.068029040301701552" self_max_project_x="0.040999342513728936" self_max_project_y="0.10798747004306765" self_max_project_z="0.023975268637452973"/>
+        <joint_index index="12"/>
+    </collision_model>
+    <collision_model link_name="LeftLink2" link_index="1" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link2.STL" parent_link_index="0" offset_x="-0.0012343334058273648" offset_y="0.2594999999967551" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="1" center_x="-0.0020289969093848781" center_y="0.12645970871013129" center_z="-0.0032008275134066955" half_extent_x="0.042709093291295748" half_extent_y="0.040493603448293329" half_extent_z="0.17025658420986939" axis_0_x="-0.98525409695611921" axis_0_y="-0.17106663755514073" axis_0_z="0.0032511454536003903" axis_1_x="0.0010545599213019896" axis_1_y="-0.025072867010843214" axis_1_z="-0.99968507003117202" axis_2_x="0.171094279081899" axis_2_y="-0.98494038238638337" axis_2_z="0.024883544964707571" max_radius="0.180141896950167" self_min_project_x="-0.062353459284675769" self_min_project_y="-0.040466631128935934" self_min_project_z="-0.29523865576212138" self_max_project_x="0.023064727297915838" self_max_project_y="0.040520575767650835" self_max_project_z="0.045274512657617366"/>
+        <joint_index index="13"/>
+    </collision_model>
+    <collision_model link_name="LeftLink3" link_index="2" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link3.STL" parent_link_index="1" offset_x="-0.001235273746333937" offset_y="0.51549999999502816" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="2" center_x="0.010280936838639341" center_y="0.04944817366945943" center_z="0.00037899428027164499" half_extent_x="0.032046625278721277" half_extent_y="0.085095380065108014" half_extent_z="0.032176349547650987" axis_0_x="-0.657189413254468" axis_0_y="-0.1334996406581942" axis_0_z="-0.74180854743685787" axis_1_x="-0.1908201219081227" axis_1_y="0.98159560007147606" axis_1_z="-0.0075999339003853805" axis_2_x="0.72917059470415979" axis_2_y="0.13655740135362171" axis_2_z="-0.67056865416922817" max_radius="0.096454794439636754" self_min_project_x="-0.045685602740024894" self_min_project_y="-0.038521960311954068" self_min_project_z="-0.018181420305380096" self_max_project_x="0.01840764781741765" self_max_project_y="0.13166879981826193" self_max_project_z="0.046171278789921874"/>
+        <joint_index index="14"/>
+    </collision_model>
+    <collision_model link_name="LeftLink4" link_index="3" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link4.STL" parent_link_index="2" offset_x="-0.0012360451165722413" offset_y="0.72549999999219494" offset_z="0.67449988796724425" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="3" center_x="-0.014264352095174744" center_y="-0.028024824238507655" center_z="-0.00018102859347073064" half_extent_x="0.028997663993902703" half_extent_y="0.03563410377559955" half_extent_z="0.060057200102665409" axis_0_x="0.012686474370885659" axis_0_y="5.007750049523807e-06" axis_0_z="-0.99991952343324164" axis_1_x="-0.98533543058118345" axis_1_y="-0.17016986208267593" axis_1_z="-0.012502290996384335" axis_2_x="-0.17015623000475866" axis_2_y="0.98541474416292707" axis_2_z="-0.0021539212804582153" max_radius="0.075614291990106428" self_min_project_x="-0.028997754647579334" self_min_project_y="-0.016807688514226638" self_min_project_z="-0.085245716812550165" self_max_project_x="0.028997573340226093" self_max_project_y="0.054460519036972493" self_max_project_z="0.034868683392780701"/>
+        <joint_index index="15"/>
+    </collision_model>
+    <collision_model link_name="LeftLink5" link_index="4" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link5.STL" parent_link_index="3" offset_x="-0.0012360451165722413" offset_y="0.72549999999219494" offset_z="0.67449988796724425" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="4" center_x="0.0045661194796603327" center_y="0.043935535428629116" center_z="-7.1988084438456487e-06" half_extent_x="0.028602730801781696" half_extent_y="0.03097283920625343" half_extent_z="0.07406294281286642" axis_0_x="-0.010052596461584629" axis_0_y="-0.0023338244385488543" axis_0_z="-0.99994674786604143" axis_1_x="0.99470372320540235" axis_1_y="0.10227254952906177" axis_1_z="-0.010238586530286975" axis_2_x="0.10229099836101371" axis_2_y="-0.9947536774882112" axis_2_z="0.0012933591888191764" max_radius="0.085221784045615603" self_min_project_x="-0.028743971559518251" self_min_project_y="-0.021937430230401168" self_min_project_z="-0.11730091464334361" self_max_project_x="0.028461490044045162" self_max_project_y="0.040008248182105785" self_max_project_z="0.030824970982389316"/>
+        <joint_index index="16"/>
+    </collision_model>
+    <collision_model link_name="LeftLink6" link_index="5" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link6.STL" parent_link_index="4" offset_x="-0.0012365740561642101" offset_y="0.86949999999025229" offset_z="0.67450041690877915" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="5" center_x="0.0017426974547197892" center_y="-0.014500002689584538" center_z="0.0044016310038713178" half_extent_x="0.031571103354631945" half_extent_y="0.03296415800613637" half_extent_z="0.014499999842049673" axis_0_x="0.57756385353882367" axis_0_y="7.2416288040555443e-06" axis_0_z="-0.81634551204312156" axis_1_x="-0.81634551204218475" axis_1_y="-3.8756796192362941e-06" axis_1_z="-0.57756385357254092" axis_2_x="-7.3463967025788612e-06" axis_2_y="0.99999999996627009" axis_2_z="3.6732187166377273e-06" max_radius="0.047891755819784027" self_min_project_x="-0.03415794101644902" self_min_project_y="-0.036928968019379567" self_min_project_z="-0.02899999916553854" self_max_project_x="0.028984265692814874" self_max_project_y="0.028999347992893172" self_max_project_z="5.1856081408063748e-10"/>
+    </collision_model>
+    <collision_model link_name="RightLink1" link_index="6" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link1.STL" parent_link_index="14" offset_x="0.0012325465941726353" offset_y="-0.2594999999967551" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="6" center_x="0.016051293683671908" center_y="0.032326341669277794" center_z="-3.952542121429814e-05" half_extent_x="0.040999921166077015" half_extent_y="0.046002154469943116" half_extent_z="0.079396313068703261" axis_0_x="0.0024475572633739784" axis_0_y="-1.0511773426702398e-05" axis_0_z="0.9999970046719866" axis_1_x="-0.98093623553846632" axis_1_y="-0.1943150721580289" axis_1_z="0.0023988621975659097" axis_2_x="0.19431446490435411" axis_2_y="-0.98093916866527597" axis_2_z="-0.00048590864569764136" max_radius="0.10050356352092486" self_min_project_x="-0.041000499815637047" self_min_project_y="-0.068029040301636201" self_min_project_z="-0.10798747004284504" self_max_project_x="0.040999342516516983" self_max_project_y="0.023975268638250037" self_max_project_z="0.05080515609456146"/>
+        <joint_index index="26"/>
+    </collision_model>
+    <collision_model link_name="RightLink2" link_index="7" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link2.STL" parent_link_index="6" offset_x="0.0012325465941726353" offset_y="-0.2594999999967551" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="7" center_x="0.0020280679215883002" center_y="-0.12645972359901128" center_z="-0.0032008275923805646" half_extent_x="0.17025658420299805" half_extent_y="0.040493603448194554" half_extent_z="0.042709093286796625" axis_0_x="-0.17108704302979572" axis_0_y="0.98494163932018242" axis_0_z="0.024883545579219013" axis_1_x="-0.0010543758814449689" axis_1_y="0.025072875353159829" axis_1_z="-0.99968507001606555" axis_2_x="-0.9852553537021872" axis_2_y="-0.17105939920038726" axis_2_z="-0.0032511453952965841" max_radius="0.18014189694258384" self_min_project_x="-0.29523865575563962" self_min_project_y="-0.040466631125291724" self_min_project_z="-0.023064727366431257" self_max_project_x="0.045274512650356424" self_max_project_y="0.040520575771097453" self_max_project_z="0.062353459207162121"/>
+        <joint_index index="27"/>
+    </collision_model>
+    <collision_model link_name="RightLink3" link_index="8" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link3.STL" parent_link_index="7" offset_x="0.0012316062536660631" offset_y="-0.51549999999502816" offset_z="0.67449911659417261" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="8" center_x="-0.010195348701359928" center_y="-0.049404982808090796" center_z="-0.0008551636784924245" half_extent_x="0.085045055608125336" half_extent_y="0.03223931161526248" half_extent_z="0.032037552656193569" axis_0_x="-0.1874985014226308" axis_0_y="0.98214756458618246" axis_0_z="-0.015181348480892837" axis_1_x="0.76791651821458984" axis_1_y="0.13692806572273175" axis_1_z="-0.6257434984645176" axis_2_x="-0.61249370039001172" axis_2_y="-0.12898397650430021" axis_2_z="-0.77988114529567198" max_radius="0.096428416335712502" self_min_project_x="-0.13164344401071643" self_min_project_y="-0.046298303913031566" self_min_project_z="-0.018753588632619115" self_max_project_x="0.038446667205534343" self_max_project_y="0.01818031931749356" self_max_project_z="0.045321516679768137"/>
+        <joint_index index="28"/>
+    </collision_model>
+    <collision_model link_name="RightLink4" link_index="9" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link4.STL" parent_link_index="8" offset_x="0.0012308348777609365" offset_y="-0.72549999999219472" offset_z="0.67449988796724436" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="9" center_x="0.014264552104614067" center_y="0.028024712668366768" center_z="-0.00018023876202880734" half_extent_x="0.028997701761045422" half_extent_y="0.035634062267396127" half_extent_z="0.060057208907205978" axis_0_x="-0.012772294911872262" axis_0_y="-5.124161347636047e-05" axis_0_z="-0.99991842960162591" axis_1_x="0.9853355374541819" axis_1_y="0.17016242559096995" axis_1_z="-0.012594742826943506" axis_2_x="0.17014919074906948" axis_2_y="-0.98541602701158437" axis_2_z="-0.0021228744998130766" max_radius="0.075614293905471225" self_min_project_x="-0.029001104798924468" self_min_project_y="-0.016807669007858607" self_min_project_z="-0.085245725301739364" self_max_project_x="0.028994298723166376" self_max_project_y="0.054460455526933647" self_max_project_z="0.034868692512672633"/>
+        <joint_index index="29"/>
+    </collision_model>
+    <collision_model link_name="RightLink5" link_index="10" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link5.STL" parent_link_index="9" offset_x="0.0012308348777609365" offset_y="-0.72549999999219472" offset_z="0.67449988796724436" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="10" center_x="-0.00456644224803041" center_y="-0.043935501882881166" center_z="-7.1988087149170085e-06" half_extent_x="0.028602730801716839" half_extent_y="0.030972839206234427" half_extent_z="0.074062942812913146" axis_0_x="0.010052613634740859" axis_0_y="0.0023337505824445434" axis_0_z="-0.99994674786577054" axis_1_x="-0.99470447451417743" axis_1_y="-0.10226524202667642" axis_1_z="-0.010238586557768214" axis_2_x="-0.10228369049162267" axis_2_y="0.99475442893281174" axis_2_z="0.0012933591804002859" max_radius="0.085221784045627538" self_min_project_x="-0.028743971559067827" self_min_project_y="-0.021937430230265603" self_min_project_z="-0.11730091464339504" self_max_project_x="0.028461490044365857" self_max_project_y="0.040008248182203401" self_max_project_z="0.030824970982431317"/>
+        <joint_index index="30"/>
+    </collision_model>
+    <collision_model link_name="RightLink6" link_index="11" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/link6.STL" parent_link_index="10" offset_x="0.0012303059342831356" offset_y="-0.86949999999025174" offset_z="0.67450041690877927" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="11" center_x="-0.0017425909317051128" center_y="0.014500015491763815" center_z="0.0044016310038712154" half_extent_x="0.031571103354631737" half_extent_y="0.032964158006136432" half_extent_z="0.014499999842049626" axis_0_x="-0.57756385357642359" axis_0_y="-2.998607827764399e-06" axis_0_z="-0.81634551204313022" axis_1_x="0.81634551204863814" axis_1_y="-2.1215293748433339e-06" axis_1_z="-0.57756385357252815" axis_2_x="-1.3492262862513371e-11" axis_2_y="-0.99999999999325451" axis_2_z="3.6732187227439539e-06" max_radius="0.047891755819783922" self_min_project_x="-0.034157941016448909" self_min_project_y="-0.036928968019379546" self_min_project_z="-0.028999999165538685" self_max_project_x="0.028984265692814579" self_max_project_y="0.02899934799289336" self_max_project_z="5.1856057815824474e-10"/>
+    </collision_model>
+    <collision_model link_name="base_link" link_index="12" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/base_link.STL" parent_link_index="-1" offset_x="0" offset_y="0" offset_z="0" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="5">
+        <obb index="12" center_x="0.0082945434917357508" center_y="-1.2973459251330866e-09" center_z="0.03997856133010165" half_extent_x="0.19951260690182393" half_extent_y="0.18950000525639663" half_extent_z="0.12676640141158502" axis_0_x="-0.9999829304621275" axis_0_y="1.0575666826600023e-12" axis_0_z="-0.0058428404373047985" axis_1_x="-1.7082289374425593e-10" axis_1_y="-0.99999999999999956" axis_1_z="2.9054755303171375e-08" axis_2_x="-0.0058428404373047317" axis_2_y="2.9055257449701237e-08" axis_2_z="0.99998293046212716" max_radius="0.30296081071943809" self_min_project_x="-0.20804059716430023" self_min_project_y="-0.18950000279890031" self_min_project_z="-0.086836986191172694" self_max_project_x="0.19098461663934768" self_max_project_y="0.189500007713893" self_max_project_z="0.16669581663199745"/>
+        <joint_index index="1"/>
+        <joint_index index="4"/>
+        <joint_index index="9"/>
+        <joint_index index="18"/>
+        <joint_index index="23"/>
+    </collision_model>
+    <collision_model link_name="base_link_leftarm" link_index="13" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/arm_base_link.STL" parent_link_index="15" offset_x="-0.0012334499999999999" offset_y="0.019000000000000003" offset_z="0.67449999999999999" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="13" center_x="0.00069289253578245363" center_y="0.069250011825916113" center_z="0.0018641757622659683" half_extent_x="0.069250002574448494" half_extent_y="0.054947374080140415" half_extent_z="0.05291289576310225" axis_0_x="3.6732051713617153e-06" axis_0_y="-0.99999999998650768" axis_0_z="3.6732050384680193e-06" axis_1_x="0.9451107376999478" axis_1_y="2.2713651671724122e-06" axis_1_z="-0.32675020042702552" axis_2_x="0.32675020041427372" axis_2_y="4.6718060483508239e-06" axis_2_z="0.9451107376955391" max_radius="0.10302694459655169" self_min_project_x="-0.13850000500679407" self_min_project_y="-0.054901476416387256" self_min_project_z="-0.050924316935676402" self_max_project_x="1.4210307930362376e-10" self_max_project_y="0.054993271743893658" self_max_project_z="0.054901474590528236"/>
+        <joint_index index="11"/>
+    </collision_model>
+    <collision_model link_name="base_link_rightarm" link_index="14" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/arm_base_link.STL" parent_link_index="15" offset_x="0.0012334300000000002" offset_y="-0.019000000000000003" offset_z="0.67449999999999999" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="14" center_x="-0.00069340127479498859" center_y="-0.069250006733772837" center_z="0.0018641757623431288" half_extent_x="0.052912895763031327" half_extent_y="0.054947374080150171" half_extent_z="0.06925000257444755" axis_0_x="-0.32675020044423031" axis_0_y="-2.2713650559280651e-06" axis_0_z="0.94511073769399967" axis_1_x="-0.94511073768959108" axis_1_y="4.6718060995321053e-06" axis_1_z="-0.32675020043147868" axis_2_x="-3.673205121679235e-06" axis_2_y="-0.99999999998650768" axis_2_z="-3.6732050834320518e-06" max_radius="0.10302694459651984" self_min_project_x="-0.050924316935518944" self_min_project_y="-0.054901476416402736" self_min_project_z="-1.4210384258195319e-10" self_max_project_x="0.054901474590543696" self_max_project_y="0.054993271743897607" self_max_project_z="0.13850000500679127"/>
+        <joint_index index="25"/>
+    </collision_model>
+    <collision_model link_name="body_2_link" link_index="15" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/body_2_link.STL" parent_link_index="16" offset_x="-0.00074646999999999995" offset_y="0.105" offset_z="0.2535" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="3">
+        <obb index="15" center_x="0.010246226301183409" center_y="-0.10492981421344769" center_z="0.23329999302312468" half_extent_x="0.15050000687214204" half_extent_y="0.17950000669886443" half_extent_z="0.30829999595882029" axis_0_x="-0.99997472919294161" axis_0_y="-0.0071092176435480336" axis_0_z="-4.6896608207823897e-09" axis_1_x="0.0071092176435480535" axis_1_y="-0.9999747291929415" axis_1_z="-1.3893250005544622e-13" axis_2_x="-4.6895412898724426e-09" axis_2_y="-3.3478734333613757e-11" axis_2_z="1" max_radius="0.38719425613200387" self_min_project_x="-0.16000000645047494" self_min_project_y="-0.074500001493738671" self_min_project_z="-0.075000002980232794" self_max_project_x="0.14100000729380921" self_max_project_y="0.28450001190399021" self_max_project_z="0.54159998893740779"/>
+        <joint_index index="2"/>
+        <joint_index index="3"/>
+        <joint_index index="17"/>
+    </collision_model>
+    <collision_model link_name="body_link" link_index="16" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/body_link.STL" parent_link_index="12" offset_x="0" offset_y="0" offset_z="0.1595" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="16" center_x="-0.011749702510264068" center_y="-8.3533450203406023e-05" center_z="0.079499997922971938" half_extent_x="0.071749999845110879" half_extent_y="0.10300000401109341" half_extent_z="0.080499998240781995" axis_0_x="-0.9999747291947636" axis_0_y="-0.0071092173872707087" axis_0_z="2.2024513442852242e-09" axis_1_x="0.0071092173872706887" axis_1_y="-0.99997472919476349" axis_1_z="-4.9468178287272847e-09" axis_2_x="2.2375637029293648e-09" axis_2_y="-4.9310351566430264e-09" axis_2_z="1" max_radius="0.14912180598700034" self_min_project_x="-0.06000000022673907" self_min_project_y="-0.10300000425450079" self_min_project_z="-0.001000000343688856" self_max_project_x="0.083499999463482716" self_max_project_y="0.10300000376768603" self_max_project_z="0.15999999613787513"/>
+        <joint_index index="0"/>
+    </collision_model>
+    <collision_model link_name="head_link" link_index="17" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/head_link.STL" parent_link_index="15" offset_x="0" offset_y="0" offset_z="0.79049999999999998" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="17" center_x="0.0015140257589256675" center_y="1.072792219299068e-05" center_z="0.10154845440864799" half_extent_x="0.066514207043163243" half_extent_y="0.11700000218062616" half_extent_z="0.10154867648131187" axis_0_x="-0.99997489716714838" axis_0_y="-0.0070855497134492112" axis_0_z="4.5617313764006713e-06" axis_1_x="0.0070855497135228762" axis_1_y="-0.99997489717755261" axis_1_z="1.971980114271736e-13" axis_2_x="4.5616168626553215e-06" axis_2_y="3.2322571651563931e-08" axis_2_z="0.99999999998959532" max_radius="0.16859796542056329" self_min_project_x="-0.068027807572208679" self_min_project_y="-0.11700000212871554" self_min_project_z="-2.1516696828427495e-07" self_max_project_x="0.065000606514117834" self_max_project_y="0.11700000223253672" self_max_project_z="0.20309713779565547"/>
+    </collision_model>
+    <collision_model link_name="left_behind_hip_link" link_index="18" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_behind_hip_link.STL" parent_link_index="12" offset_x="-0.1135" offset_y="0.187" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="18" center_x="-0.067634275564892432" center_y="0.036050000071526665" center_z="-0.18582363137376645" half_extent_x="0.24625062875365308" half_extent_y="0.036049998521805335" half_extent_z="0.049000000647253107" axis_0_x="-0.34202016960469711" axis_0_y="7.1572570616889839e-15" axis_0_z="-0.93969261122112391" axis_1_x="7.6991891133755734e-16" axis_1_y="-1" axis_1_z="-7.9879087051271576e-15" axis_2_x="-0.93969261122112402" axis_2_y="-3.3986418672243259e-15" axis_2_z="0.34202016960469644" max_radius="0.25365325666827626" self_min_project_x="-0.048501248961651366" self_min_project_y="-0.072099998593330591" self_min_project_z="-0.049000001552646749" self_max_project_x="0.44400000854565486" self_max_project_y="-1.549719880200584e-09" self_max_project_z="0.048999999741859784"/>
+        <joint_index index="5"/>
+    </collision_model>
+    <collision_model link_name="left_behind_leg_link" link_index="19" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_behind_leg_link.STL" parent_link_index="18" offset_x="-0.23581000000000002" offset_y="0.23099999999999998" offset_z="-0.25202999999999998" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="19" center_x="-0.077777091308745411" center_y="-0.0083499997854230949" center_z="-0.21369079978129293" half_extent_x="0.067954331370021395" half_extent_y="0.031849998980763322" half_extent_z="0.23019500147105881" axis_0_x="-0.9396926181067049" axis_0_y="1.2917058576659027e-14" axis_0_z="0.34202015068671987" axis_1_x="-1.5686040096361313e-14" axis_1_y="-1" axis_1_z="-5.228680032120438e-15" axis_2_x="0.34202015068671998" axis_2_y="-1.0226198405079119e-14" axis_2_z="0.93969261810670468" max_radius="0.24211970652815948" self_min_project_x="-0.067954332350945834" self_min_project_y="-0.023499999195337906" self_min_project_z="-0.4576000010722493" self_max_project_x="0.06795433038909704" self_max_project_y="0.040199998766188766" self_max_project_z="0.0027900018698681361"/>
+        <joint_index index="6"/>
+    </collision_model>
+    <collision_model link_name="left_behind_wheel_link" link_index="20" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_behind_wheel_link.STL" parent_link_index="19" offset_x="-0.36906000000000005" offset_y="0.25950000000000001" offset_z="-0.61812999999999996" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="20" center_x="-1.3877787807814457e-17" center_y="0.012250000610947796" center_z="2.7755575615628914e-17" half_extent_x="0.090000002462670342" half_extent_y="0.024749999865889799" half_extent_z="0.089927007340550758" axis_0_x="-0.92413634880229167" axis_0_y="-1.0022846475833033e-15" axis_0_z="-0.38206283360511445" axis_1_x="-1.1214374046878832e-15" axis_1_y="-1" axis_1_z="5.0464683210954742e-15" axis_2_x="-0.38206283360511445" axis_2_y="4.9383296851891302e-15" axis_2_z="0.92413634880229156" max_radius="0.1296126135291997" self_min_project_x="-0.090000002462670398" self_min_project_y="-0.037000000476837616" self_min_project_z="-0.089927007340550674" self_max_project_x="0.09000000246267037" self_max_project_y="0.012499999254942027" self_max_project_z="0.089927007340550869"/>
+    </collision_model>
+    <collision_model link_name="left_front_hip_link" link_index="21" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_front_hip_link.STL" parent_link_index="23" offset_x="0.10050000000000001" offset_y="0.13" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="21" center_x="0.066268882511918162" center_y="0.075250002555528939" center_z="-0.1798884574899868" half_extent_x="0.0514996086340909" half_extent_y="0.056250003166506156" half_extent_z="0.24320621248504953" axis_0_x="-0.93835295162565646" axis_0_y="2.5707786757608438e-13" axis_0_z="-0.34567866317639356" axis_1_x="-2.0526788988657052e-13" axis_1_y="-1" axis_1_z="-1.8651745763731648e-13" axis_2_x="-0.34567866317639329" axis_2_y="-1.0390956439726482e-13" axis_2_z="0.93835295162565646" max_radius="0.25488337399096778" self_min_project_x="-0.05149960863406175" self_min_project_y="-0.13150000572201517" self_min_project_z="-0.43491281625108624" self_max_project_x="0.051499608634120217" self_max_project_y="-0.018999999389002792" self_max_project_z="0.051499608719012852"/>
+        <joint_index index="8"/>
+    </collision_model>
+    <collision_model link_name="left_front_knee_link" link_index="22" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_front_knee_link.STL" parent_link_index="21" offset_x="0.23218" offset_y="0.22450000000000001" offset_z="-0.27777999999999997" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="22" center_x="0.082711606252913211" center_y="-0.019500002264960889" center_z="-0.14787351479275207" half_extent_x="0.059999999562388252" half_extent_y="0.042499996721751712" half_extent_z="0.21252697785160982" axis_0_x="-0.91494976849838505" axis_0_y="-1.4513853188196498e-14" axis_0_z="-0.40356774044112059" axis_1_x="-2.8735186383264652e-14" axis_1_y="-1" axis_1_z="1.0187929717702922e-13" axis_2_x="-0.40356774044112059" axis_2_y="1.0447015241768271e-13" axis_2_z="0.91494976849838494" max_radius="0.22488656247889544" self_min_project_x="-0.075999984319622932" self_min_project_y="-0.022999994456808354" self_min_project_z="-0.38120355202202671" self_max_project_x="0.044000014805153594" self_max_project_y="0.061999998986695254" self_max_project_z="0.043850403681192918"/>
+        <joint_index index="10"/>
+    </collision_model>
+    <collision_model link_name="left_front_roll_link" link_index="23" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_front_roll_link.STL" parent_link_index="12" offset_x="0.10050000000000001" offset_y="0.13" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="23" center_x="-0.0060000009834504792" center_y="0.0140000004321412" center_z="1.334477075717877e-08" half_extent_x="0.072124961378683197" half_extent_y="0.042499998584399526" half_extent_z="0.072124824554142472" axis_0_x="-0.70710796070733783" axis_0_y="-4.6867022188624165e-13" axis_0_z="-0.70710560166378977" axis_1_x="3.7192472563758787e-13" axis_1_y="-1" axis_1_z="2.9078048128206499e-13" axis_2_x="-0.70710560166378977" axis_2_y="-5.7435420969695666e-14" axis_2_z="0.70710796070733772" max_radius="0.11050000113355808" self_min_project_x="-0.067882322355202251" self_min_project_y="-0.056499999016542971" self_min_project_z="-0.067882180812563572" self_max_project_x="0.076367600402164199" self_max_project_y="0.028499998152256109" self_max_project_z="0.0763674682957214"/>
+        <joint_index index="7"/>
+    </collision_model>
+    <collision_model link_name="left_front_wheel_link" link_index="24" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/left_front_wheel_link.STL" parent_link_index="22" offset_x="0.35599000000000003" offset_y="0.23400000000000001" offset_z="-0.61795" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="24" center_x="3.2708846742224296e-10" center_y="-0.028499999980103021" center_z="-1.1965155949367201e-10" half_extent_x="0.089702018982289203" half_extent_y="0.028500000049700228" half_extent_z="0.089702019112905484" axis_0_x="-0.99678347411764456" axis_0_y="-1.8857479306985774e-14" axis_0_z="0.080141785143325858" axis_1_x="2.1668826539326234e-14" axis_1_y="-1" axis_1_z="3.3842324595127492e-14" axis_2_x="0.080141785143325844" axis_2_y="3.5500422872146359e-14" axis_2_z="0.99678347411764467" max_radius="0.13001982327808306" self_min_project_x="-0.089702019317914133" self_min_project_y="-6.9597213409045011e-11" self_min_project_z="-0.089702019205959757" self_max_project_x="0.089702018646664272" self_max_project_y="0.057000000029803252" self_max_project_z="0.08970201901985124"/>
+    </collision_model>
+    <collision_model link_name="right_behind_hip_link" link_index="25" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_behind_hip_link.STL" parent_link_index="12" offset_x="-0.1135" offset_y="-0.187" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="25" center_x="-0.067634275564897789" center_y="-0.036050000071526318" center_z="-0.18582363137376409" half_extent_x="0.2462506287536535" half_extent_y="0.036049998521809373" half_extent_z="0.049000000647252726" axis_0_x="-0.34202016960476089" axis_0_y="-1.6004520544290837e-14" axis_0_z="-0.93969261122110059" axis_1_x="-1.0124433684087744e-13" axis_1_y="-1" axis_1_z="5.3798083929705791e-14" axis_2_x="-0.93969261122110082" axis_2_y="1.1345610059534422e-13" axis_2_z="0.34202016960476062" max_radius="0.2536532566682772" self_min_project_x="-0.048501248961651866" self_min_project_y="1.5497137670350547e-09" self_min_project_z="-0.049000001552657921" self_max_project_x="0.44400000854565519" self_max_project_y="0.072099998593332576" self_max_project_z="0.048999999741847697"/>
+        <joint_index index="19"/>
+    </collision_model>
+    <collision_model link_name="right_behind_leg_link" link_index="26" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_behind_leg_link.STL" parent_link_index="25" offset_x="-0.23581000000000002" offset_y="-0.23099999999999998" offset_z="-0.25202999999999998" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="26" center_x="-0.077777092743759602" center_y="0.008349999693179469" center_z="-0.2136907990518703" half_extent_x="0.23019500165437812" half_extent_y="0.067954331375524812" half_extent_z="0.031849999041599596" axis_0_x="-0.34202014166172545" axis_0_y="5.6976052622285188e-10" axis_0_z="-0.93969262139153431" axis_1_x="-0.9396926213915342" axis_1_y="-7.291883022369947e-10" axis_1_z="0.34202014166172534" axis_2_x="-4.9034321599730833e-10" axis_2_y="1" axis_2_z="7.8479687609790861e-10" max_radius="0.24211970671199748" self_min_project_x="-0.002790002243059253" self_min_project_y="-0.067954328580545578" self_min_project_z="-0.02349999947798654" self_max_project_x="0.45760000106569709" self_max_project_y="0.067954334170504047" self_max_project_z="0.040199998605212679"/>
+        <joint_index index="20"/>
+    </collision_model>
+    <collision_model link_name="right_behind_wheel_link" link_index="27" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_behind_wheel_link.STL" parent_link_index="26" offset_x="-0.36906000000000005" offset_y="-0.25950000000000001" offset_z="-0.61812999999999996" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="27" center_x="4.3021142204224816e-16" center_y="-0.012250000610956846" center_z="-1.0928757898653885e-15" half_extent_x="0.089999997743933066" half_extent_y="0.024749999865902018" half_extent_z="0.089927012085987384" axis_0_x="-0.8673645644294955" axis_0_y="-3.7990445076214155e-14" axis_0_z="-0.49767329883371419" axis_1_x="-8.9714992374986357e-14" axis_1_y="-1" axis_1_z="2.3283844114820679e-13" axis_2_x="-0.49767329883371425" axis_2_y="2.4666569668861339e-13" axis_2_z="0.86736456442949561" max_radius="0.12961261354506909" self_min_project_x="-0.089999997743932414" self_min_project_y="-0.012499999254945186" self_min_project_z="-0.089927012085991576" self_max_project_x="0.089999997743933691" self_max_project_y="0.037000000476858877" self_max_project_z="0.089927012085983221"/>
+    </collision_model>
+    <collision_model link_name="right_front_hip_link" link_index="28" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_front_hip_link.STL" parent_link_index="30" offset_x="0.10050000000000001" offset_y="-0.13" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="28" center_x="0.064865163632250256" center_y="-0.075499999337006365" center_z="-0.1803993585835264" half_extent_x="0.051499608642722065" half_extent_y="0.056499999947878271" half_extent_z="0.24320620472296839" axis_0_x="-0.94101801713870625" axis_0_y="5.9162941107830103e-13" axis_0_z="-0.33835645615288257" axis_1_x="-3.6033123420964415e-13" axis_1_y="-1" axis_1_z="-7.4669867211814463e-13" axis_2_x="-0.3383564561528824" axis_2_y="-5.8071810577779053e-13" axis_2_z="0.94101801713870636" max_radius="0.25493865477838817" self_min_project_x="-0.051499608642789546" self_min_project_y="0.018999999389239408" self_min_project_z="-0.43491279832467405" self_max_project_x="0.051499608642654682" self_max_project_y="0.13199999928499598" self_max_project_z="0.051499611121262723"/>
+        <joint_index index="22"/>
+    </collision_model>
+    <collision_model link_name="right_front_knee_link" link_index="29" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_front_knee_link.STL" parent_link_index="28" offset_x="0.23218" offset_y="-0.22450000000000001" offset_z="-0.27777999999999997" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="29" center_x="0.082711611847593222" center_y="0.019499999471002692" center_z="-0.14787351232504878" half_extent_x="0.060000005677205336" half_extent_y="0.042499999515715349" half_extent_z="0.21252697785162331" axis_0_x="-0.91494976849821674" axis_0_y="2.26092271642773e-14" axis_0_z="-0.40356774044150195" axis_1_x="-5.2245789989462289e-15" axis_1_y="-1" axis_1_z="-4.4408921491042944e-14" axis_2_x="-0.40356774044150229" axis_2_y="-3.8346806473805936e-14" axis_2_z="0.91494976849821674" max_radius="0.22488656463836351" self_min_project_x="-0.075999996549106219" self_min_project_y="-0.061999998986711949" self_min_project_z="-0.38120355202205347" self_max_project_x="0.044000014805304591" self_max_project_y="0.023000000044718832" self_max_project_z="0.043850403681193278"/>
+        <joint_index index="24"/>
+    </collision_model>
+    <collision_model link_name="right_front_roll_link" link_index="30" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_front_roll_link.STL" parent_link_index="12" offset_x="0.10050000000000001" offset_y="-0.13" offset_z="0.084000000000000005" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="1">
+        <obb index="30" center_x="-0.0060000009834484808" center_y="-0.014000000432134741" center_z="1.489437611093547e-08" half_extent_x="0.07212496932275099" half_extent_y="0.042499998584390089" half_extent_z="0.072124816610013215" axis_0_x="-0.7071080976744003" axis_0_y="-2.1165439281206816e-14" axis_0_z="-0.70710546469624369" axis_1_x="-6.0409195999094849e-15" axis_1_y="-1" axis_1_z="3.6000615453514633e-14" axis_2_x="-0.70710546469624369" axis_2_y="2.9631677733884428e-14" axis_2_z="0.7071080976744003" max_radius="0.11050000113352473" self_min_project_x="-0.067882330573194649" self_min_project_y="-0.028499998152255318" self_min_project_z="-0.067882172594500412" self_max_project_x="0.07636760807230733" self_max_project_y="0.056499999016524874" self_max_project_z="0.076367460625526018"/>
+        <joint_index index="21"/>
+    </collision_model>
+    <collision_model link_name="right_front_wheel_link" link_index="31" geometry_type="3" mesh_filename="package://rm_arm_control/meshes/right_front_wheel_link.STL" parent_link_index="29" offset_x="0.35599000000000003" offset_y="-0.2297554" offset_z="-0.61795" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1" obb_count="1" joint_index_count="0">
+        <obb index="31" center_x="2.5927465036046371e-10" center_y="0.024255356285721067" center_z="5.2182268193978754e-09" half_extent_x="0.089702019545633463" half_extent_y="0.02849999954924368" half_extent_z="0.089702016499581483" axis_0_x="-0.81766836616895044" axis_0_y="8.9577014892478104e-14" axis_0_z="-0.57568953696119851" axis_1_x="-1.5387301812731943e-13" axis_1_y="-1" axis_1_z="6.2815251070962678e-14" axis_2_x="-0.57568953696119851" axis_2_y="1.4001243735842529e-13" axis_2_z="0.81766836616895044" max_radius="0.13001982175408211" self_min_project_x="-0.089702022761710548" self_min_project_y="-0.052755355834964761" self_min_project_z="-0.089702012382060797" self_max_project_x="0.089702016329556378" self_max_project_y="0.0042446432635226267" self_max_project_z="0.089702020617102168"/>
+    </collision_model>
+    <joint_model joint_name="body_2_joint" parent_link="16" child_link="15" nextLinkOffset_x="-0.00074646999999999995" nextLinkOffset_y="0.105" nextLinkOffset_z="0.094" joint_index="0" type="1" axis_x="-0.0071092000000000004" axis_y="0.99997000000000003" axis_z="0" limit_lower="-0.5" limit_upper="0.5" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="body_joint" parent_link="12" child_link="16" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0.1595" joint_index="1" type="1" axis_x="0" axis_y="0" axis_z="1" limit_lower="-1.5" limit_upper="1.5" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="head_joint" parent_link="15" child_link="17" nextLinkOffset_x="0.00074646999999999995" nextLinkOffset_y="-0.105" nextLinkOffset_z="0.53700000000000003" joint_index="2" type="1" axis_x="0" axis_y="0" axis_z="1" limit_lower="-3.1400000000000001" limit_upper="3.1400000000000001" limit_effort="10" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_arm_base_joint" parent_link="15" child_link="13" nextLinkOffset_x="-0.00048697999999999998" nextLinkOffset_y="-0.085999999999999993" nextLinkOffset_z="0.42099999999999999" joint_index="3" type="6" axis_x="0" axis_y="0" axis_z="0" limit_lower="0" limit_upper="0" limit_effort="0" limit_velocity="0" rotation_x="-0.50000183660255171" rotation_y="0.49999999999662692" rotation_z="0.49999999999662692" rotation_w="0.49999816339744835"/>
+    <joint_model joint_name="left_behind_hip_joint" parent_link="12" child_link="18" nextLinkOffset_x="-0.1135" nextLinkOffset_y="0.187" nextLinkOffset_z="0.084000000000000005" joint_index="4" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-0.29999999999999999" limit_upper="1" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_behind_leg_joint" parent_link="18" child_link="19" nextLinkOffset_x="-0.12231" nextLinkOffset_y="0.043999999999999997" nextLinkOffset_z="-0.33603" joint_index="5" type="3" axis_x="-0.34201999999999999" axis_y="0" axis_z="-0.93969000000000003" limit_lower="-0.29999999999999999" limit_upper="0" limit_effort="60" limit_velocity="0.29999999999999999" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_behind_wheel_joint" parent_link="19" child_link="20" nextLinkOffset_x="-0.13325000000000001" nextLinkOffset_y="0.028500000000000001" nextLinkOffset_z="-0.36609999999999998" joint_index="6" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-100000" limit_upper="100000" limit_effort="80" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_front_hip_joint" parent_link="23" child_link="21" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="7" type="1" axis_x="0" axis_y="-1" axis_z="0" limit_lower="-0.29999999999999999" limit_upper="1" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_front_knee_joint" parent_link="21" child_link="22" nextLinkOffset_x="0.13167999999999999" nextLinkOffset_y="0.094500000000000001" nextLinkOffset_z="-0.36177999999999999" joint_index="8" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="0" limit_upper="2" limit_effort="100" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_front_roll_joint" parent_link="12" child_link="23" nextLinkOffset_x="0.10050000000000001" nextLinkOffset_y="0.13" nextLinkOffset_z="0.084000000000000005" joint_index="9" type="1" axis_x="-1" axis_y="0" axis_z="0" limit_lower="-0.17499999999999999" limit_upper="0" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_front_wheel_joint" parent_link="22" child_link="24" nextLinkOffset_x="0.12381" nextLinkOffset_y="0.0094999999999999998" nextLinkOffset_z="-0.34016999999999997" joint_index="10" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-100000" limit_upper="100000" limit_effort="80" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="left_joint1" parent_link="13" child_link="0" nextLinkOffset_x="-8.8340582736490258e-07" nextLinkOffset_y="0.24049999999675509" nextLinkOffset_z="-8.8340582737878037e-07" joint_index="11" type="1" axis_x="-3.6732051033605551e-06" axis_y="0.99999999998650768" axis_z="-3.6732051036381108e-06" limit_lower="-3.1000000000000001" limit_upper="3.1000000000000001" limit_effort="60" limit_velocity="3.1400000000000001" rotation_x="-0.50000183660255171" rotation_y="0.49999999999662692" rotation_z="0.49999999999662692" rotation_w="0.49999816339744835"/>
+    <joint_model joint_name="left_joint2" parent_link="0" child_link="1" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="12" type="1" axis_x="0.9999999999865079" axis_y="3.6732051034160662e-06" axis_z="-3.6732051036381108e-06" limit_lower="-2.2679999999999998" limit_upper="2.2679999999999998" limit_effort="60" limit_velocity="3.1400000000000001" rotation_x="0.49999999999662692" rotation_y="0.50000183660255182" rotation_z="0.49999999999662692" rotation_w="0.4999981633974484"/>
+    <joint_model joint_name="left_joint3" parent_link="1" child_link="2" nextLinkOffset_x="-9.4034050657221258e-07" nextLinkOffset_y="0.25599999999827305" nextLinkOffset_z="-2.7755575615628914e-17" joint_index="13" type="1" axis_x="0.99999999998650813" axis_y="3.6732051034081397e-06" axis_z="-3.6732051038601554e-06" limit_lower="-2.355" limit_upper="2.355" limit_effort="30" limit_velocity="3.9199999999999999" rotation_x="0.70710807985947377" rotation_y="1.1102230246251565e-16" rotation_z="0.70710548250646621" rotation_w="-2.5973434669146478e-06"/>
+    <joint_model joint_name="left_joint4" parent_link="2" child_link="3" nextLinkOffset_x="-7.7137023830420592e-07" nextLinkOffset_y="0.20999999999716681" nextLinkOffset_z="7.7137307164976541e-07" joint_index="14" type="1" axis_x="-7.3463967141251807e-06" axis_y="0.99999999996626965" axis_z="3.6732185948462615e-06" limit_lower="-3.1000000000000001" limit_upper="3.1000000000000001" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="0.49999816339407543" rotation_y="0.49999999999325401" rotation_z="0.49999816339407538" rotation_w="-0.50000367319835726"/>
+    <joint_model joint_name="left_joint5" parent_link="3" child_link="4" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="15" type="1" axis_x="0.99999999998650857" axis_y="3.6732051033296354e-06" axis_z="-3.6732051040822e-06" limit_lower="-2.2330000000000001" limit_upper="2.2330000000000001" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="0.70710807985947388" rotation_y="5.5511151231257827e-17" rotation_z="0.70710548250646643" rotation_w="-2.5973434669146478e-06"/>
+    <joint_model joint_name="left_joint6" parent_link="4" child_link="5" nextLinkOffset_x="-5.2893959196872243e-07" nextLinkOffset_y="0.14399999999805738" nextLinkOffset_z="5.2894153485685824e-07" joint_index="16" type="1" axis_x="-7.3463967142917141e-06" axis_y="0.99999999996627009" axis_z="3.6732185944021722e-06" limit_lower="-6.2800000000000002" limit_upper="6.2800000000000002" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="0.49999816339407549" rotation_y="0.49999999999325417" rotation_z="0.49999816339407549" rotation_w="-0.50000367319835737"/>
+    <joint_model joint_name="right_arm_base_joint" parent_link="15" child_link="14" nextLinkOffset_x="0.0019799000000000001" nextLinkOffset_y="-0.124" nextLinkOffset_z="0.42099999999999999" joint_index="17" type="6" axis_x="0" axis_y="0" axis_z="0" limit_lower="0" limit_upper="0" limit_effort="0" limit_velocity="0" rotation_x="0.50000183660255171" rotation_y="0.49999999999662692" rotation_z="-0.49999999999662692" rotation_w="0.49999816339744835"/>
+    <joint_model joint_name="right_behind_hip_joint" parent_link="12" child_link="25" nextLinkOffset_x="-0.1135" nextLinkOffset_y="-0.187" nextLinkOffset_z="0.084000000000000005" joint_index="18" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-0.29999999999999999" limit_upper="1" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_behind_leg_joint" parent_link="25" child_link="26" nextLinkOffset_x="-0.12231" nextLinkOffset_y="-0.043999999999999997" nextLinkOffset_z="-0.33603" joint_index="19" type="3" axis_x="-0.34201999999999999" axis_y="0" axis_z="-0.93969000000000003" limit_lower="-0.29999999999999999" limit_upper="0" limit_effort="60" limit_velocity="0.29999999999999999" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_behind_wheel_joint" parent_link="26" child_link="27" nextLinkOffset_x="-0.13325000000000001" nextLinkOffset_y="-0.028500000000000001" nextLinkOffset_z="-0.36609999999999998" joint_index="20" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-100000" limit_upper="100000" limit_effort="80" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_front_hip_joint" parent_link="30" child_link="28" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="21" type="1" axis_x="0" axis_y="-1" axis_z="0" limit_lower="-0.29999999999999999" limit_upper="1" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_front_knee_joint" parent_link="28" child_link="29" nextLinkOffset_x="0.13167999999999999" nextLinkOffset_y="-0.094500000000000001" nextLinkOffset_z="-0.36177999999999999" joint_index="22" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="0" limit_upper="2" limit_effort="100" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_front_roll_joint" parent_link="12" child_link="30" nextLinkOffset_x="0.10050000000000001" nextLinkOffset_y="-0.13" nextLinkOffset_z="0.084000000000000005" joint_index="23" type="1" axis_x="1" axis_y="0" axis_z="0" limit_lower="-0.17499999999999999" limit_upper="0" limit_effort="120" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_front_wheel_joint" parent_link="29" child_link="31" nextLinkOffset_x="0.12381" nextLinkOffset_y="-0.0052554000000000003" nextLinkOffset_z="-0.34016999999999997" joint_index="24" type="1" axis_x="0" axis_y="1" axis_z="0" limit_lower="-100000" limit_upper="100000" limit_effort="80" limit_velocity="1" rotation_x="0" rotation_y="0" rotation_z="0" rotation_w="1"/>
+    <joint_model joint_name="right_joint1" parent_link="14" child_link="6" nextLinkOffset_x="-8.8340582736490258e-07" nextLinkOffset_y="-0.24049999999675509" nextLinkOffset_z="-8.8340582737878037e-07" joint_index="25" type="1" axis_x="-3.6732051033605551e-06" axis_y="-0.99999999998650768" axis_z="-3.6732051036381108e-06" limit_lower="-3.1000000000000001" limit_upper="3.1000000000000001" limit_effort="60" limit_velocity="3.1400000000000001" rotation_x="0.50000183660255171" rotation_y="0.49999999999662692" rotation_z="-0.49999999999662692" rotation_w="0.49999816339744835"/>
+    <joint_model joint_name="right_joint2" parent_link="6" child_link="7" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="26" type="1" axis_x="-0.9999999999865079" axis_y="3.6732051034160662e-06" axis_z="-3.6732051036381108e-06" limit_lower="-2.2679999999999998" limit_upper="2.2679999999999998" limit_effort="60" limit_velocity="3.1400000000000001" rotation_x="0.49999999999662692" rotation_y="-0.50000183660255182" rotation_z="-0.49999999999662692" rotation_w="0.4999981633974484"/>
+    <joint_model joint_name="right_joint3" parent_link="7" child_link="8" nextLinkOffset_x="-9.4034050657221258e-07" nextLinkOffset_y="-0.25599999999827305" nextLinkOffset_z="2.7755575615628914e-17" joint_index="27" type="1" axis_x="-0.9999999999865079" axis_y="3.6732051034049421e-06" axis_z="-3.6732051038601554e-06" limit_lower="-2.355" limit_upper="2.355" limit_effort="30" limit_velocity="3.9199999999999999" rotation_x="-2.5973530076717211e-06" rotation_y="-0.70710807985470348" rotation_z="5.5511151231257827e-17" rotation_w="0.70710548251123639"/>
+    <joint_model joint_name="right_joint4" parent_link="8" child_link="9" nextLinkOffset_x="-7.7137590512654173e-07" nextLinkOffset_y="-0.20999999999716656" nextLinkOffset_z="7.7137307174800979e-07" joint_index="28" type="1" axis_x="-1.3492484907118296e-11" axis_y="-0.99999999999325417" axis_z="3.6732185955123953e-06" limit_lower="-3.1000000000000001" limit_upper="3.1000000000000001" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="0.49999816339407521" rotation_y="-0.4999999999932539" rotation_z="0.50000183659917874" rotation_w="0.50000000000674638"/>
+    <joint_model joint_name="right_joint5" parent_link="9" child_link="10" nextLinkOffset_x="0" nextLinkOffset_y="0" nextLinkOffset_z="0" joint_index="29" type="1" axis_x="-0.99999999998650824" axis_y="3.6732051034049425e-06" axis_z="-3.6732051043042446e-06" limit_lower="-2.2330000000000001" limit_upper="2.2330000000000001" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="-2.5973530076717211e-06" rotation_y="-0.70710807985470359" rotation_z="5.5511151231257827e-17" rotation_w="0.7071054825112365"/>
+    <joint_model joint_name="right_joint6" parent_link="10" child_link="11" nextLinkOffset_x="-5.2894347780105725e-07" nextLinkOffset_y="-0.14399999999805707" nextLinkOffset_z="5.2894153491292113e-07" joint_index="30" type="1" axis_x="-1.3492484907118296e-11" axis_y="-0.99999999999325462" axis_z="3.6732185950683061e-06" limit_lower="-6.2800000000000002" limit_upper="6.2800000000000002" limit_effort="10" limit_velocity="3.9199999999999999" rotation_x="0.49999816339407532" rotation_y="-0.49999999999325401" rotation_z="0.50000183659917885" rotation_w="0.50000000000674649"/>
+</robot>