note

  • MuJoCo 专用的世界描述语言（MJCF），定义了世界（重力、地板、灯光）、机械臂本体、夹爪与物体等
  • 机械臂核心类：负责计算机械臂该怎么动。
    • 逆运动学（IK - solve_ik_pose）：这是机器人学最经典的问题。你告诉机械臂“手要去桌面上坐标 (0.4, 0.1, 0.03) 的位置抓东西”，代码会通过雅可比矩阵数学计算，反推算出它的 6个关节电机分别需要转动多少度 才能正好把手伸到那里。
    • 动作平滑（轨迹规划 - plan_trajectory）：如果让电机直接转到目标角度，机械臂会瞬间“瞬移”并鬼畜。这部分代码使用了 Ruckig 库（或者备用的五次多项式公式 _plan_quintic），把这个动作拉长成几十帧，让它起步加速和停止减速都非常丝滑柔和。
  • 随机抓取搬运过程：目标计算、空手靠近、吸附抓取、搬运、平稳放置、安全撤退

文章目录

一、MuJoCo机械臂交互Demo

一个项目宣传风格的 MuJoCo 交互 Demo，包含：

  • 启动后显示 Hello Every-Embodied
  • 输入 1/2/3/4 触发不同动作：
    • 1：机械臂打招呼（Wave）
    • 2：趣味动作（Dance）
    • 3：随机方块抓取与放置（Random Pick-and-Place）
    • 4：自动连播模式（录屏更方便）
  • 采用轨迹规划 + IK：
    • 优先使用 ruckig（jerk 限制轨迹）
    • 未安装时自动回退到 quintic 插值
  • 抓取稳定性增强：
    • 末端姿态约束（抓取时保持更自然的朝向）
    • 随机点可达性与简单碰撞筛选（先筛后抓）
    • 放置防弹飞：落桌稳定阶段 + 先张爪再抬升撤离
  • 不依赖大模型或 GraspNet，适合教学与快速演示
  • MuJoCo 界面 Branding：
    • 窗口标题显示 hello_every_embodied
    • 场景中启用 site label，可见 hello_every_embodied 标签

注：为什么先用 MuJoCo？
对“快速体验 + 跨平台”场景，MuJoCo 的优势是：

  • 安装轻：pip install mujoco 即可
  • 跨平台：Windows / Linux / macOS 均可用
  • 物理仿真稳定，适合做轨迹规划与控制演示
    后续如需更复杂任务（视觉感知、并行训练、大规模场景），可再扩展到 Isaac Sim / ManiSkill / Habitat 等环境。

跑起来的效果：
在这里插入图片描述

二、代码分析

1、MuJoCo 专用的世界描述语言（MJCF）

MuJoCo 专用的世界描述语言（MJCF），定义了：
世界：重力、地板、灯光。
机械臂本体：从底座、肩膀、大臂、小臂一直到手腕的各个圆柱体（capsule），以及连接它们的“铰链（hinge）”。
夹爪与物体：一个能开合的两指夹爪，以及桌上随机出现的小方块（cube）和绿色的目标放置区。

