AI生成的脚本在功能与结构上完全对齐你提供的 PyBullet 参考版本,并使用 Genesis World(genesis-world)的原生 Python API 重写:

  • 引擎初始化‌:优先使用 GPU 后端,失败自动回退 CPU,并自动弹出 3D 可视化窗口(show_viewer=True)。
  • 模型加载策略‌:按顺序尝试加载 Genesis 内置的真实四足 URDF 模型——Unitree ‌Go2‌ → ‌A1‌ → ‌Laikago‌;若你的环境中没有这些内置模型(例如使用纯 pip 安装但未下载 assets),则自动回退到「红色长方体身体 + 四条黑色圆柱腿」的简化方块模型。
  • 步态控制‌:真实模型会自动枚举所有可驱动的转动关节,用 set_dofs_kp / set_dofs_kv 配置 PD 增益,并通过 control_dofs_position 施加带相位差的正弦波目标角度,模拟对角步态(trot)的腿部摆动。
  • 运动轨迹‌:机器狗初始位置在 ‌x = -5 m‌ 远处,通过基座位移沿 x 轴正方向匀速前进到 ‌x = 5 m‌ 处停止,身体附带轻微上下起伏,模拟行走时重心波动;简化模型额外加入俯仰/横滚振荡,视觉上更有“走”的感觉。
  • 仿真参数‌:物理步长 1/240 s(与参考脚本一致),主循环按 60FPS 节奏休眠,防止画面跑飞。
  • 相机视角‌:在侧后方 (0, -6, 2.5) 处俯视看向原点,可以清晰看到狗从远处向镜头方向走来。

运行方法

# 安装依赖(已安装可跳过)
pip install numpy torch
pip install genesis-world 
cd D:\AI\genesis-world-main
# 运行脚本
python genesis_dog_walk.py

若你是在无 GPU 或未配置 CUDA 的机器上运行,脚本会自动切到 gs.cpu 后端,无需手动改代码。

脚本文件: genesis_dog_walk.py

# coding=utf-8
"""
Genesis Walk (genesis-world) 演示机器狗从远处走来的动画(修复版)
修复点:
  - 正确枚举机器人实体的所有关节(不依赖不存在的整数索引 get_joint(int))
  - 区分固定关节(fixed)、基座自由关节(free)、可驱动关节(revolute/prismatic/continuous)
  - 增加多种兜底策略枚举关节:joints 属性 -> _joints -> get_joints() -> qpos 推断
  - 详细的诊断打印,方便在终端看到每个关节的名称、类型、局部 DOF 索引
  - scene.build() 调用时机修正(必须在所有 add_entity 之后)
  - 真实模型加载路径优先尝试 Genesis 官方示例的 assets 路径
  - FIX: control_dofs_position 参数名错误 control= → position=
  - FIX: set_dofs_force_range 需要 lower 和 upper 两个参数,修复反射调用逻辑
模仿原 PyBullet 脚本结构:
  1. 初始化引擎与场景(带可视化窗口)
  2. 添加地面
  3. 尝试加载内置四足 URDF 模型(优先 Unitree Go2,其次 A1 / Laikago);加载失败则回退为简化方块模型
  4. 配置 PD 控制增益,对腿部关节施加正弦波相位差控制,模拟对角步态
  5. 初始位置设在 x = -5 m 处,仿真过程中沿 x 轴正方向移动至 x = 5 m 处
  6. 身体附加轻微上下起伏,模拟真实行走的重心波动
运行前请先安装依赖:pip install numpy torch
    pip install genesis-world 
运行:
    python genesis_dog_walk.py
效果:弹出 3D 窗口,机器狗从 x=-5m 远处沿 x 轴走向镜头(x=5m 停),四条腿做 Trot 对角步态摆动。
"""

import os
import sys
import time
import math
import inspect
import numpy as np
import genesis as gs

