change robot urdf

config/robot_description/hc_description/dual_arm.urdf

@@ -559,7 +559,37 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <sphere radius="0.002"/>
+        <sphere radius="0.004"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 1 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="0.006 -0.2851 -0.01" rpy="0 0 -1.5707963267"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_FINGER_TIP"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="R_CAM">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.004"/>
       </geometry>
       <material name="">
         <color rgba="0 1 0 1"/> <!-- 绿色 -->
@@ -573,10 +603,11 @@
     </collision>
   </link>
 
-  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+  <joint name="R_CAM_FIXED" type="fixed">
-    <origin xyz="-0.01212 -0.17655 0.07506" rpy="3.14159 0 -1.5708"/>
+    <origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535"/>
     <parent link="R_WRIST_R_S"/>
-    <child link="R_FINGER_TIP"/>
+    <child link="R_CAM"/>
     <axis xyz="0 0 1"/>
   </joint>
+
 </robot>

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)
+#set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-pybind11_add_module(robot_wrapper ${CMAKE_CURRENT_SOURCE_DIR}/robot_wrapper.cpp)
+#pybind11_add_module(robot_wrapper ${CMAKE_CURRENT_SOURCE_DIR}/robot_wrapper.cpp)
-target_link_libraries(robot_wrapper PRIVATE
+#target_link_libraries(robot_wrapper PRIVATE
-        cmvr_es::device::canbus
+#        cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
+#        cmvr_es::device::ti5motor
-        cmvr_es::device::humanoid_robot
+#        cmvr_es::device::humanoid_robot
-        pthread
+#        pthread
-        glog::glog
+#        glog::glog
-        proto-objects
+#        proto-objects
-        ccd
+#        ccd
-        fcl
+#        fcl
-        cmvr_es::device_manager
+#        cmvr_es::device_manager
-        ${OpenCV_LIBS})
+#        ${OpenCV_LIBS})

example/solve_fk.cpp

@@ -31,7 +31,7 @@ int main(int argc, char** argv) {
     std::vector<std::string> 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<std::string> joint_names = {
         "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",

example/solve_ik.cpp

@@ -48,7 +48,7 @@ int main(int argc, char **argv)
     std::vector<std::string> 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<std::string> joint_names = {

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

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