MODEL_XML = r"""
<mujoco model="every_embodied_ur_demo">
  <compiler angle="degree" coordinate="local"/>
  <option timestep="0.01" gravity="0 0 -9.81" iterations="80" ls_iterations="20"/>

  <asset>
    <texture name="grid" type="2d" builtin="checker" rgb1="0.94 0.96 1.0" rgb2="0.90 0.93 0.97" width="512" height="512"/>
    <material name="mat_floor" texture="grid" texrepeat="5 5"/>
    <material name="mat_ur" rgba="0.93 0.93 0.95 1"/>
    <material name="mat_gripper" rgba="0.14 0.14 0.18 1"/>
    <material name="mat_cube" rgba="0.97 0.43 0.18 1"/>
  </asset>

  <worldbody>
    <light name="key" pos="0.0 0.0 2.0" dir="0 0 -1"/>
    <geom name="floor" type="plane" size="2 2 0.1" material="mat_floor"/>

    <body name="robot_base" pos="0 0 0.05">
      <geom name="robot_base_geom" type="cylinder" size="0.09 0.05" rgba="0.16 0.16 0.18 1"/>

      <body name="shoulder_link" pos="0 0 0.05">
        <joint name="shoulder_pan" type="hinge" axis="0 0 1" range="-180 180" damping="2"/>
        <geom name="robot_shoulder_geom" type="capsule" fromto="0 0 0 0 0 0.16" size="0.03" material="mat_ur"/>

        <body name="upper_arm_link" pos="0 0 0.16">
          <joint name="shoulder_lift" type="hinge" axis="0 1 0" range="-170 170" damping="2"/>
          <geom name="robot_upper_arm_geom" type="capsule" fromto="0 0 0 0.25 0 0" size="0.026" material="mat_ur"/>

          <body name="forearm_link" pos="0.25 0 0">
            <joint name="elbow" type="hinge" axis="0 1 0" range="-170 170" damping="1.8"/>
            <geom name="robot_forearm_geom" type="capsule" fromto="0 0 0 0.24 0 0" size="0.022" material="mat_ur"/>

            <body name="wrist1_link" pos="0.24 0 0">
              <joint name="wrist_1" type="hinge" axis="0 1 0" range="-180 180" damping="1.2"/>
              <geom name="robot_wrist1_geom" type="capsule" fromto="0 0 0 0.12 0 0" size="0.018" material="mat_ur"/>

              <body name="wrist2_link" pos="0.12 0 0">
                <joint name="wrist_2" type="hinge" axis="0 0 1" range="-180 180" damping="1.0"/>
                <geom name="robot_wrist2_geom" type="capsule" fromto="0 0 0 0.08 0 0" size="0.016" material="mat_ur"/>

                <body name="wrist3_link" pos="0.08 0 0">
                  <joint name="wrist_3" type="hinge" axis="0 1 0" range="-180 180" damping="0.8"/>
                  <geom name="robot_wrist3_geom" type="box" pos="0.04 0 0" size="0.035 0.025 0.02" material="mat_gripper"/>

                  <body name="tool" pos="0.08 0 0">
                    <site name="ee_site" pos="0 0 0" size="0.008" rgba="0 1 0 1"/>
                    <body name="left_finger" pos="0 0.018 0">
                      <joint name="left_finger_joint" type="slide" axis="0 1 0" range="0 0.03" damping="0.4"/>
                      <geom name="robot_left_finger_geom" type="box" pos="0.02 0.01 0" size="0.02 0.004 0.012" material="mat_gripper"/>
                    </body>
                    <body name="right_finger" pos="0 -0.018 0">
                      <joint name="right_finger_joint" type="slide" axis="0 -1 0" range="0 0.03" damping="0.4"/>
                      <geom name="robot_right_finger_geom" type="box" pos="0.02 -0.01 0" size="0.02 0.004 0.012" material="mat_gripper"/>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>

    <body name="object" pos="0.42 0.0 0.03">
      <freejoint name="object_free"/>
      <geom name="cube" type="box" size="0.02 0.02 0.02" material="mat_cube" friction="1.5 0.08 0.03" solref="0.005 1"/>
    </body>

    <body name="drop_zone" pos="0.30 -0.22 0.002">
      <geom name="drop_zone_geom" type="box" size="0.06 0.06 0.002" rgba="0.2 0.82 0.32 0.45"/>
    </body>

    <body name="banner_anchor" pos="0.12 -0.34 0.22">
      <site name="hello_every_embodied" type="sphere" size="0.001" rgba="0 0 0 0"/>
    </body>
  </worldbody>

  <actuator>
    <position joint="shoulder_pan" kp="120"/>
    <position joint="shoulder_lift" kp="120"/>
    <position joint="elbow" kp="110"/>
    <position joint="wrist_1" kp="90"/>
    <position joint="wrist_2" kp="80"/>
    <position joint="wrist_3" kp="70"/>
    <position joint="left_finger_joint" kp="150"/>
    <position joint="right_finger_joint" kp="150"/>
  </actuator>
</mujoco>
"""

2、机械臂核心类

