HiveCoreRD/hivecore_robot_arm
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加实机最大角度限制

Browse Source
This commit is contained in:
zj
2025-10-27 17:04:30 +08:00
parent 3d387b2bba
commit cac66111db
3 changed files with 69 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
rm_arm_control/include/col_simulator.hpp
View File

@@ -34,27 +34,29 @@ struct JointInfo {
int parent_link; // 父连杆
int child_link; // 子连杆
Eigen::Quaterniond rotationAll;
Eigen::Vector3d axis; // 关节轴
Eigen::Vector3d offset; // 关节偏移
Eigen::Quaterniond rotation; // 关节旋转
Eigen::Vector3d axis; // 关节轴
Eigen::Vector3d offset; // 关节偏移
Eigen::Vector3d leftBottom; // 左下角点关于关节原点偏移
Eigen::Vector3d nextLinkOffset; // 下一个连杆关于关节原点偏移
Eigen::Quaterniond rotation; // 关节旋转
double limit_lower;
double limit_upper;
double limit_v;
double limit_e;
double angle; // 关节角度
float *angle[COLLISION_CHECK_SIMPLE_CNT]; // 关节角度
};
struct CollisionStructure {
std::string link_name; // 所属链接名称
std::string geometry_type; // 几何类型
std::vector<double> dimensions; // 几何尺寸/缩放因子
std::vector<double> origin_xyz; // 原点坐标
std::vector<double> origin_rpy; // 原点姿态
std::string mesh_filename; // mesh文件路径（如STL）
std::vector<double> mesh_bounds; // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
Polyhedron origin_points;
JointInfo *joint_list;
Eigen::Quaterniond rotationAdd; // 不平行轴关节叠加旋转
bool rotation_add_flag;
std::vector<JointInfo> joint_list;
int joint_count;
int link_index;
int parent_link_index;
};
class CollisionSimulator
@@ -75,10 +77,12 @@ private:
int InitPolyhedrons(const urdf::Model &urdf_model);
void InitArmSinglePolyhedron(int index, Polyhedron *poly, Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation);
void UpdatePolyhedrons(int startLinkIndex, int endLinkIndex, int simple, float *jointList);
int InitCollisionStructureList(const urdf::Model &urdf_model);
int GetMeshLinkdimensions(const std::string& filename, CollisionStructure& col_struct);
std::pair<Eigen::Vector3d, Eigen::Matrix3d> GetBodyTransform(int startIndex);
void InitArmPolyhedronsList();
void InitPolyhedronsList();
void UpdateArmPolyhedrons();
KinematicsManager *trajectory_[BODY_CNT];
@@ -92,6 +96,7 @@ private:
float *joints_[COLLISION_CHECK_SIMPLE_CNT][BODY_CNT];
std::unordered_map<int, Polyhedron> addedPolyhedrons_;
std::map<std::string, int> linkCollisionMap_;
};
#endif // COL_SIMULATOR_HPP

46
rm_arm_control/simulator/col_simulator.cpp
View File

@@ -122,8 +122,50 @@ static int InitJoint(const urdf::Model &urdf_model, std::vector<JointInfo> &join
return 0;
}
int CollisionSimulator::GetMeshLinkdimensions(const std::string& filename, CollisionStructure& col_struct)
{
// 这里可以添加读取网格文件并计算边界盒的代码
// 假设已经计算出边界盒的最小和最大坐标
col_struct.mesh_bounds = {min_x, max_x, min_y, max_y, min_z, max_z};
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) {
CollisionStructure colStruct;
colStruct.link_name = link->name;
colStruct.geometry_type = collision->geometry->type;
if (collision->geometry->type == urdf::Geometry::BOX) {
urdf::Box* box = dynamic_cast<urdf::Box*>(collision->geometry.get());
colStruct.mesh_bounds = {-box->dim.x / 2, box->dim.x / 2,
-box->dim.y / 2, box->dim.y / 2,
-box->dim.z / 2, box->dim.z / 2};
} 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;
}
}
colStruct.link_index = linkIndex;
linkIndex++;
collision_structures_.push_back(colStruct);
}
}
return result;
}
int CollisionSimulator::InitPolyhedrons(const urdf::Model &urdf_model)
{
InitCollisionStructureList(urdf_model);
std::vector<CollisionStructure> collision_structures_temp;
InitJoint(urdf_model, jointMap_);
InitCollisionStructure(urdf_model, collision_structures_temp);
@@ -144,7 +186,7 @@ void CollisionSimulator::InitCollisionSimulator(const urdf::Model &urdf_model, f
}
InitPolyhedrons(urdf_model);
InitArmPolyhedronsList();
InitPolyhedronsList();
UpdateArmPolyhedrons();
}
@@ -272,7 +314,7 @@ void CollisionSimulator::UpdatePolyhedrons(int startLinkIndex, int endLinkIndex,
}
}
void CollisionSimulator::InitArmPolyhedronsList()
void CollisionSimulator::InitPolyhedronsList()
{
for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
polyhedrons_[i].resize(BASE);

10
rm_arm_control/simulator/rm_driver_kinematics.cpp
View File

@@ -101,6 +101,16 @@ int RmArmDriverAndKinematics::InitHandle(rm_robot_handle **robotHandle_, const c
api_log("Invalid degree of freedom(%s)", robot_ip_address);
return UNKNOWN_ERR;
}
result = rm_set_joint_max_pos(*robotHandle_, 2, 112.0f);
if (result != 0) {
api_log("Failed to set joint max pos for arm(%s).", robot_ip_address);
return UNKNOWN_ERR;
}
result = rm_set_joint_min_pos(*robotHandle_, 2, -112.0f);
if (result != 0) {
api_log("Failed to set joint max pos for arm(%s).", robot_ip_address);
return UNKNOWN_ERR;
}
return 0;
}