From 08cb9da4d08211c9d96bb9af46f350721381274b Mon Sep 17 00:00:00 2001
From: blank885 <1160821687@qq.com>
Date: Thu, 18 Dec 2025 21:09:36 +0800
Subject: [PATCH 1/3] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=A8=A1=E5=9E=8B?=
=?UTF-8?q?=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/LocoMuJoCo/rolling_log.xml | 28 ++++++++++++++++++++++++++++
1 file changed, 28 insertions(+)
create mode 100644 src/LocoMuJoCo/rolling_log.xml
diff --git a/src/LocoMuJoCo/rolling_log.xml b/src/LocoMuJoCo/rolling_log.xml
new file mode 100644
index 0000000000..2ddb876930
--- /dev/null
+++ b/src/LocoMuJoCo/rolling_log.xml
@@ -0,0 +1,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
From 317b0323235d352260895300eff0a26a35b4c104 Mon Sep 17 00:00:00 2001
From: blank885 <1160821687@qq.com>
Date: Fri, 19 Dec 2025 15:36:43 +0800
Subject: [PATCH 2/3] update
---
src/LocoMuJoCo/rolling_log.py | 222 ++++++++++++++++++++++++++++++++++
1 file changed, 222 insertions(+)
create mode 100644 src/LocoMuJoCo/rolling_log.py
diff --git a/src/LocoMuJoCo/rolling_log.py b/src/LocoMuJoCo/rolling_log.py
new file mode 100644
index 0000000000..8e60c20fe6
--- /dev/null
+++ b/src/LocoMuJoCo/rolling_log.py
@@ -0,0 +1,222 @@
+import numpy as np
+import jax
+import mujoco
+from loco_mujoco.task_factories import ImitationFactory, DefaultDatasetConf
+from loco_mujoco.trajectory import TrajectoryInfo, TrajectoryModel, TrajectoryData
+import matplotlib.pyplot as plt
+import time
+
+class LogMotionSynchronizer:
+ """滚木运动同步器:空中下落+落地反馈+直线滚动(无冗余代码,无报错)"""
+ def __init__(self, env, log_init_pos=[0.0, 0.0, 1.5]):
+ self.env = env
+ self.model = env.get_model()
+ self.data = env.get_data()
+
+ # 滚木状态标记
+ self.is_landed = False
+ self.land_time = None
+ self.roll_torque = 10.0 # 滚动扭矩
+
+ # 添加滚木模型(极简版)
+ self._add_log_to_model(log_init_pos)
+
+ # 关节索引(直接定义,无需通过关节名查找ID)
+ self.log_pos_ids = slice(0, 3) # x/y/z位置
+ self.log_rot_ids = slice(3, 6) # rx/ry/rz姿态
+ self.log_vel_ids = slice(0, 6) # 速度/角速度
+
+ # 数据记录
+ self.log_pos_history = []
+ self.log_vel_history = []
+ self.time_history = []
+
+ def _add_log_to_model(self, init_pos):
+ """添加滚木模型(无自定义assets,兼容旧版Mujoco)"""
+ spec = self.env._mjspec
+
+ # 1. 滚木主体(自由关节)
+ log_body = spec.worldbody.add_body(
+ name="log",
+ pos=init_pos # 空中初始位置:z=1.5m
+ )
+
+ # 自由关节(6自由度:下落+滚动)
+ log_body.add_joint(
+ name="log_root",
+ type=mujoco.mjtJoint.mjJNT_FREE
+ )
+
+ # 2. 胶囊体滚木(直接设置物理属性)
+ # 核心滚木
+ log_body.add_geom(
+ name="log_core",
+ type=mujoco.mjtGeom.mjGEOM_CAPSULE,
+ fromto=(-0.5, 0.0, 0.0, 0.5, 0.0, 0.0),
+ size=(0.18, 0.0, 0.0),
+ rgba=(0.72, 0.5, 0.3, 1.0),
+ friction=(0.8, 0.1, 0.1),
+ density=1200.0,
+ condim=3,
+ margin=0.001
+ )
+ # 外层树皮
+ log_body.add_geom(
+ name="bark_outer",
+ type=mujoco.mjtGeom.mjGEOM_CAPSULE,
+ fromto=(-0.5, 0.0, 0.0, 0.5, 0.0, 0.0),
+ size=(0.19, 0.0, 0.0),
+ rgba=(0.6, 0.4, 0.22, 1.0),
+ friction=(0.85, 0.1, 0.1),
+ density=1300.0,
+ condim=3,
+ margin=0.001
+ )
+
+ # 编译模型
+ self.env._model = spec.compile()
+ self.env._data = mujoco.MjData(self.env._model)
+
+ def check_landing(self):
+ """检测滚木落地"""
+ if self.is_landed:
+ return True
+
+ # 遍历所有接触点
+ for i in range(self.data.ncon):
+ contact = self.data.contact[i]
+ # 获取接触的几何名称
+ geom1_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom1) or ""
+ geom2_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, contact.geom2) or ""
+
+ # 判断滚木与地面接触
+ if ("log" in geom1_name or "log" in geom2_name) and "floor" in (geom1_name + geom2_name):
+ self.is_landed = True
+ self.land_time = time.time()
+ log_pos = self.data.qpos[self.log_pos_ids]
+ print(f"\n【落地反馈】滚木于 {self.land_time - self.start_time:.2f}s 落地!")
+ print(f"落地位置:x={log_pos[0]:.2f}, y={log_pos[1]:.2f}, z={log_pos[2]:.2f}")
+ return True
+ return False
+
+ def apply_roll_force(self):
+ """落地后施加扭矩,让滚木沿x轴滚动"""
+ if not self.is_landed:
+ return
+
+ # 绕y轴施加扭矩
+ mujoco.mj_applyFT(
+ self.model,
+ self.data,
+ np.array([0.0, 0.0, 0.0], dtype=np.float64),
+ np.array([0.0, self.roll_torque, 0.0], dtype=np.float64),
+ self.data.body("log").xpos,
+ self.data.body("log").id
+ )
+
+ def update(self, step_time):
+ """每步更新滚木状态"""
+ self.check_landing()
+ self.apply_roll_force()
+
+ # 记录数据
+ self.log_pos_history.append(self.data.qpos[self.log_pos_ids].copy().astype(np.float64))
+ self.log_vel_history.append(self.data.qvel[self.log_vel_ids].copy().astype(np.float64))
+ self.time_history.append(step_time)
+
+ def plot_motion_curve(self):
+ """绘制滚木运动曲线"""
+ if not self.time_history:
+ print("无运动数据可绘制!")
+ return
+
+ pos_array = np.array(self.log_pos_history)
+ vel_array = np.array(self.log_vel_history)
+ time_array = np.array(self.time_history)
+
+ fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8))
+
+ # 位置曲线
+ ax1.plot(time_array, pos_array[:, 0], label="X轴位置(滚动方向)", color="blue")
+ ax1.plot(time_array, pos_array[:, 2], label="Z轴位置(高度)", color="red")
+ if self.land_time:
+ ax1.axvline(x=self.land_time - self.start_time,
+ color="green", linestyle="--", label="落地时刻")
+ ax1.set_title("滚木位置变化")
+ ax1.set_xlabel("时间 (s)")
+ ax1.set_ylabel("位置 (m)")
+ ax1.legend()
+ ax1.grid(True)
+
+ # 速度曲线
+ ax2.plot(time_array, vel_array[:, 0], label="X轴速度", color="blue")
+ ax2.plot(time_array, vel_array[:, 5], label="绕Y轴角速度(滚动)", color="orange")
+ if self.land_time:
+ ax2.axvline(x=self.land_time - self.start_time,
+ color="green", linestyle="--", label="落地时刻")
+ ax2.set_title("滚木速度变化")
+ ax2.set_xlabel("时间 (s)")
+ ax2.set_ylabel("速度 (m/s) / 角速度 (rad/s)")
+ ax2.legend()
+ ax2.grid(True)
+
+ plt.tight_layout()
+ plt.show()
+
+def main():
+ # 1. 创建LocoMuJoCo环境
+ env = ImitationFactory.make(
+ "UnitreeH1",
+ n_substeps=20,
+ default_dataset_conf=DefaultDatasetConf(["walk", "squat"])
+ )
+ # 核心修复:移除return_info=True,适配旧版LocoEnv
+ env.reset(jax.random.PRNGKey(0))
+
+ # 构造机器人直立动作(不依赖info,直接适配H1关节)
+ # 方案:使用"微小力矩+位置保持",避免机器人因重力倒下
+ stand_action = np.zeros(env.action_dim, dtype=np.float64)
+ # 针对UnitreeH1机器人,为核心关节施加平衡力矩(适配所有动作维度)
+ # 即使维度不匹配,也不会报错,仅核心关节生效
+ torque_bias = 0.6 # 基础平衡力矩,可微调
+ for j in range(min(12, env.action_dim)):
+ # 髋/膝/踝关节(前12个关节)施加平衡力矩
+ stand_action[j] = torque_bias if j % 3 != 1 else torque_bias * 1.2 # 膝关节力矩稍大
+
+ # 初始化滚木(空中位置:z=1.5m)
+ log_sync = LogMotionSynchronizer(env, log_init_pos=[0.0, 0.0, 1.5])
+ log_sync.start_time = time.time()
+
+ # 运行仿真(1000步,约5秒)
+ total_steps = 1000
+ print("开始仿真:机器人直立静止,滚木从空中下落...")
+ try:
+ for step in range(total_steps):
+ # 执行直立动作,保持机器人不倒
+ env.step(stand_action)
+
+ # 更新滚木状态(完全不变)
+ log_sync.update(step * env.dt)
+
+ # 渲染画面
+ env.render()
+
+ # 每50步打印状态
+ if step % 50 == 0:
+ log_pos = log_sync.data.qpos[log_sync.log_pos_ids]
+ log_vel = log_sync.data.qvel[log_sync.log_vel_ids]
+ print(f"\n第 {step} 步 - 状态:")
+ print(f" 机器人:直立静止")
+ print(f" 滚木位置:x={log_pos[0]:.2f}, y={log_pos[1]:.2f}, z={log_pos[2]:.2f}")
+ print(f" 滚木速度:x={log_vel[0]:.2f} m/s, 绕Y轴角速度={log_vel[5]:.2f} rad/s")
+ print(f" 滚木落地状态:{'已落地' if log_sync.is_landed else '下落中'}")
+
+ print("\n仿真结束!")
+ log_sync.plot_motion_curve()
+
+ except KeyboardInterrupt:
+ print("\n仿真被中断!")
+ log_sync.plot_motion_curve()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
From f3cc47597536bda260ba50a7be44e6a963cc3678 Mon Sep 17 00:00:00 2001
From: blank885 <1160821687@qq.com>
Date: Fri, 19 Dec 2025 22:55:15 +0800
Subject: [PATCH 3/3] update
---
src/LocoMuJoCo/rolling_log.xml | 63 ++++++++++++++++++++++++----------
1 file changed, 45 insertions(+), 18 deletions(-)
diff --git a/src/LocoMuJoCo/rolling_log.xml b/src/LocoMuJoCo/rolling_log.xml
index 2ddb876930..6a68b0555e 100644
--- a/src/LocoMuJoCo/rolling_log.xml
+++ b/src/LocoMuJoCo/rolling_log.xml
@@ -1,28 +1,55 @@
-
-
-
-
-
+
+
+
+
-
-
+
+
+
+
-
-
+
+
+
+
+
+
-
-
-
+
+
+
+
+
-
-
-
-
+ name="log_core"
+ type="capsule"
+ fromto="-0.5 0 0 0.5 0 0"
+ size="0.18 0"
+ material="wood_mat"
+ density="800"
+ friction="0.8 0.1
+ solref="0.02 1"
+
+
+
+ name="bark_outer"
+ type="capsule"
+ fromto="-0.5 0 0 0.5 0 0"
+ size="0.19 0"
+ material="bark_mat"
+ density="850"
+ friction="0.85 0.1 0.1"
+ solref="0.02 1"
-
-
+
+
+
\ No newline at end of file