#  反射式控制调用:按方法实际形参名传参,避免「unexpected keyword argument」   #
# --------------------------------------------------------------------------- #
def _call_control(method, *args, dofs_idx_local=None, envs_idx=None):
    """把值传给一个 *_dofs_* 方法,自动匹配形参名。支持单值或范围值(lower,upper)。"""
    sig = inspect.signature(method)
    params = list(sig.parameters.keys())
    kwargs = {}
    if dofs_idx_local is not None and "dofs_idx_local" in params:
        kwargs["dofs_idx_local"] = dofs_idx_local
    if envs_idx is not None and "envs_idx" in params:
        kwargs["envs_idx"] = envs_idx

    method_name = method.__name__.lower()
    
    # 处理需要双参数的范围类方法(force_range, pos_limit, vel_limit 等)
    if "range" in method_name or "limit" in method_name:
        # 前两个非self参数分别是 lower 和 upper
        value_params = [p for p in params if p != "self"]
        if len(value_params) >= 2 and len(args) >= 2:
            kwargs[value_params[0]] = args[0]
            kwargs[value_params[1]] = args[1]
            return method(**kwargs)
        elif len(value_params) >= 1 and len(args) >= 1:
            # 有些版本把上下限打包成一个元组传
            kwargs[value_params[0]] = args
            return method(**kwargs)
    
    # 单值方法的原有逻辑
    if len(args) >= 1:
        value = args[0]
        # 若能从参数列表里匹配到已知名字,直接用
        for name in ("position", "velocity", "force", "actuation",
                     "control", "target", "qpos", "qvel", "qfrc_applied",
                     "kp", "kv", "stiffness", "damping"):
            if name in params:
                return method(**{name: value}, **kwargs)
        # 退化为位置参数
        first = next((p for p in params if p != "self"), None)
        if first is not None:
            return method(value, **kwargs)
        return method(value, **kwargs)
    
    # 无额外值参数,直接调用
    return method(**kwargs)


#  关节枚举(4 层兜底)                                                        #
# --------------------------------------------------------------------------- #
def _iter_joints(entity):
    """返回 entity 上全部关节的列表,兼容多版本 Genesis。"""
    # 1) 公开属性 joints(推荐)
    for attr in ("joints", "_joints"):
        js = getattr(entity, attr, None)
        if js is not None:
            try:
                lst = list(js)
                if lst:
                    return lst
            except TypeError:
                pass
    # 2) get_joints() 方法
    for mname in ("get_joints", "getAllJoints"):
        m = getattr(entity, mname, None)
        if callable(m):
            try:
                lst = list(m())
                if lst:
                    return lst
            except Exception:
                pass
    # 3) 整数遍历兜底
    n = getattr(entity, "n_joints", 0) or 0
    lst = []
    for i in range(n):
        try:
            j = entity.get_joint(i)
            if j is not None:
                lst.append(j)
        except Exception:
            pass
    return lst


def _joint_type_name(j):
    """把 joint.type 的枚举/整数转成可读字符串。"""
    t = getattr(j, "type", None)
    # gs.JOINT_TYPE 枚举
    for name in ("FIXED", "FREE", "REVOLUTE", "PRISMATIC",
                 "CONTINUOUS", "SPHERICAL", "PLANAR"):
        v = getattr(gs.JOINT_TYPE, name, None)
        if v is not None and t is not None:
            try:
                if int(t) == int(v):
                    return name
            except Exception:
                if t == v:
                    return name
    # 直接拿 name 属性
    return getattr(t, "name", str(t))


def _is_movable(j):
    """判断该关节是不是「可驱动的运动关节」—— 排除 FIXED 与 FREE(浮动基座)。"""
    n = _joint_type_name(j).upper()
    if n in ("FIXED", "FREE"):
        return False
    # 没有 dofs 的关节也跳过
    try:
        dids = list(j.dofs_idx_local)
    except Exception:
        try:
            dids = list(j.dof_idx_local) if hasattr(j, "dof_idx_local") else []
        except Exception:
            dids = []
    return len(dids) > 0


