优化碰撞检测
This commit is contained in:
@@ -8,14 +8,14 @@ set(CMAKE_C_STANDARD 11)
|
||||
if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID STREQUAL "Clang")
|
||||
add_compile_options(-Wno-define-redefinition)
|
||||
endif()
|
||||
add_compile_options(-march=native -O3 -DEIGEN_VECTORIZE_SSE4_2)
|
||||
add_compile_options(-g)
|
||||
|
||||
#set(CMAKE_BUILD_TYPE Release)
|
||||
|
||||
# 全局设置 C++ 编译选项
|
||||
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
|
||||
# 全局设置 C 编译选项
|
||||
# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
|
||||
# 全局设置 C++/C 编译选项
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
|
||||
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
|
||||
|
||||
set(ROS_DOMAIN_ID_STR $ENV{ROS_DOMAIN_ID})
|
||||
if(NOT ROS_DOMAIN_ID_STR)
|
||||
@@ -68,6 +68,7 @@ set(SOURCES
|
||||
simulator/rm_arm_simulator.cpp
|
||||
simulator/trapezoidal_trajectory_kinematics.cpp
|
||||
simulator/rm_driver_kinematics.cpp
|
||||
simulator/col_simulator.cpp
|
||||
src/goal_manager.cpp
|
||||
driver/arm_driver.cpp
|
||||
driver/rm_arm_driver.cpp
|
||||
|
||||
@@ -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
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
}]
|
||||
)
|
||||
|
||||
@@ -1,27 +1,19 @@
|
||||
#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 "col_simulator.hpp"
|
||||
|
||||
typedef CGAL::Exact_predicates_inexact_constructions_kernel CgalK;
|
||||
typedef CgalK::Point_3 CgalPoint;
|
||||
|
||||
using namespace Eigen;
|
||||
const int g_bodyLinkCnt[BODY_CNT] = {USED_ARM_DOF + 1, USED_ARM_DOF + 1, USED_OTHER_DOF + 1};
|
||||
const int g_jointLinkCnt[BODY_CNT] = {USED_ARM_DOF, USED_ARM_DOF, USED_OTHER_DOF};
|
||||
const int g_bodyJointCnt[BODY_CNT] = {USED_ARM_DOF, USED_ARM_DOF, USED_OTHER_DOF};
|
||||
float g_leftArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
|
||||
float g_rightArmJoints[COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF] = {0};
|
||||
float g_otherJoints[COLLISION_CHECK_SIMPLE_CNT][USED_OTHER_DOF] = {0};
|
||||
|
||||
const std::string g_bodyStartName[BODY_CNT] = {"LeftLink1", "RightLink1", "base_link"};
|
||||
|
||||
bool *g_linkCollisionDetectMatrix = nullptr;
|
||||
static float zeroFixedAngle = 0;
|
||||
|
||||
int g_linkStartIndex[BODY_CNT] = {0};
|
||||
|
||||
@@ -31,37 +23,7 @@ static double toRadians(float degrees) {
|
||||
return (static_cast<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;
|
||||
}
|
||||
|
||||
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 +45,32 @@ int CollisionSimulator::InitJoint(const urdf::Model &urdf_model)
|
||||
{"right_front_roll_joint", BODY_TYPE_OTHERS}, {"right_front_hip_joint", BODY_TYPE_OTHERS}, {"right_front_knee_joint", BODY_TYPE_OTHERS},
|
||||
{"right_front_wheel_joint", BODY_TYPE_OTHERS}
|
||||
};
|
||||
for (const auto& joint_pair : urdf_model.joints_) {
|
||||
urdf::JointConstSharedPtr joint = joint_pair.second;
|
||||
JointsInfo joint_info;
|
||||
joint_info.name = joint->name;
|
||||
joint_info.type = joint->type;
|
||||
joint_info.parent_link = -1;
|
||||
joint_info.child_link = -1;
|
||||
if (joint->parent_link_name != "") {
|
||||
if (linkCollisionMap_.find(joint->parent_link_name) != linkCollisionMap_.end()) {
|
||||
joint_info.parent_link = linkCollisionMap_[joint->parent_link_name];
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s 的父链接 %s 未找到！", joint->name.c_str(), joint->parent_link_name.c_str());
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if (joint->child_link_name != "") {
|
||||
if (linkCollisionMap_.find(joint->child_link_name) != linkCollisionMap_.end()) {
|
||||
joint_info.child_link = linkCollisionMap_[joint->child_link_name];
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Joint %s 的子链接 %s 未找到！", joint->name.c_str(), joint->child_link_name.c_str());
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
joint_info.nextLinkOffset = Vector3d(joint->parent_to_joint_origin_transform.position.x,
|
||||
joint->parent_to_joint_origin_transform.position.y,
|
||||
joint->parent_to_joint_origin_transform.position.z);
|
||||
joint_info.rotation = Quaterniond(
|
||||
joint->parent_to_joint_origin_transform.rotation.w,
|
||||
joint->parent_to_joint_origin_transform.rotation.x,
|
||||
joint->parent_to_joint_origin_transform.rotation.y,
|
||||
joint->parent_to_joint_origin_transform.rotation.z);
|
||||
if (joint->type != urdf::Joint::FIXED) {
|
||||
int jointAngleIndex = jointNameToIndexMap.at(joint->name);
|
||||
int bodyType = jointNameToBodyType.at(joint->name);
|
||||
for (JointsInfo &joint : jointMap_) {
|
||||
if (joint.type != urdf::Joint::FIXED) {
|
||||
int jointAngleIndex = jointNameToIndexMap.at(joint.name);
|
||||
int bodyType = jointNameToBodyType.at(joint.name);
|
||||
for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
|
||||
joint_info.angle[i] = &joints_[i][bodyType][jointAngleIndex];
|
||||
}
|
||||
joint_info.axis = Vector3d(joint->axis.x, joint->axis.y, joint->axis.z);
|
||||
if (joint->limits != NULL) {
|
||||
joint_info.limit_lower = joint->limits->lower;
|
||||
joint_info.limit_upper = joint->limits->upper;
|
||||
joint_info.limit_v = joint->limits->velocity;
|
||||
joint_info.limit_e = joint->limits->effort;
|
||||
joint.angle[i] = &joints_[i][bodyType][jointAngleIndex];
|
||||
}
|
||||
} else {
|
||||
joint_info.axis = Vector3d::Zero();
|
||||
joint_info.limit_lower = 0.0;
|
||||
joint_info.limit_upper = 0.0;
|
||||
joint_info.limit_v = 0.0;
|
||||
joint_info.limit_e = 0.0;
|
||||
}
|
||||
jointMap_.push_back(joint_info);
|
||||
CollisionStructureInfo *parentColStruct = &collision_structures_[joint_info.parent_link];
|
||||
parentColStruct->joint_list.push_back(joint_info);
|
||||
parentColStruct->joint_count++;
|
||||
CollisionStructureInfo *childColStruct = &collision_structures_[joint_info.child_link];
|
||||
childColStruct->parent_link_index = joint_info.parent_link;
|
||||
}
|
||||
InitJointModel();
|
||||
return 0;
|
||||
}
|
||||
|
||||
int CollisionSimulator::GetMeshLinkdimensions(const std::string& filename, CollisionStructureInfo& col_struct)
|
||||
{
|
||||
shapes::Mesh* mesh = shapes::createMeshFromResource(filename);
|
||||
if (!mesh) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "无法加载网格文件: %s", filename.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<Eigen::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 +91,14 @@ int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_st
|
||||
for (tinyxml2::XMLElement* dc_elem = robot_root->FirstChildElement("disable_collisions");
|
||||
dc_elem;
|
||||
dc_elem = dc_elem->NextSiblingElement("disable_collisions")) {
|
||||
std::string link1 = dc_elem->Attribute("link1");
|
||||
std::string link2 = dc_elem->Attribute("link2");
|
||||
const char* link1_c = dc_elem->Attribute("link1");
|
||||
const char* link2_c = dc_elem->Attribute("link2");
|
||||
if (!link1_c || !link2_c) {
|
||||
RCLCPP_WARN(rclcpp::get_logger(GET_FUNC_LINE_STR()), "disable_collisions missing link1/link2 attribute, skipping.");
|
||||
continue;
|
||||
}
|
||||
std::string link1 = link1_c;
|
||||
std::string link2 = link2_c;
|
||||
int link1Index = linkCollisionMap_.find(link1) != linkCollisionMap_.end() ? linkCollisionMap_[link1] : -1;
|
||||
int link2Index = linkCollisionMap_.find(link2) != linkCollisionMap_.end() ? linkCollisionMap_[link2] : -1;
|
||||
if (link1Index != -1 && link2Index != -1) {
|
||||
@@ -274,126 +106,377 @@ int CollisionSimulator::InitLinkCollisionDetectMatrix(const std::string& srdf_st
|
||||
CollisionStructureInfo* colStruct2 = &collision_structures_[link2Index];
|
||||
for (int obb1Idx : colStruct1->obbLinkIndexList) {
|
||||
for (int obb2Idx : colStruct2->obbLinkIndexList) {
|
||||
g_linkCollisionDetectMatrix[obb1Idx * maxObbCnt + obb2Idx] = false;
|
||||
g_linkCollisionDetectMatrix[obb2Idx * maxObbCnt + obb1Idx] = false;
|
||||
if (obb1Idx >= 0 && obb1Idx < maxObbCnt && obb2Idx >= 0 && obb2Idx < maxObbCnt) {
|
||||
linkCollisionDetectMatrix_[
|
||||
static_cast<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 == urdf::Joint::REVOLUTE) {
|
||||
double angleRad = toRadians(jointInfo->angle[simple][0]);
|
||||
rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation;
|
||||
} else if (jointInfo->type == urdf::Joint::PRISMATIC) {
|
||||
double displacement = jointInfo->angle[simple][0];
|
||||
offset = offset + rotation * jointInfo->axis * displacement;
|
||||
}
|
||||
|
||||
for (int i = 0; i < colStruct->obbModelList.size(); i++) {
|
||||
int obbIndex = colStruct->obbLinkIndexList[i];
|
||||
addedOBBs_[simple][obbIndex] = gjk_interface::updateOBBFromPoints(colStruct->obbModelList[i],
|
||||
offset,
|
||||
rotation);
|
||||
nowChild->rotation[simple] = rotation;
|
||||
nowChild->offset[simple] = offset;
|
||||
|
||||
for (int i = 0; i < nowChild->obbModelList.size(); i++) {
|
||||
int obbIndex = nowChild->obbLinkIndexList[i];
|
||||
addedOBBs_[simple][obbIndex] = gjk_interface::updateOBBFromPoints(nowChild->obbModelList[i],
|
||||
offset,
|
||||
rotation);
|
||||
}
|
||||
|
||||
if (nowChild->joint_count > 0) {
|
||||
childCnt++;
|
||||
child.push_back(nowChild);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -414,22 +497,43 @@ void CollisionSimulator::InitPolyhedronsList()
|
||||
}
|
||||
}
|
||||
|
||||
int CollisionSimulator::CheckCollision()
|
||||
int checkCnt = 0;
|
||||
int64_t prepareTime = 0;
|
||||
int64_t checkTime = 0;
|
||||
int64_t checkTime2 = 0;
|
||||
int CollisionSimulator::CheckCollision(rclcpp::Node *node)
|
||||
{
|
||||
for (int bodyType = 0; bodyType < BODY_CNT; bodyType++) {
|
||||
int64_t currentTimeNs = node->get_clock()->now().nanoseconds();
|
||||
for (int bodyType = 0; bodyType < BODY_TYPE_OTHERS; bodyType++) {
|
||||
float *jointList = joints_[simpleUpdateIndex_][bodyType];
|
||||
trajectory_[bodyType]->getAngleForChecking(jointList);
|
||||
}
|
||||
if (CheckJointsAngleLimit(simpleUpdateIndex_, BODY_TYPE_OTHERS) != 0) {
|
||||
if (CheckBodyAngleLimit(simpleUpdateIndex_, BODY_TYPE_OTHERS) != 0) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "CheckCollision joints limit error.");
|
||||
return ARM_AIM_CANNOT_REACH;
|
||||
}
|
||||
UpdatePolyhedrons(BODY_TYPE_OTHERS, simpleUpdateIndex_);
|
||||
int64_t updateNs = node->get_clock()->now().nanoseconds();
|
||||
prepareTime += updateNs - currentTimeNs;
|
||||
int lastSimpleIndex = (simpleUpdateIndex_ + COLLISION_CHECK_SIMPLE_CNT - 1) % COLLISION_CHECK_SIMPLE_CNT;
|
||||
std::vector<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 +556,66 @@ int CollisionSimulator::RemovePolyhedron(int id)
|
||||
return -1; // 未找到对应ID的多面体
|
||||
}
|
||||
|
||||
int CollisionSimulator::CheckJointsAngleLimit(int simple, int bodyType)
|
||||
int CollisionSimulator::CheckBodyAngleLimit(int simple, int bodyType)
|
||||
{
|
||||
CollisionStructureInfo *colStruct = &collision_structures_[g_linkStartIndex[bodyType]];
|
||||
std::vector<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 +632,7 @@ int CollisionSimulator::UpdateCollisionForNewGoal(int bodyType)
|
||||
trajectory->getAngleForStarting(jointList20);
|
||||
trajectory->getAngleForChecking(jointList40);
|
||||
|
||||
if (CheckJointsAngleLimit(simple20, bodyType) != 0 || CheckJointsAngleLimit(simple40, bodyType) != 0) {
|
||||
if (CheckBodyAngleLimit(simple20, bodyType) != 0 || CheckBodyAngleLimit(simple40, bodyType) != 0) {
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
|
||||
return ARM_AIM_CANNOT_REACH;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -240,8 +240,7 @@ int RmArmDriverAndKinematics::computeMovingTrajectory(NodataTrajectory *newTraje
|
||||
for (int i = 0; i < USED_ARM_DOF; ++i) {
|
||||
joints[i] = end[i];
|
||||
}
|
||||
rm_movej(robotHandle, joints, 5, 1, 0, 0);
|
||||
return 0;
|
||||
return rm_movej(robotHandle, joints, 5, 1, 0, 0);
|
||||
}
|
||||
|
||||
int RmArmDriverAndKinematics::computeSteppingTrajectory(NodataTrajectory *newTrajectory, const float *start, const float *speed, int armId)
|
||||
|
||||
@@ -28,9 +28,9 @@ RmArmHandler::~RmArmHandler()
|
||||
|
||||
void PrintData(ArmStatusCallbackData *data)
|
||||
{
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK==================");
|
||||
/*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "==================MYCALLBACK==================");
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "errCode: %d", data->errCode);
|
||||
/*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "current:");
|
||||
for (size_t i = 0; i < USED_ARM_DOF; i++){
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointCurrent[i]);
|
||||
}
|
||||
@@ -44,14 +44,14 @@ void PrintData(ArmStatusCallbackData *data)
|
||||
for (size_t i = 0; i < USED_ARM_DOF; i++){
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d ", data->jointErrCode[i]);
|
||||
}
|
||||
*/
|
||||
|
||||
std::string posStr = "joints position: ";
|
||||
for (size_t i = 0; i < USED_ARM_DOF; i++){
|
||||
posStr += std::to_string(data->jointAngle[i]) + " ";
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%s", posStr.c_str());
|
||||
|
||||
/*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "speed:");
|
||||
for (size_t i = 0; i < USED_ARM_DOF; i++){
|
||||
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%.6f ", data->jointSpeed[i]);
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -1800,4 +1800,4 @@
|
||||
effort="80"
|
||||
velocity="1" />
|
||||
</joint>
|
||||
</robot>
|
||||
</robot>
|
||||
161
rm_arm_control/urdf/FHrobot.xml
Normal file
161
rm_arm_control/urdf/FHrobot.xml
Normal file
@@ -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>
|
||||
Reference in New Issue
Block a user