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/4] =?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/4] 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/4] 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 From 92d18dd1190135081a393861ab88ce3c1eefb23f Mon Sep 17 00:00:00 2001 From: blank885 <1160821687@qq.com> Date: Sat, 20 Dec 2025 19:28:47 +0800 Subject: [PATCH 4/4] update --- src/LocoMuJoCo/triple.py | 131 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 src/LocoMuJoCo/triple.py diff --git a/src/LocoMuJoCo/triple.py b/src/LocoMuJoCo/triple.py new file mode 100644 index 0000000000..f326d71489 --- /dev/null +++ b/src/LocoMuJoCo/triple.py @@ -0,0 +1,131 @@ +import numpy as np +import jax +import mujoco +from loco_mujoco.task_factories import ImitationFactory, DefaultDatasetConf, LAFAN1DatasetConf +from loco_mujoco.trajectory import Trajectory, TrajectoryInfo, TrajectoryModel, TrajectoryData +from loco_mujoco.core.utils.mujoco import mj_jntname2qposid + +# ===================== 1. 初始化环境+获取帧率参数 ===================== +env = ImitationFactory.make( + "UnitreeH1", + n_substeps=20, + default_dataset_conf=DefaultDatasetConf(["walk", "squat"]), # 行走和下蹲属于默认数据集 + lafan1_dataset_conf=LAFAN1DatasetConf(["dance2_subject4"]) # 舞蹈属于LAFAN1数据集 +) +env.reset(jax.random.PRNGKey(0)) +ENV_DT = env.dt # 环境时间步长(默认0.02s) +FPS = int(1 / ENV_DT) +print(f"环境参数:dt={ENV_DT}s | FPS={FPS}") + +# ===================== 2. 配置各阶段时长及循环参数 ===================== +WALK_DURATION = 8 # 行走8秒 +STAY_DURATION = 1 # 停留1秒 +SQUAT_DURATION = 10 # 下蹲10秒 +DANCE_DURATION = 10 # 跳舞10秒 +CYCLE_TIMES = 3 # 循环次数(可调整) + +# 计算各阶段步数 +WALK_STEPS = int(WALK_DURATION * FPS) +STAY_STEPS = int(STAY_DURATION * FPS) +SQUAT_STEPS = int(SQUAT_DURATION * FPS) +DANCE_STEPS = int(DANCE_DURATION * FPS) +SINGLE_CYCLE_STEPS = WALK_STEPS + STAY_STEPS + SQUAT_STEPS + DANCE_STEPS +TOTAL_STEPS = SINGLE_CYCLE_STEPS * CYCLE_TIMES + +# ===================== 3. 轨迹片段生成函数 ===================== +def get_trajectory_segment(env, dataset_type, dataset_name, target_steps): + """获取指定数据集的轨迹片段并调整到目标步数""" + if dataset_type == "default": + temp_env = ImitationFactory.make( + "UnitreeH1", + default_dataset_conf=DefaultDatasetConf([dataset_name]), + n_substeps=env._n_substeps + ) + elif dataset_type == "lafan1": + temp_env = ImitationFactory.make( + "UnitreeH1", + lafan1_dataset_conf=LAFAN1DatasetConf([dataset_name]), + n_substeps=env._n_substeps + ) + else: + raise ValueError(f"不支持的数据集类型: {dataset_type}") + + temp_env.reset(jax.random.PRNGKey(0)) + raw_qpos = np.array(temp_env.th.traj.data.qpos) + raw_qvel = np.array(temp_env.th.traj.data.qvel) + + if len(raw_qpos) == 0: + raise ValueError(f"数据集 {dataset_name} 无有效轨迹数据!") + + # 循环填充轨迹至目标步数 + traj_qpos = [raw_qpos[i % len(raw_qpos)] for i in range(target_steps)] + traj_qvel = [raw_qvel[i % len(raw_qvel)] for i in range(target_steps)] + return np.array(traj_qpos), np.array(traj_qvel) + +# ===================== 4. 预生成单周期轨迹片段 ===================== +model = env.get_model() +root_joint_ind = mj_jntname2qposid("root", model) # 获取根关节索引 + +# 4.1 行走阶段 +walk_qpos, walk_qvel = get_trajectory_segment( + env, "default", "walk", WALK_STEPS +) +print(f"行走阶段:{WALK_DURATION}秒 | {WALK_STEPS}步") + +# 4.2 停留阶段(复用行走结束姿态,速度归零) +last_walk_qpos = walk_qpos[-1].copy() +last_walk_qvel = np.zeros_like(walk_qvel[0]) +stay_qpos = np.tile(last_walk_qpos, (STAY_STEPS, 1)) +stay_qvel = np.tile(last_walk_qvel, (STAY_STEPS, 1)) +print(f"停留阶段:{STAY_DURATION}秒 | {STAY_STEPS}步") + +# 4.3 下蹲阶段(固定根位置为行走结束位置) +squat_qpos, squat_qvel = get_trajectory_segment( + env, "default", "squat", SQUAT_STEPS +) +root_pos_walk_end = walk_qpos[-1, root_joint_ind[:2]] # 获取行走结束时的x/y位置 +squat_qpos[:, root_joint_ind[:2]] = root_pos_walk_end # 固定下蹲时的根位置 +print(f"下蹲阶段:{SQUAT_DURATION}秒 | {SQUAT_STEPS}步") + +# 4.4 跳舞阶段(固定根位置为行走结束位置) +dance_qpos, dance_qvel = get_trajectory_segment( + env, "lafan1", "dance2_subject4", DANCE_STEPS +) +dance_qpos[:, root_joint_ind[:2]] = root_pos_walk_end # 固定跳舞时的根位置 +print(f"跳舞阶段:{DANCE_DURATION}秒 | {DANCE_STEPS}步") + +# ===================== 5. 生成多循环完整轨迹 ===================== +full_qpos = [] +full_qvel = [] + +for cycle in range(CYCLE_TIMES): + print(f"生成第 {cycle+1}/{CYCLE_TIMES} 个循环轨迹...") + # 按顺序拼接单循环的四个阶段 + full_qpos.extend([walk_qpos, stay_qpos, squat_qpos, dance_qpos]) + full_qvel.extend([walk_qvel, stay_qvel, squat_qvel, dance_qvel]) + +# 合并为numpy数组 +full_qpos = np.concatenate(full_qpos, axis=0) +full_qvel = np.concatenate(full_qvel, axis=0) + +# 验证总时长 +total_duration = len(full_qpos) / FPS +print(f"总轨迹:{CYCLE_TIMES}次循环 | {total_duration:.1f}秒 | {len(full_qpos)}步") + +# ===================== 6. 加载并播放轨迹 ===================== +jnt_names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) for i in range(model.njnt)] +traj_info = TrajectoryInfo( + jnt_names, + model=TrajectoryModel(model.njnt, jax.numpy.array(model.jnt_type)), + frequency=FPS +) +traj_data = TrajectoryData( + jax.numpy.array(full_qpos), + jax.numpy.array(full_qvel), + split_points=jax.numpy.array([0, len(full_qpos)]) +) +traj = Trajectory(traj_info, traj_data) +env.load_trajectory(traj) + +# 播放完整轨迹 +env.play_trajectory(n_steps_per_episode=len(full_qpos), render=True) \ No newline at end of file