Skip to content
Merged
Changes from all commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
17c299d
提交选题
dengzhuo0119 Dec 19, 2025
5e6eb70
增加初始代码
dengzhuo0119 Dec 21, 2025
265bcd4
添加初始代码
dengzhuo0119 Dec 22, 2025
45ee132
Merge branch 'OpenHUTB:main' into main
dengzhuo0119 Dec 22, 2025
8611cef
feat: v0.2-添加夹爪开合控制功能(修复XML ctrlrange属性错误)
dengzhuo0119 Dec 22, 2025
81cc517
feat: v0.3-实现夹爪与立方体的接触检测功能
dengzhuo0119 Dec 22, 2025
e8e7b35
feat: v0.1-初始版本-基础机械臂可视化
dengzhuo0119 Dec 22, 2025
be9b7ac
添加初始代码
dengzhuo0119 Dec 22, 2025
d7d8e9c
Merge branch 'OpenHUTB:main' into main
dengzhuo0119 Dec 22, 2025
7596774
新增文件头注释和仿真休眠常量,支持Ctrl+C优雅退出
dengzhuo0119 Dec 23, 2025
12cfd77
Merge branch 'OpenHUTB:main' into main
dengzhuo0119 Dec 23, 2025
50017ed
新增文件头注释和仿真休眠常量,支持Ctrl+C优雅退出
dengzhuo0119 Dec 23, 2025
c2d307a
Merge branch 'OpenHUTB:main' into main
dengzhuo0119 Dec 23, 2025
88c69ac
提取PD控制参数为类内常量并添加类初始化文档字符串
dengzhuo0119 Dec 23, 2025
fd9472a
提取PD控制参数为类内常量并添加类初始化文档字符串
dengzhuo0119 Dec 23, 2025
9517f2e
提取雅克比阻尼系数为常量并完善_compute_jacobian文档
dengzhuo0119 Dec 24, 2025
9f209b4
Merge branch 'main' into main
dengzhuo0119 Dec 24, 2025
f5b25ba
Merge branch 'OpenHUTB:main' into main
dengzhuo0119 Dec 24, 2025
1cf25b0
提取关节速度限制常量并完善_move_step文档字符串
dengzhuo0119 Dec 24, 2025
f3eb289
提取关节速度限制常量并完善_move_step文档字符串
dengzhuo0119 Dec 24, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 16 additions & 4 deletions src/Robot_arm_motion/robot_arm.py
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,12 @@ def __init__(self):
self.PD_KD = 100 # 微分增益
self.TORQUE_LIMIT = 20 # 力矩限制

# 【优化1】提取雅克比伪逆的阻尼系数为类内常量
# 雅克比伪逆参数
self.JACOBIAN_DAMPING = 0.01 # 雅克比伪逆的阻尼系数

# 【优化1】提取关节速度限制为类内常量
self.JOINT_VEL_LIMIT = 0.5 # 关节速度上限

# 打印模型信息
print("=" * 50)
print("📌 模型Body列表:", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
Expand All @@ -84,7 +87,16 @@ def _compute_jacobian(self):
return self.jacp[:, self.joint_ids]

def _move_step(self, target, tol=0.003, speed=0.3):
"""单步移动控制(修复维度匹配问题)"""
"""单步位置控制:基于雅克比伪逆实现末端执行器的位置跟踪

Args:
target (np.ndarray): 末端执行器的目标位置,形状为(3,)的三维坐标[x, y, z]
tol (float): 位置误差容忍阈值,当实际位置与目标位置的欧氏距离小于该值时,认为到达目标
speed (float): 移动速度系数,控制机械臂的运动速度

Returns:
bool: 若到达目标位置返回True,否则返回False
"""
ee_pos = self.get_ee_pos()
error = target - ee_pos
error_norm = np.linalg.norm(error)
Expand All @@ -97,15 +109,15 @@ def _move_step(self, target, tol=0.003, speed=0.3):

# ========== 修正:正确的阻尼伪逆计算 ==========
# 方法1:使用正则化参数的伪逆(推荐)
# 【优化2】使用类内常量替代硬编码的阻尼系数
jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))

# 方法2:若方法1仍报错,可改用numpy伪逆(自动处理维度)
# jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)

# 关节速度指令
joint_vel_cmd = speed * jacobian_pinv @ error
joint_vel_cmd = np.clip(joint_vel_cmd, -0.5, 0.5) # 速度限制
# 【优化2】使用类内常量替代硬编码的关节速度限制
joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT)

# PD力矩计算
torque = np.zeros(7)
Expand Down
Loading