From 9bc8d3e7db0e3d0dc90dc42afcc3beb079396763 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 22 Aug 2025 10:40:49 +0800 Subject: [PATCH] change robot urdf --- .../hc_description/dual_arm.urdf | 39 ++++++++- example/CMakeLists.txt | 28 +++---- example/solve_fk.cpp | 2 +- example/solve_ik.cpp | 2 +- python/math/rpy2rotation.py | 79 +++++++++++++++++++ src/devices/canbus/CMakeLists.txt | 4 +- 6 files changed, 132 insertions(+), 22 deletions(-) create mode 100644 python/math/rpy2rotation.py diff --git a/config/robot_description/hc_description/dual_arm.urdf b/config/robot_description/hc_description/dual_arm.urdf index b606515a..38221c58 100644 --- a/config/robot_description/hc_description/dual_arm.urdf +++ b/config/robot_description/hc_description/dual_arm.urdf @@ -559,7 +559,37 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -573,10 +603,11 @@ - - + + - + + diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index 1f7f49c3..10694a21 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(PkgConfig REQUIRED) find_package(fcl REQUIRED) find_package(OpenCV REQUIRED) -find_package(pybind11 REQUIRED) +#find_package(pybind11 REQUIRED) include_directories(${CMAKE_SOURCE_DIR}/include) @@ -65,16 +65,16 @@ target_link_libraries(torqueOn PRIVATE # ---------------- pybind11 模块 ---------------- -set(CMAKE_POSITION_INDEPENDENT_CODE ON) -pybind11_add_module(robot_wrapper ${CMAKE_CURRENT_SOURCE_DIR}/robot_wrapper.cpp) -target_link_libraries(robot_wrapper PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5motor - cmvr_es::device::humanoid_robot - pthread - glog::glog - proto-objects - ccd - fcl - cmvr_es::device_manager - ${OpenCV_LIBS}) \ No newline at end of file +#set(CMAKE_POSITION_INDEPENDENT_CODE ON) +#pybind11_add_module(robot_wrapper ${CMAKE_CURRENT_SOURCE_DIR}/robot_wrapper.cpp) +#target_link_libraries(robot_wrapper PRIVATE +# cmvr_es::device::canbus +# cmvr_es::device::ti5motor +# cmvr_es::device::humanoid_robot +# pthread +# glog::glog +# proto-objects +# ccd +# fcl +# cmvr_es::device_manager +# ${OpenCV_LIBS}) \ No newline at end of file diff --git a/example/solve_fk.cpp b/example/solve_fk.cpp index 423e01fd..ad52beda 100644 --- a/example/solve_fk.cpp +++ b/example/solve_fk.cpp @@ -31,7 +31,7 @@ int main(int argc, char** argv) { std::vector link_names = { "PELVIS_S", "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S", - "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_FINGER_TIP" + "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP" }; std::vector joint_names = { "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R", diff --git a/example/solve_ik.cpp b/example/solve_ik.cpp index 730c8a9c..d9d41864 100644 --- a/example/solve_ik.cpp +++ b/example/solve_ik.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) std::vector link_names = { "PELVIS_S", "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S", - "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_FINGER_TIP" + "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP" }; std::vector joint_names = { diff --git a/python/math/rpy2rotation.py b/python/math/rpy2rotation.py new file mode 100644 index 00000000..5039208b --- /dev/null +++ b/python/math/rpy2rotation.py @@ -0,0 +1,79 @@ +import numpy as np + +def rpy_to_matrix(roll, pitch, yaw, degrees=False): + """ + URDF: R = Rz(yaw) @ Ry(pitch) @ Rx(roll) + roll, pitch, yaw 默认为弧度；degrees=True 时按角度输入。 + """ + if degrees: + roll, pitch, yaw = np.deg2rad([roll, pitch, yaw]) + + cr, sr = np.cos(roll), np.sin(roll) + cp, sp = np.cos(pitch), np.sin(pitch) + cy, sy = np.cos(yaw), np.sin(yaw) + + Rx = np.array([[1, 0, 0], + [0, cr, -sr], + [0, sr, cr]], dtype=float) + Ry = np.array([[ cp, 0, sp], + [ 0, 1, 0], + [-sp, 0, cp]], dtype=float) + Rz = np.array([[cy, -sy, 0], + [sy, cy, 0], + [ 0, 0, 1]], dtype=float) + + return Rz @ Ry @ Rx + + +def matrix_to_rpy(R, degrees=False, eps=1e-9): + """ + 从旋转矩阵恢复 URDF 的 (roll, pitch, yaw)，满足 R = Rz(yaw)·Ry(pitch)·Rx(roll)。 + 返回弧度；degrees=True 时返回角度。 + 含万向节锁处理。 + """ + R = np.asarray(R, dtype=float) + assert R.shape == (3, 3) + + # 可选：小幅正交化以抑制数值误差（不想要可注释掉） + # 使用极分解的简化：R ≈ U*V^T + U, _, Vt = np.linalg.svd(R) + R = U @ Vt + + # R[2,0] = -sin(pitch) + sp_neg = R[2, 0] + sp_neg = np.clip(sp_neg, -1.0, 1.0) + + # 非万向节锁：|sp_neg| < 1 + if abs(sp_neg) < 1.0 - eps: + pitch = np.arcsin(-sp_neg) # pitch + roll = np.arctan2(R[2, 1], R[2, 2]) # roll + yaw = np.arctan2(R[1, 0], R[0, 0]) # yaw + else: + # 万向节锁：|sp_neg| ≈ 1，此时 yaw 与 roll 耦合 + # 令 yaw = 0，通过 R 的其它项恢复 roll + pitch = np.pi/2 if sp_neg < 0 else -np.pi/2 + yaw = 0.0 + # 当 pitch = ±pi/2 时，R[0,1] 与 R[1,1] 携带 roll 信息 + # 推导自 R = Rz(yaw)·Ry(±pi/2)·Rx(roll) + roll = np.arctan2(-R[0, 1] if sp_neg < 0 else R[0, 1], + R[1, 1]) + + if degrees: + return tuple(np.rad2deg([roll, pitch, yaw])) + return (roll, pitch, yaw) + + +# ---------- 简单自检 ---------- +if __name__ == "__main__": + # rpy = (0.3, -0.6, 1.2) # roll, pitch, yaw (rad) + # R = rpy_to_matrix(*rpy) + # rpy_back = matrix_to_rpy(R) + # print("R:\n", R) + # print("rpy back:", rpy_back) + # print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back)))) + R = np.array([ + [-1, 0, 0], + [0, -1, 0], + [0, 0, 1] + ]) + print(matrix_to_rpy(R)) \ No newline at end of file diff --git a/src/devices/canbus/CMakeLists.txt b/src/devices/canbus/CMakeLists.txt index 847377d9..f9cf48ba 100644 --- a/src/devices/canbus/CMakeLists.txt +++ b/src/devices/canbus/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(protobuf REQUIRED) add_library(canbus SHARED ${CMAKE_CURRENT_SOURCE_DIR}/can_client/socket/socket_can_client_raw.cc - ${CMAKE_CURRENT_SOURCE_DIR}/can_client/pcan/pcan_client.cc +# ${CMAKE_CURRENT_SOURCE_DIR}/can_client/pcan/pcan_client.cc ${CMAKE_CURRENT_SOURCE_DIR}/common/byte.cc) target_include_directories(canbus PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -17,7 +17,7 @@ target_link_libraries(canbus gRPC::grpc++ protobuf::libprotobuf glog::glog - PUBLIC pcanbasic +# PUBLIC pcanbasic ) # --------------------------------------------------------