机械臂核心类：负责计算机械臂该怎么动。

  • 逆运动学（IK - solve_ik_pose）：这是机器人学最经典的问题。你告诉机械臂“手要去桌面上坐标 (0.4, 0.1, 0.03) 的位置抓东西”，代码会通过雅可比矩阵数学计算，反推算出它的 6个关节电机分别需要转动多少度 才能正好把手伸到那里。
  • 动作平滑（轨迹规划 - plan_trajectory）：如果让电机直接转到目标角度，机械臂会瞬间“瞬移”并鬼畜。这部分代码使用了 Ruckig 库（或者备用的五次多项式公式 _plan_quintic），把这个动作拉长成几十帧，让它起步加速和停止减速都非常丝滑柔和。
class URTrajectoryDemo:
    def __init__(self, model: mujoco.MjModel, data: mujoco.MjData):
        self.model = model
        self.data = data
        self.idx = self._build_kinematics_index()
        self.drop_pos = np.array([0.30, -0.22, 0.03], dtype=np.float64)
        self.rng = np.random.default_rng()
        self.carrying = False
        self.attach_offset = np.array([0.0, 0.0, -0.02], dtype=np.float64)
        self.attach_quat = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
        self.pick_success = 0
        self.pick_total = 0

        self.home = np.array([0.0, -0.7, 1.2, -1.1, 1.57, 0.0], dtype=np.float64)
        self.wave_a = np.array([0.3, -0.6, 1.1, -1.3, 1.2, 0.8], dtype=np.float64)
        self.wave_b = np.array([0.3, -0.6, 1.1, -1.3, 1.2, -0.8], dtype=np.float64)
        self.dance_a = np.array([-0.9, -0.8, 1.5, -1.2, 1.2, 0.5], dtype=np.float64)
        self.dance_b = np.array([1.0, -0.9, 1.4, -1.0, 1.6, -0.4], dtype=np.float64)
        self.base_xy = np.array([0.0, 0.0], dtype=np.float64)

    def _build_kinematics_index(self) -> KinematicsIndex:
        arm_jnt_ids = []
        arm_qpos_ids = []
        arm_dof_ids = []
        for name in ARM_JOINTS:
            jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
            arm_jnt_ids.append(jid)
            arm_qpos_ids.append(self.model.jnt_qposadr[jid])
            arm_dof_ids.append(self.model.jnt_dofadr[jid])

        grip_qpos_ids = []
        for name in GRIPPER_JOINTS:
            jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name)
            grip_qpos_ids.append(self.model.jnt_qposadr[jid])

        object_jid = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, "object_free")
        ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
        return KinematicsIndex(
            arm_qpos_ids=np.array(arm_qpos_ids, dtype=np.int32),
            arm_dof_ids=np.array(arm_dof_ids, dtype=np.int32),
            arm_jnt_ids=np.array(arm_jnt_ids, dtype=np.int32),
            gripper_qpos_ids=np.array(grip_qpos_ids, dtype=np.int32),
            object_qpos_adr=int(self.model.jnt_qposadr[object_jid]),
            object_dof_adr=int(self.model.jnt_dofadr[object_jid]),
            ee_site_id=ee_site_id,
        )

    def print_banner(self) -> None:
        print("\n" + "=" * 72)
        print("Hello Every-Embodied! | UR + Gripper + Random Block Grasp Demo")
        print("=" * 72)
        print(f"轨迹规划后端: {'Ruckig (jerk-limited)' if HAS_RUCKIG else 'Quintic fallback'}")
        print("输入说明：")
        print("  1 -> 机械臂打招呼")
        print("  2 -> 机械臂趣味动作")
        print("  3 -> 随机方块抓取与放置（轨迹规划 + IK）")
        print("  4 -> 自动连播模式（适合录屏宣传）")
        print("  q -> 退出")
        print("-" * 72)

    def set_random_block(self, pos: Optional[np.ndarray] = None) -> np.ndarray:
        if pos is None:
            x = self.rng.uniform(0.33, 0.50)
            y = self.rng.uniform(-0.16, 0.16)
            z = 0.03
            pos = np.array([x, y, z], dtype=np.float64)
        yaw = float(self.rng.uniform(-0.35, 0.35))
        cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
        quat = np.array([cy, 0.0, 0.0, sy], dtype=np.float64)
        qadr = self.idx.object_qpos_adr
        self.data.qpos[qadr : qadr + 3] = pos
        self.data.qpos[qadr + 3 : qadr + 7] = quat
        self.data.qvel[self.idx.object_dof_adr : self.idx.object_dof_adr + 6] = 0.0
        self.carrying = False
        mujoco.mj_forward(self.model, self.data)
        return pos

    def get_ee_pos(self) -> np.ndarray:
        return self.data.site_xpos[self.idx.ee_site_id].copy()

    def get_ee_rotmat(self) -> np.ndarray:
        return self.data.site_xmat[self.idx.ee_site_id].reshape(3, 3).copy()

    @staticmethod
    def _normalize(v: np.ndarray) -> np.ndarray:
        n = np.linalg.norm(v)
        if n < 1e-8:
            return v.copy()
        return v / n

    def build_grasp_orientation(self, target_pos: np.ndarray) -> np.ndarray:
        # tool-z points down for natural top-down grasp; tool-x points from base to target
        z_axis = np.array([0.0, 0.0, -1.0], dtype=np.float64)
        x_hint = np.array([target_pos[0] - self.base_xy[0], target_pos[1] - self.base_xy[1], 0.0], dtype=np.float64)
        if np.linalg.norm(x_hint) < 1e-6:
            x_hint = np.array([1.0, 0.0, 0.0], dtype=np.float64)
        x_axis = self._normalize(x_hint)
        y_axis = self._normalize(np.cross(z_axis, x_axis))
        x_axis = self._normalize(np.cross(y_axis, z_axis))
        return np.column_stack([x_axis, y_axis, z_axis])

    @staticmethod
    def orientation_error(R_cur: np.ndarray, R_des: np.ndarray) -> np.ndarray:
        # small-angle orientation error in world frame
        e = 0.5 * (
            np.cross(R_cur[:, 0], R_des[:, 0])
            + np.cross(R_cur[:, 1], R_des[:, 1])
            + np.cross(R_cur[:, 2], R_des[:, 2])
        )
        return e

    @staticmethod
    def rotmat_to_quat(rotmat: np.ndarray) -> np.ndarray:
        quat = np.zeros(4, dtype=np.float64)
        mujoco.mju_mat2Quat(quat, rotmat.reshape(-1))
        return quat

    @staticmethod
    def _smoothstep(t: np.ndarray) -> np.ndarray:
        return t * t * (3.0 - 2.0 * t)

    def _plan_quintic(self, q0: np.ndarray, q1: np.ndarray, steps: int = 120) -> np.ndarray:
        t = np.linspace(0.0, 1.0, steps, dtype=np.float64)
        s = self._smoothstep(t)[:, None]
        return q0[None, :] + (q1 - q0)[None, :] * s

    def _plan_ruckig(self, q0: np.ndarray, q1: np.ndarray) -> Optional[np.ndarray]:
        if not HAS_RUCKIG:
            return None
        dof = len(q0)
        otg = Ruckig(dof, self.model.opt.timestep)
        inp = InputParameter(dof)
        out = OutputParameter(dof)
        inp.current_position = q0.tolist()
        inp.current_velocity = [0.0] * dof
        inp.current_acceleration = [0.0] * dof
        inp.target_position = q1.tolist()
        inp.target_velocity = [0.0] * dof
        inp.target_acceleration = [0.0] * dof
        inp.max_velocity = [1.2] * dof
        inp.max_acceleration = [2.0] * dof
        inp.max_jerk = [8.0] * dof

        traj = []
        result = Result.Working
        safe_max_steps = 3000
        for _ in range(safe_max_steps):
            result = otg.update(inp, out)
            traj.append(np.array(out.new_position, dtype=np.float64))
            out.pass_to_input(inp)
            if result == Result.Finished:
                break
            if result != Result.Working:
                return None
        if not traj:
            return None
        return np.vstack(traj)

    def plan_trajectory(self, q0: np.ndarray, q1: np.ndarray) -> np.ndarray:
        traj = self._plan_ruckig(q0, q1)
        if traj is not None:
            return traj
        return self._plan_quintic(q0, q1, steps=140)

    def solve_ik_pose(
        self,
        target_pos: np.ndarray,
        target_rot: Optional[np.ndarray] = None,
        max_iters: int = 260,
        w_pos: float = 1.0,
        w_ori: float = 0.35,
    ) -> Optional[np.ndarray]:
        q_backup = self.data.qpos.copy()
        qvel_backup = self.data.qvel.copy()
        arm_q = self.data.qpos[self.idx.arm_qpos_ids].copy()

        for _ in range(max_iters):
            self.data.qpos[self.idx.arm_qpos_ids] = arm_q
            mujoco.mj_forward(self.model, self.data)
            err_pos = target_pos - self.get_ee_pos()
            err_ori = np.zeros(3, dtype=np.float64)
            if target_rot is not None:
                err_ori = self.orientation_error(self.get_ee_rotmat(), target_rot)
            if np.linalg.norm(err_pos) < 0.003 and (target_rot is None or np.linalg.norm(err_ori) < 0.05):
                self.data.qpos[:] = q_backup
                self.data.qvel[:] = qvel_backup
                mujoco.mj_forward(self.model, self.data)
                return arm_q.copy()

            jacp = np.zeros((3, self.model.nv), dtype=np.float64)
            jacr = np.zeros((3, self.model.nv), dtype=np.float64)
            mujoco.mj_jacSite(self.model, self.data, jacp, jacr, self.idx.ee_site_id)
            Jp = jacp[:, self.idx.arm_dof_ids]
            Jr = jacr[:, self.idx.arm_dof_ids]

            if target_rot is None:
                J = Jp
                err = err_pos
            else:
                J = np.vstack([w_pos * Jp, w_ori * Jr])
                err = np.hstack([w_pos * err_pos, w_ori * err_ori])

            lam = 1e-3
            dq = J.T @ np.linalg.solve(J @ J.T + lam * np.eye(J.shape[0]), err)
            dq = np.clip(dq, -0.08, 0.08)
            arm_q = arm_q + dq

            # joint range clamp
            for i, jid in enumerate(self.idx.arm_jnt_ids):
                jmin, jmax = self.model.jnt_range[jid]
                arm_q[i] = np.clip(arm_q[i], jmin, jmax)

        self.data.qpos[:] = q_backup
        self.data.qvel[:] = qvel_backup
        mujoco.mj_forward(self.model, self.data)
        return None

    def is_robot_collision_low_risk(self, arm_q: np.ndarray) -> bool:
        # simple collision screening: avoid robot-floor and deep robot-cube intersections
        q_backup = self.data.qpos.copy()
        qvel_backup = self.data.qvel.copy()

        self.data.qpos[self.idx.arm_qpos_ids] = arm_q
        self.data.qpos[self.idx.gripper_qpos_ids] = GRIPPER_OPEN
        mujoco.mj_forward(self.model, self.data)

        ok = True
        for i in range(self.data.ncon):
            c = self.data.contact[i]
            g1 = int(c.geom1)
            g2 = int(c.geom2)
            n1 = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, g1) or ""
            n2 = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, g2) or ""
            pair = {n1, n2}
            if "floor" in pair and any(name.startswith("robot_") for name in pair):
                ok = False
                break
            if "cube" in pair and any(name.startswith("robot_") for name in pair) and c.dist < -0.002:
                ok = False
                break

        self.data.qpos[:] = q_backup
        self.data.qvel[:] = qvel_backup
        mujoco.mj_forward(self.model, self.data)
        return ok

    def _set_ctrl(self, q_arm: np.ndarray, q_gripper: np.ndarray) -> None:
        self.data.ctrl[:6] = q_arm
        self.data.ctrl[6:8] = q_gripper

    def _update_attached_object(self) -> None:
        if not self.carrying:
            return
        ee = self.get_ee_pos()
        qadr = self.idx.object_qpos_adr
        self.data.qpos[qadr : qadr + 3] = ee + self.attach_offset
        self.data.qpos[qadr + 3 : qadr + 7] = self.attach_quat
        self.data.qvel[self.idx.object_dof_adr : self.idx.object_dof_adr + 6] = 0.0

    def pin_object_pose(self, pos: np.ndarray, quat: np.ndarray) -> None:
        qadr = self.idx.object_qpos_adr
        self.data.qpos[qadr : qadr + 3] = pos
        self.data.qpos[qadr + 3 : qadr + 7] = quat
        self.data.qvel[self.idx.object_dof_adr : self.idx.object_dof_adr + 6] = 0.0
        mujoco.mj_forward(self.model, self.data)

    def settle_object(self, steps: int = 40, viewer=None, realtime: bool = True) -> None:
        # Small settle loop to avoid contact impulse "catapult" at release.
        q_hold = self.data.qpos[self.idx.arm_qpos_ids].copy()
        qadr = self.idx.object_qpos_adr
        for _ in range(steps):
            self._set_ctrl(q_hold, GRIPPER_OPEN)
            mujoco.mj_step(self.model, self.data)
            # Keep object velocities small during initial release.
            self.data.qvel[self.idx.object_dof_adr : self.idx.object_dof_adr + 6] *= 0.2
            # Safety clamp: if object sinks slightly, pin back to table top.
            if self.data.qpos[qadr + 2] < 0.021:
                self.data.qpos[qadr + 2] = 0.021
                self.data.qvel[self.idx.object_dof_adr : self.idx.object_dof_adr + 3] = 0.0
            if viewer is not None:
                viewer.sync()
            if realtime:
                time.sleep(self.model.opt.timestep)

    def execute_trajectory(self, q_traj: np.ndarray, gripper: np.ndarray, viewer=None, realtime: bool = True) -> None:
        for q in q_traj:
            self._set_ctrl(q, gripper)
            mujoco.mj_step(self.model, self.data)
            self._update_attached_object()
            if viewer is not None:
                viewer.sync()
            if realtime:
                time.sleep(self.model.opt.timestep)

    def move_arm_to(self, target_q: np.ndarray, gripper: np.ndarray, viewer=None, realtime: bool = True) -> None:
        q0 = self.data.qpos[self.idx.arm_qpos_ids].copy()
        traj = self.plan_trajectory(q0, target_q)
        self.execute_trajectory(traj, gripper, viewer=viewer, realtime=realtime)

    def move_to_xyz(
        self,
        target_xyz: np.ndarray,
        gripper: np.ndarray,
        viewer=None,
        realtime: bool = True,
        target_rot: Optional[np.ndarray] = None,
    ) -> bool:
        q_goal = self.solve_ik_pose(target_xyz, target_rot=target_rot)
        if q_goal is None:
            return False
        self.move_arm_to(q_goal, gripper, viewer=viewer, realtime=realtime)
        return True

    def sample_reachable_block(self, trials: int = 25) -> Tuple[np.ndarray, np.ndarray]:
        for _ in range(trials):
            x = self.rng.uniform(0.34, 0.48)
            y = self.rng.uniform(-0.14, 0.14)
            z = 0.03
            pos = np.array([x, y, z], dtype=np.float64)
            target_rot = self.build_grasp_orientation(pos)
            pre = pos + np.array([0.0, 0.0, 0.13], dtype=np.float64)
            grasp = pos + np.array([0.0, 0.0, 0.05], dtype=np.float64)
            q_pre = self.solve_ik_pose(pre, target_rot=target_rot)
            q_grasp = self.solve_ik_pose(grasp, target_rot=target_rot)
            if q_pre is None or q_grasp is None:
                continue
            if not self.is_robot_collision_low_risk(q_pre):
                continue
            if not self.is_robot_collision_low_risk(q_grasp):
                continue
            self.set_random_block(pos)
            return pos, target_rot

        # fallback to central easy pose
        fallback = np.array([0.40, 0.0, 0.03], dtype=np.float64)
        self.set_random_block(fallback)
        return fallback, self.build_grasp_orientation(fallback)

    def routine_wave(self, viewer=None, realtime: bool = True) -> None:
        self.move_arm_to(self.wave_a, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
        for _ in range(2):
            self.move_arm_to(self.wave_b, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            self.move_arm_to(self.wave_a, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
        self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)

    def routine_dance(self, viewer=None, realtime: bool = True) -> None:
        for _ in range(2):
            self.move_arm_to(self.dance_a, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            self.move_arm_to(self.dance_b, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
        self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)

    def routine_autoshow(self, viewer=None, realtime: bool = True, rounds: int = 2) -> None:
        for i in range(rounds):
            print(f"[AutoShow] Round {i + 1}/{rounds}")
            self.routine_wave(viewer=viewer, realtime=realtime)
            self.routine_dance(viewer=viewer, realtime=realtime)
            self.routine_random_pick(viewer=viewer, realtime=realtime)

    def routine_random_pick(self, viewer=None, realtime: bool = True) -> None:
        self.pick_total += 1
        block, grasp_rot = self.sample_reachable_block()
        pre = block + np.array([0.0, 0.0, 0.13], dtype=np.float64)
        grasp = block + np.array([0.0, 0.0, 0.055], dtype=np.float64)
        lift = block + np.array([0.0, 0.0, 0.18], dtype=np.float64)
        place_pre = self.drop_pos + np.array([0.0, 0.0, 0.14], dtype=np.float64)
        place = self.drop_pos + np.array([0.0, 0.0, 0.055], dtype=np.float64)
        place_rot = self.build_grasp_orientation(self.drop_pos)

        ok = self.move_to_xyz(pre, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        ok = ok and self.move_to_xyz(grasp, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        if not ok:
            print("[抓取] IK 失败：目标位置不可达，已返回 home。")
            self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            return

        # close gripper and attach block when close enough (for robust tutorial demo)
        self.execute_trajectory(
            self._plan_quintic(
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                steps=30,
            ),
            GRIPPER_CLOSE,
            viewer=viewer,
            realtime=realtime,
        )
        dist = np.linalg.norm(self.get_ee_pos() - block)
        self.carrying = dist < 0.07
        self.attach_quat = self.rotmat_to_quat(grasp_rot)
        if not self.carrying:
            print("[抓取] 夹爪与目标偏差较大，本次跳过搬运。")
            self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            return

        self.move_to_xyz(lift, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        self.move_to_xyz(place_pre, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=place_rot)
        self.move_to_xyz(place, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=place_rot)

        place_quat = self.rotmat_to_quat(place_rot)
        # Place deterministically on table to avoid late-stage catapult.
        placed_pos = self.drop_pos.copy()
        placed_pos[2] = 0.021  # cube half-height + tiny clearance
        self.pin_object_pose(placed_pos, place_quat)
        self.carrying = False
        self.settle_object(steps=35, viewer=viewer, realtime=realtime)

        # Open gripper first, then retreat up to avoid side impulses.
        self.execute_trajectory(
            self._plan_quintic(
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                steps=24,
            ),
            GRIPPER_OPEN,
            viewer=viewer,
            realtime=realtime,
        )
        self.move_to_xyz(place_pre, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=place_rot)

        self.pick_success += 1
        rate = 100.0 * self.pick_success / max(1, self.pick_total)
        print(f"[抓取] 成功放置到目标区域，累计成功率：{self.pick_success}/{self.pick_total} ({rate:.1f}%)")
        self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)

3、随机抓取搬运

过程：目标计算、空手靠近、吸附抓取、搬运、平稳放置、安全撤退

    def routine_random_pick(self, viewer=None, realtime: bool = True) -> None:
        self.pick_total += 1
        
        # ==========================================
        # 步骤 1: 确定所有的目标空间坐标点 (Waypoints)
        # ==========================================
        # 随机采样一个桌上合理的方块坐标，并生成抓取时的旋转姿态
        block, grasp_rot = self.sample_reachable_block()
        # 计算一系列关键路点：预抓取点(头顶)、抓取点、抬起点、预放置点(目标区头顶)、放置点
        pre = block + np.array([0.0, 0.0, 0.13], dtype=np.float64)
        grasp = block + np.array([0.0, 0.0, 0.055], dtype=np.float64)
        lift = block + np.array([0.0, 0.0, 0.18], dtype=np.float64)
        place_pre = self.drop_pos + np.array([0.0, 0.0, 0.14], dtype=np.float64)
        place = self.drop_pos + np.array([0.0, 0.0, 0.055], dtype=np.float64)
        place_rot = self.build_grasp_orientation(self.drop_pos)

        # ==========================================
        # 步骤 2: 空手靠近阶段 (Approach)
        # ==========================================
        # 控制机械臂依次移动到预抓取点，然后下降到抓取点（夹爪保持张开 GRIPPER_OPEN）
        ok = self.move_to_xyz(pre, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        ok = ok and self.move_to_xyz(grasp, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        if not ok:
            print("[抓取] IK 失败：目标位置不可达，已返回 home。")
            self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            return

        # ==========================================
        # 步骤 3: 闭合夹爪并建立“物理吸附”机制 (Grasp & Attach)
        # ==========================================
        # 保持手臂位置不变，仅执行夹爪闭合动作 (GRIPPER_CLOSE)
        self.execute_trajectory(
            self._plan_quintic(
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                steps=30,
            ),
            GRIPPER_CLOSE,
            viewer=viewer,
            realtime=realtime,
        )
        # Hack(作弊机制)：检查手与方块距离，若足够近，则开启 carrying 标志位
        # 一旦开启，_update_attached_object 函数每帧都会把方块强行绑定在手里，防止掉落
        dist = np.linalg.norm(self.get_ee_pos() - block)
        self.carrying = dist < 0.07
        self.attach_quat = self.rotmat_to_quat(grasp_rot)
        if not self.carrying:
            print("[抓取] 夹爪与目标偏差较大，本次跳过搬运。")
            self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)
            return

        # ==========================================
        # 步骤 4: 搬运过程 (Transport)
        # ==========================================
        # 提起 -> 移动到绿色目标区上空 -> 降落到桌面 (这期间由于 carrying=True, 方块一直跟着手走)
        self.move_to_xyz(lift, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=grasp_rot)
        self.move_to_xyz(place_pre, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=place_rot)
        self.move_to_xyz(place, GRIPPER_CLOSE, viewer=viewer, realtime=realtime, target_rot=place_rot)

        # ==========================================
        # 步骤 5: 平稳放置物体 (Settle)
        # ==========================================
        place_quat = self.rotmat_to_quat(place_rot)
        # 为了防止松手瞬间物理碰撞导致方块乱飞，将方块强制重置到桌面上
        placed_pos = self.drop_pos.copy()
        placed_pos[2] = 0.021  # cube half-height + tiny clearance
        self.pin_object_pose(placed_pos, place_quat)
        
        # 解除物理绑定，方块恢复自由落体
        self.carrying = False
        # 空跑几十帧仿真，让方块彻底稳定不晃动
        self.settle_object(steps=35, viewer=viewer, realtime=realtime)

        # ==========================================
        # 步骤 6: 松开夹爪并安全撤退 (Release & Retreat)
        # ==========================================
        # 保持手臂不动，先张开夹爪 (GRIPPER_OPEN)
        self.execute_trajectory(
            self._plan_quintic(
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                self.data.qpos[self.idx.arm_qpos_ids].copy(),
                steps=24,
            ),
            GRIPPER_OPEN,
            viewer=viewer,
            realtime=realtime,
        )
        # 手臂垂直上升退回上空，防止横向移动时碰倒方块
        self.move_to_xyz(place_pre, GRIPPER_OPEN, viewer=viewer, realtime=realtime, target_rot=place_rot)

        # 更新统计并退回原点 (Home)
        self.pick_success += 1
        rate = 100.0 * self.pick_success / max(1, self.pick_total)
        print(f"[抓取] 成功放置到目标区域，累计成功率：{self.pick_success}/{self.pick_total} ({rate:.1f}%)")
        self.move_arm_to(self.home, GRIPPER_OPEN, viewer=viewer, realtime=realtime)

Reference

[1] https://github.com/datawhalechina/every-embodied/blob/main/examples/01_hello_every_embodied_mujoco.py

Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

H3C交换机Console登录与基础检查实例

本文是 H3C 交换机中小企业运维实战系列第 2 篇，记录通过 Console 登录设备并做基础检查的方法，重点包括串口参数、版本检查、当前配置、端口状态、VLAN 和启动配置确认。

avatar DAMO开发者矩阵
cover

【机器人控制】基于matlab模拟具有环形末端执行器轨迹跟踪的平面机器人联动机制【含Matlab源码 15890期】

avatar DAMO开发者矩阵

基于遗传算法的模糊PID控制器整定附Matlab代码

在自动化控制领域，PID 控制器以其结构简单、稳定性好、可靠性高的特点，成为众多控制系统的核心组成部分。从工业生产中的温度、压力控制，到机器人的运动控制，PID 控制器都发挥着不可或缺的作用。然而，传统 PID 控制器的参数整定往往依赖工程师的经验和反复试错，面对日益复杂多变的系统，这种方法显得力不从心。为了突破这一局限，基于遗传算法的模糊 PID 控制器整定方法应运而生，融合智能算法与模糊控制理

avatar DAMO开发者矩阵