SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

change robot urdf

Browse Source
This commit is contained in:
xtkuang 2025-08-22 10:40:49 +08:00
parent c307ba0bb7
commit 9bc8d3e7db
6 changed files with 132 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
config/robot_description/hc_description/dual_arm.urdf
View File

@ -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">
<origin xyz="-0.01212 -0.17655 0.07506" rpy="3.14159 0 -1.5708"/>
<joint name="R_CAM_FIXED" type="fixed">
<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>

28
example/CMakeLists.txt
View File

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

2
example/solve_fk.cpp
View File

@ -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",

2
example/solve_ik.cpp
View File

@ -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 = {

79
python/math/rpy2rotation.py Normal file
View File

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

4
src/devices/canbus/CMakeLists.txt
View File

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