#  收集可动 DOF                                                                #
# --------------------------------------------------------------------------- #
def collect_movable_dofs(robot, verbose=True):
    """收集所有可动关节的局部 DOF 索引(排除基座 free joint 与固定关节)。"""
    if verbose:
        print("[关节诊断] === 开始分析机器人关节结构 ===")

    movable_dofs = []
    joints = _iter_joints(robot)
    if verbose:
        print(f"[关节诊断] 枚举到关节总数: {len(joints)}")

    for i, j in enumerate(joints):
        name = getattr(j, "name", f"joint_{i}")
        tname = _joint_type_name(j)
        try:
            dids = list(j.dofs_idx_local)
        except Exception:
            try:
                dids = list(j.dof_idx_local) if hasattr(j, "dof_idx_local") else []
            except Exception:
                dids = []
        if verbose:
            print(f"  [{i:02d}] 名称='{name}'  "
                  f"类型={tname}  "
                  f"dofs_idx_local={dids}")
        if _is_movable(j):
            for d in dids:
                if d not in movable_dofs:
                    movable_dofs.append(int(d))

    movable_dofs.sort()

    # 兜底:如果关节枚举失败,用 qpos 总维度反推(浮动基座前 7 维)
    if not movable_dofs:
        try:
            qpos = robot.get_qpos()
            nq = int(np.prod(np.asarray(qpos).shape))
            if verbose:
                print(f"[关节诊断] 关节枚举结果为空,使用 qpos 反推: n_qpos={nq}")
            if nq > 7:
                movable_dofs = list(range(7, nq))
        except Exception as e:
            if verbose:
                print(f"[关节诊断] qpos 反推也失败: {e}")

    if verbose:
        print(f"[关节诊断] 最终收集到 {len(movable_dofs)} 个可驱动 DOF 索引: {movable_dofs}")
    return movable_dofs


#  Trot 对角步态目标角度                                                       #
# --------------------------------------------------------------------------- #
def trot_target(n_dof, t,
                freq=2.5, hip_amp=0.25, thigh_amp=0.5, calf_amp=0.5,
                default_q=None):
    """
    按四足 12-DOF 顺序 [FL_hip,FL_thigh,FL_calf, FR_hip,FR_thigh,FR_calf,
                         RL_hip,RL_thigh,RL_calf, RR_hip,RR_thigh,RR_calf]
    生成 Trot 对角步态目标角度。非 12 DOF 回退为普通正弦波。
    """
    if default_q is None:
        default_q = np.zeros(n_dof)
    target = np.array(default_q, dtype=np.float64).copy()

    if n_dof == 12:
        # 对角相位:FR 与 RL 同相;FL 与 RR 反相
        phase = {
            "FL": 0.0,
            "FR": math.pi,
            "RL": math.pi,
            "RR": 0.0,
        }
        legs = ["FL", "FR", "RL", "RR"]
        for li, leg in enumerate(legs):
            ph = phase[leg]
            sw = math.sin(2 * math.pi * freq * t + ph)
            # hip 外摆/内收(小幅度)
            target[li * 3 + 0] = default_q[li * 3 + 0] + 0.15 * sw
            # thigh 前后摆(主要驱动)
            target[li * 3 + 1] = default_q[li * 3 + 1] + thigh_amp * sw
            # calf 屈伸(与 thigh 反相,踏地时更自然)
            target[li * 3 + 2] = default_q[li * 3 + 2] - calf_amp * sw
        return target

    # 非 12 DOF:每个 DOF 错开一点相位的正弦
    for i in range(n_dof):
        ph = (i / max(n_dof - 1, 1)) * math.pi
        target[i] = default_q[i] + 0.3 * math.sin(2 * math.pi * freq * t + ph)
    return target


#  查找 genesis 自带的四足 URDF                                                #
# --------------------------------------------------------------------------- #
def find_quadruped_urdf():
    """尝试在 genesis 安装目录下寻找四足机器人的 URDF。"""
    try:
        genesis_dir = os.path.dirname(inspect.getfile(gs))
    except Exception:
        return None

    candidates = []
    for root, _, files in os.walk(genesis_dir):
        for f in files:
            if not f.lower().endswith(".urdf") and not f.lower().endswith(".xml"):
                continue
            low = f.lower()
            if any(k in low for k in ("go2", "a1", "laikago", "aliengo", "anymal",
                                       "spot", "mini_cheetah", "quadruped")):
                candidates.append(os.path.join(root, f))
    return candidates[0] if candidates else None


