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