📚 课程目录第4章实现关节位置控制

4. 实现关节位置控制

任务目标

在Isaac Sim中编写Python脚本，通过设置关节位置指令（Joint Position Command），驱动机器人各关节运动到指定角度，并验证关节实际到达目标位置。

环境依赖

Isaac Sim 2023.1.0及以上版本（或Isaac Lab对应版本）
已安装的机器人资产（本案例使用Franka Emika Panda机械臂，Isaac Sim内置）
Python 3.7+（Isaac Sim自带Python环境）

实现原理

Isaac Sim中关节位置控制的核心是调用ArticulationView的set_joint_position_targets()方法，将目标角度写入物理引擎的PD控制器。物理引擎在每个仿真步长内根据当前关节位置与目标位置的误差计算力矩，驱动关节运动。本案例通过一个简单的Panda机械臂场景，演示从加载资产到发送位置指令的完整闭环。

完整代码

"""关节位置控制示例 - Franka Panda机械臂"""
import carb
import numpy as np
from omni.isaac.core import World
from omni.isaac.core.articulations import ArticulationView
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.rotations import quat_to_euler_angles

def main():
    # 1. 初始化世界
    world = World(stage_units_in_meters=1.0)
    world.scene.add_default_ground_plane()
    
    # 2. 加载Panda机械臂
    assets_root = get_assets_root_path()
    if assets_root is None:
        carb.log_error("无法获取资产根路径")
        return
    usd_path = f"{assets_root}/Isaac/Robots/Franka/franka_alt.usd"
    add_reference_to_stage(usd_path, "/World/Franka")
    
    # 3. 创建ArticulationView并重置
    franka_view = ArticulationView("/World/Franka", reset_xform_properties=False)
    world.scene.add(franka_view)
    world.reset()
    
    # 4. 获取关节名称与初始位置
    joint_names = franka_view.dof_names
    print(f"关节列表: {joint_names}")
    
    # Panda有9个关节（7个臂关节+2个手指关节）
    # 目标位置：臂关节运动到指定角度，手指保持张开
    target_positions = np.array([
        0.5,    # joint1: 0.5 rad (~28.6°)
        -0.3,   # joint2: -0.3 rad
        0.8,    # joint3: 0.8 rad
        -1.2,   # joint4: -1.2 rad
        0.6,    # joint5: 0.6 rad
        1.0,    # joint6: 1.0 rad
        -0.4,   # joint7: -0.4 rad
        0.04,   # finger_joint1: 张开
        0.04    # finger_joint2: 张开
    ], dtype=np.float32)
    
    # 5. 发送位置指令并仿真
    print(f"发送目标位置: {target_positions[:7]}")
    for step in range(300):  # 仿真300步（约5秒，60Hz）
        franka_view.set_joint_position_targets(target_positions)
        world.step(render=True)
        
        # 每50步打印一次当前关节位置
        if step % 50 == 0:
            current_pos = franka_view.get_joint_positions()
            error = np.abs(current_pos - target_positions)
            max_error = np.max(error[:7])  # 只检查臂关节
            print(f"Step {step:3d} | 最大关节误差: {max_error:.4f} rad")
    
    # 6. 最终验证
    final_pos = franka_view.get_joint_positions()
    final_error = np.abs(final_pos - target_positions)
    max_final_error = np.max(final_error[:7])
    print(f"\n最终最大关节误差: {max_final_error:.4f} rad")
    
    if max_final_error < 0.05:
        print("✓ 关节位置控制验证通过：所有关节到达目标位置（误差<0.05 rad）")
    else:
        print("✗ 关节未完全到达目标位置，请检查PD增益或增加仿真步数")
    
    # 保持窗口打开
    import time
    time.sleep(2)

if __name__ == "__main__":
    main()

运行方式

启动Isaac Sim：./isaac-sim.sh（Linux）或isaac-sim.bat（Windows）
在Isaac Sim的Python终端中执行：exec(open("joint_position_control.py").read())
或使用命令行直接运行：python joint_position_control.py（需在Isaac Sim的Python环境中）

验证方法

视觉验证：运行脚本后，观察Panda机械臂从初始零位平滑运动到目标姿态，7个臂关节均发生明显转动
数值验证：脚本每50步打印最大关节误差，最终误差应小于0.05 rad（约2.9°）
通过标准：控制台输出✓ 关节位置控制验证通过，且机械臂保持目标姿态稳定不抖动

关键参数说明

目标位置数组：长度必须与机械臂自由度一致（Panda为9），顺序对应dof_names返回的关节顺序
仿真步数：300步约5秒，PD控制器需要足够时间收敛；若增大目标角度范围，需相应增加步数
误差阈值：0.05 rad为工程可接受精度，实际稳态误差通常小于0.01 rad

常见问题

关节顺序不匹配：务必先打印dof_names确认关节顺序，再构造目标数组
目标位置超出限位：Panda关节限位约±2.8 rad，超出限位会导致控制失效，需检查目标角度是否在限位内
控制不生效：确认world.reset()在发送指令前调用，且每步都调用set_joint_position_targets()（PD控制器需要持续接收目标）