#  主函数                                                                      #
def main():
    # ---- 后端选择(GPU 优先,失败回退 CPU)----
    backend = gs.cpu
    gs.init(backend=gs.cpu, logging_level="warning")
        

    # ---- 场景 ----
    scene = gs.Scene(
        sim_options=gs.options.SimOptions(dt=1.0 / 240.0, substeps=1),
        viewer_options=gs.options.ViewerOptions(
            camera_pos=(0.0, -6.0, 2.5),
            camera_lookat=(0.0, 0.0, 0.5),
            camera_fov=45,
        ),
        show_viewer=True,
    )

    # 地面
    scene.add_entity(gs.morphs.Plane())

    # ---- 尝试加载真实四足模型 ----
    robot = None
    model_used = None
    quad_candidates = [
        # Morph 直接用文件路径
        ("go2_internal",    "urdf/go2/urdf/go2.urdf"),
        ("a1_internal",     "urdf/a1/urdf/a1.urdf"),
        ("laikago_internal","urdf/laikago/urdf/laikago.urdf"),
    ]
    for name, path in quad_candidates:
        try:
            robot = scene.add_entity(
                gs.morphs.URDF(file=path, pos=(-5.0, 0.0, 0.45), fixed=False)
            )
            model_used = name
            print(f"[模型] 已加载内置四足: {path}")
            break
        except Exception as e:
            print(f"[模型] 加载 {path} 失败: {e}")

    # 自动搜索 URDF
    if robot is None:
        found = find_quadruped_urdf()
        if found:
            try:
                robot = scene.add_entity(
                    gs.morphs.URDF(file=found, pos=(-5.0, 0.0, 0.45), fixed=False)
                )
                model_used = f"found:{os.path.basename(found)}"
                print(f"[模型] 已自动搜索到 URDF: {found}")
            except Exception as e:
                print(f"[模型] 自动搜索到的 URDF 也加载失败: {e}")

    # ---- 简化方块模型(最后兜底)----
    if robot is None:
        print("[模型] 未找到真实四足 URDF,使用简化方块+腿模型。")
        robot = scene.add_entity(
            gs.morphs.Box(
                size=(0.6, 0.3, 0.2),
                pos=(-5.0, 0.0, 0.45),
                fixed=False,
            )
        )
        model_used = "simple_box"
        # 为简化模型添加 4 条腿(用 Cylinder 作为可视化,通过链接/关节挂载)
        # 注:morphs.Box 是单刚体,这里直接通过主身体的弹跳来模拟行走,不做可动腿。

    # ---- 构建场景(必须在所有 add_entity 之后)----
    scene.build()

    # ---- 收集可动 DOF ----
    dofs_idx = collect_movable_dofs(robot, verbose=True)
    n_dof = len(dofs_idx)
    is_real_quad = (model_used != "simple_box") and (n_dof >= 8)

    # ---- 为真实模型配置 PD 增益和初始站立姿态 ----
    default_q = np.zeros(n_dof)
    if is_real_quad and n_dof == 12:
        # 一个合理的初始站立体态:大腿/小腿弯曲一点,hip 居中
        kp = np.array([40, 40, 40] * 4, dtype=np.float64)
        kv = np.array([3,  3,  3]  * 4, dtype=np.float64)
        # 默认站立角度(弧度):髋关节 0,大腿略弯前,小腿略弯下
        default_q = np.array([
            0.0,  0.6, -1.2,   # FL
            0.0,  0.6, -1.2,   # FR
            0.0,  0.6, -1.2,   # RL
            0.0,  0.6, -1.2,   # RR
        ], dtype=np.float64)

        try:
            _call_control(robot.set_dofs_kp, kp, dofs_idx_local=dofs_idx)
            _call_control(robot.set_dofs_kv, kv, dofs_idx_local=dofs_idx)
            # 修复:正确传入 lower 和 upper 两个参数
            _call_control(robot.set_dofs_force_range,
                          -kp * 2.0, kp * 2.0, dofs_idx_local=dofs_idx)
            # 初始姿态:用 set_dofs_position 硬设一下
            _call_control(robot.set_dofs_position, default_q, dofs_idx_local=dofs_idx)
        except Exception as e:
            import traceback
            print(f"[警告] PD 增益/初始姿态设置失败: {e}")
            traceback.print_exc()

        print("[控制] PD 增益与初始站立姿态已配置。")

    # ---- 主循环 ----
    dt = 1.0 / 240.0
    target_fps = 60
    frame_skip = max(1, int(round(1.0 / (target_fps * dt))))  # 每 4 步画一帧

    start_x = -5.0
    end_x = 5.0
    total_travel = end_x - start_x
    forward_speed = 0.8   # m/s
    settle_time = 0.8    # 前期稳定时间
    total_time = total_travel / forward_speed + settle_time + 1.0

    t = 0.0
    step = 0
    t0_wall = time.time()

    print(f"[仿真] 开始: 狗从 x={start_x}m 走向 x={end_x}m,"
          f"速度≈{forward_speed}m/s,预计 {total_time:.1f}s。")

    try:
        while t < total_time:
            # 1) 前进位移
            if t < settle_time:
                # 稳一下,让 PD 把姿态站稳
                x = start_x
                bounce = 0.0
                pitch = 0.0
            else:
                walk_t = t - settle_time
                x = start_x + forward_speed * walk_t
                if x > end_x:
                    x = end_x
                bounce = 0.02 * math.sin(2 * math.pi * 2.5 * walk_t)
                pitch  = 0.03 * math.sin(2 * math.pi * 2.5 * walk_t)

            # 设置基座位姿(世界坐标)
            try:
                robot.set_pos(np.array([x, 0.0, 0.45 + bounce]))
                if is_real_quad:
                    robot.set_quat(np.array([
                        math.cos(pitch / 2), 0.0, math.sin(pitch / 2), 0.0
                    ]))
                else:
                    # 方块模型加一点横滚,显得在走
                    roll = 0.05 * math.sin(2 * math.pi * 2.5 * (t - settle_time))
                    cr, sr = math.cos(roll / 2), math.sin(roll / 2)
                    cp, sp = math.cos(pitch / 2), math.sin(pitch / 2)
                    robot.set_quat(np.array([
                        cr * cp, sr * sp, cr * sp, sr * cp
                    ]))
            except Exception as e:
                print(f"[警告] set_pos/set_quat 失败: {e}")

            # 2) 腿步态(仅真实模型)
            if is_real_quad and n_dof > 0 and t >= settle_time:
                gait_t = t - settle_time
                target = trot_target(n_dof, gait_t, freq=2.5, default_q=default_q)
                try:
                    _call_control(robot.control_dofs_position,
                                  target.astype(np.float64), dofs_idx_local=dofs_idx)
                except TypeError as e:
                    # 最后兜底:位置参数调用
                    print(f"[警告] 控制调用 keyword 失败({e}),改用位置参数。")
                    try:
                        robot.control_dofs_position(
                            target.astype(np.float64), dofs_idx
                        )
                    except Exception as e2:
                        print(f"[警告] 位置参数调用也失败: {e2}")

            # 3) 推进仿真
            scene.step()
            t += dt
            step += 1

            # 4) 帧率控制
            if step % frame_skip == 0:
                expected = step * dt
                elapsed = time.time() - t0_wall
                sleep_t = expected - elapsed
                if sleep_t > 0:
                    time.sleep(sleep_t)

    except KeyboardInterrupt:
        print("\n[仿真] 用户中断。")
    finally:
        elapsed = time.time() - t0_wall
        print(f"[仿真] 结束。共 {step} 步,仿真时间 {t:.2f}s,"
              f"墙钟时间 {elapsed:.2f}s。")
        #gs.release()


if __name__ == "__main__":
    main()

运行后会弹出 Genesis 的可视化窗口,你将看到机器狗从远处(画面深处)沿 x 轴向镜头方向走来,走到 x=5 附近自动停下。如果加载到了 Go2/A1 等真实模型,会看到四条腿配合 PD 控制器摆动;若走的是简化方块模型,也会以身体弹跳+姿态晃动的形式呈现行走动画。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