cyy/RoboGauge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v0.1.9

Browse Source
This commit is contained in:
wty-yy
2025-12-18 13:50:23 +08:00
parent 9d509829a5
commit 1e1a04b4c0
18 changed files with 195 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
CMD.md Normal file
View File

@@ -0,0 +1,27 @@
# Single Run
```bash
python robogauge/scripts/run.py \
--task go2_moe_flat \
--model-path /home/xfy/Coding/robot_gauge/mytest/models/kaiwu/kaiwu_script_v6-2_124004.pt \
--experiment-name debug \
--headless
```
# Multi Run
```bash
python robogauge/scripts/run.py \
--task go2_moe_flat \
--model-path /home/xfy/Coding/robot_gauge/mytest/models/kaiwu/kaiwu_script_v6-2_124004.pt \
--experiment-name debug \
--multi \
--num-processes 1 \
--headless
python robogauge/scripts/run.py \
--task go2_moe_flat \
--model-path /home/xfy/Coding/robot_gauge/mytest/models/kaiwu/kaiwu_script_v6-2_102003.pt \
--experiment-name debug \
--multi \
--num-processes 1 \
--headless
```

13
README.md
View File

@@ -45,7 +45,7 @@
| 5 | `dof_power` | 电机耗能 | NA | 10 | `1-x` |
### 速度追踪目标
针对在虚实迁移中发现的问题, 整理指标内容如下:
针对在虚实迁移中发现的问题, 整理指标 (metrics) 内容如下:
| # | 描述 | 标准化范围 | 对应真机问题 | 地形 |
| - | - | - | - | - |
@@ -55,16 +55,11 @@
| 4 | 高速移动/速度对角突变base高度变化 | 固定高度 | 高速移动时机身存在趴低问题, 速度发生对角突变时无法平衡 | 平地 |
| 5 | 高速移动急停稳定性 | 固定用时 | 楼梯上静止时, 关节不稳定 | Any |
总结速度最总目标如下:
总结速度最总目标 (goals) 如下:
| # | 目标名称 Goals | 描述 | reset条件 | 最大reset次数 |
| 1 | `max_velocity` | 单一维度的最大线/角速度 | 每次执行一个维度的指令 | 6 |
| 2 | `diagonal_velocity` | 对角线速度变化 | 每次执行一对对角指令 | 6 |
| 3 | `move_stance` | 全线速度移动急停 | 每次执行一个方向的指令, 再急停 | 6 |
1. `max_velocity`: 单一维度的最大线/角速度, 每次reset只执行单一指令
2. `digonal_velocity`: 对角线速度变化, 每次reset执行一对指令, 总reset
3. `move_stance`:
| 1 | `max_velocity` | 单一维度的最大线/角速度 | 每次执行一个方向的指令, 再急停 | 6 |
| 2 | `diagonal_velocity` | 对角线速度变化 | 每次执行一对对角指令 | 8 |
## 创建新任务
评测任务注册在[`tasks/__init__.py`](./robogauge/tasks/__init__.py)中完成, 包含四个部分:

6
UPDATE.md
View File

@@ -1,4 +1,10 @@
# UPDATE
## 20251218
### v0.1.9
1. 在`MaxVelocityGoal`基础上加入`end_stance`, 最终保持站立姿态
2. 在开始goal控制前, 先等机器人落地, 通过线速度小于0.05阈值判断静止后, 执行goal
3. 支持1920x1080录像保存
Fix Bugs: 修复Video frame skip过大问题
## 20251206
### v0.1.8
1. 加入run_eval_models.sh多模型评估bash脚本

1
resources/terrains/flat.xml
View File

@@ -5,6 +5,7 @@
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="-130" elevation="-20"/>
<global offwidth="1920" offheight="1080"/>
</visual>
<asset>

4
robogauge/tasks/__init__.py
View File

@@ -4,8 +4,8 @@ from robogauge.tasks.robots import RobotConfig, Go2Config, Go2MoEConfig
from robogauge.tasks.pipeline import BasePipeline
from robogauge.tasks.gauge import BaseGaugeConfig
from robogauge.tasks.custom.go2_flat_task import Go2FlatGaugeConfig, Go2FlatConfig, Go2MoEFlatConfig
from robogauge.tasks.custom.go2_flat_task import Go2FlatGaugeConfig, Go2FlatConfig, Go2MoEFlatConfig, Go2MoEFlatMujocoConfig
task_register.register('base', BasePipeline, MujocoConfig, BaseGaugeConfig, RobotConfig)
task_register.register('go2_flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go2FlatConfig)
task_register.register('go2_moe_flat', BasePipeline, MujocoConfig, Go2FlatGaugeConfig, Go2MoEFlatConfig)
task_register.register('go2_moe_flat', BasePipeline, Go2MoEFlatMujocoConfig, Go2FlatGaugeConfig, Go2MoEFlatConfig)

12
robogauge/tasks/custom/go2_flat_task.py
View File

@@ -1,5 +1,6 @@
from robogauge.tasks.robots import Go2Config, Go2MoEConfig
from robogauge.tasks.gauge import FlatGaugeConfig
from robogauge.tasks.simulator.mujoco_config import MujocoConfig
class Go2FlatGaugeConfig(FlatGaugeConfig):
class metrics(FlatGaugeConfig.metrics):
@@ -11,10 +12,12 @@ class Go2FlatGaugeConfig(FlatGaugeConfig):
class goals(FlatGaugeConfig.goals):
class max_velocity(FlatGaugeConfig.goals.max_velocity):
enabled = True
cmd_duration = 5.0
move_duration = 5.0
end_stance = True
stance_duration = 2.0
class diagonal_velocity(FlatGaugeConfig.goals.diagonal_velocity):
enabled = False
enabled = True
cmd_duration = 6.0
class Go2FlatConfig(Go2Config):
@@ -32,3 +35,8 @@ class Go2MoEFlatConfig(Go2MoEConfig):
class control(Go2Config.control):
# model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_moe_cts_124k.pt"
model_path = "/home/xfy/Coding/kaiwu2025/rob_finals/sim2real/models/v6-2_106503/kaiwu_script_v6-2_106503.pt"
class Go2MoEFlatMujocoConfig(MujocoConfig):
class domain_rand(MujocoConfig.domain_rand):
base_mass = 0.0
friction = 1.0

4
robogauge/tasks/gauge/base_gauge.py
View File

@@ -43,10 +43,10 @@ class BaseGauge:
for name, kwargs in self.goals_cfg.items():
if not kwargs['enabled']: continue
if name == 'max_velocity':
self.goals.append(MaxVelocityGoal(robot_cfg.commands, **kwargs))
self.goals.append(MaxVelocityGoal(robot_cfg.control.control_dt, robot_cfg.commands, **kwargs))
log_str += f" - Max Velocity Goal: {kwargs}\n"
elif name == 'diagonal_velocity':
self.goals.append(DiagonalVelocityGoal(robot_cfg.commands, **kwargs))
self.goals.append(DiagonalVelocityGoal(robot_cfg.control.control_dt, robot_cfg.commands, **kwargs))
log_str += f" - Diagonal Velocity Goal: {kwargs}\n"
else:
raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")

4
robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py
View File

@@ -19,7 +19,9 @@ class FlatGaugeConfig(BaseGaugeConfig):
class goals:
class max_velocity: # goal with maximum velocity
enabled = True
cmd_duration = 5.0 # [s] duration for each velocity command
move_duration = 5.0 # [s] duration for each velocity command
end_stance = True # whether to end with zero velocity command
standce_duration = 2.0 # [s] duration for the ending stance command
class diagonal_velocity: # goal with diagonal velocity changes
enabled = True

8
robogauge/tasks/gauge/goal_data.py
View File

@@ -1,6 +1,6 @@
from enum import Enum
from dataclasses import dataclass
from typing import List, Optional, Literal
from typing import List, Optional, Literal, Tuple
@dataclass
class VelocityGoal:
@@ -31,10 +31,10 @@ class VelocityGoal:
@dataclass
class PositionGoal:
# relative to robot's current position
target_pos: List[float] # x, y, z [m], z is ignored for ground robots
target_pos: Tuple[float, float, float] = (0.0, 0.0, 0.0) # x, y, z [m], z is ignored for ground robots
# reach target orientation
target_quat: List[float] # x, y, z, w quaternion
tolerance: float # [m] position tolerance to consider goal reached
target_quat: Tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) # x, y, z, w quaternion
tolerance: float = 0.01 # [m] position tolerance to consider goal reached
@dataclass
class GoalData:

70
robogauge/tasks/gauge/goals/velocity_goals.py
View File

@@ -20,38 +20,47 @@ from robogauge.utils.logger import logger
class BaseVelocityGoal(BaseGoal):
name = "base_velocity_goal"
def __init__(self, cmd_duration: float = 5, **kwargs):
def __init__(self, control_dt: float, cmd_duration: float = 5, **kwargs):
super().__init__()
self.control_dt = control_dt
self.cmd_duration = cmd_duration
self.goal_start_time = None
self.goal_runtime = 0.0
self.first_goal_after_reset = True
self.last_reset_time = 0.0
self.goals = []
def is_reset(self, sim_data: SimData) -> bool:
if sim_data.sim_time - self.last_reset_time >= self.cmd_duration:
self.last_reset_time = sim_data.sim_time
self.first_goal_after_reset = True
return True
return False
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
if self.goal_start_time is None:
self.goal_start_time = sim_data.sim_time
self.count = int((sim_data.sim_time - self.goal_start_time) / self.cmd_duration)
if self.count >= self.total:
return None
self.current_goal = self.goals[self.count]
self.sub_name = str(self.current_goal)
return GoalData(
goal_type='velocity',
velocity_goal=self.current_goal
)
def update_runtime_count(self, sim_data: SimData):
if self.first_goal_after_reset:
self.last_reset_time = sim_data.sim_time # update last reset time (stance after reset)
self.first_goal_after_reset = False
self.goal_runtime += self.control_dt
self.count = int(self.goal_runtime / self.cmd_duration)
class MaxVelocityGoal(BaseVelocityGoal):
name = "max_velocity"
""" Goal class for maximizing velocity commands. """
def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
super().__init__(cmd_duration=cmd_duration)
def __init__(self,
control_dt: float,
max_velocity: RobotConfig.commands,
move_duration: float = 5,
end_stance: bool = True,
stance_duration: float = 2.0,
**kwargs
):
cmd_duration = move_duration + (stance_duration if end_stance else 0)
super().__init__(control_dt=control_dt, cmd_duration=cmd_duration)
self.move_duration = move_duration
self.end_stance = end_stance
self.stance_duration = stance_duration
kwargs.pop('enabled', None)
if kwargs:
logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
@@ -72,16 +81,35 @@ class MaxVelocityGoal(BaseVelocityGoal):
return
self.sub_name = str(self.goals[0])
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
self.update_runtime_count(sim_data)
if self.count >= self.total:
return None
self.current_goal = self.goals[self.count]
if self.end_stance and self.goal_runtime - self.count * self.cmd_duration >= self.move_duration:
self.current_goal = VelocityGoal() # zero velocity
self.sub_name = str(self.current_goal)
return GoalData(
goal_type='velocity',
velocity_goal=self.current_goal
)
class DiagonalVelocityGoal(BaseVelocityGoal):
name = "diagonal_velocity"
def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 6, **kwargs):
def __init__(self,
control_dt: float,
max_velocity: RobotConfig.commands,
cmd_duration: float = 6,
**kwargs
):
""" Goal class for diagonal velocity changes.
Args:
control_dt (float): Control timestep.
max_velocity (RobotConfig.commands): Maximum velocity commands.
cmd_duration (float, optional): Duration for a pair of diagonal commands.
"""
super().__init__(cmd_duration=cmd_duration)
super().__init__(control_dt=control_dt, cmd_duration=cmd_duration)
kwargs.pop('enabled', None)
if kwargs:
logger.warning(f"Unused kwargs in DiagonalVelocityGoal: {kwargs}")
@@ -108,13 +136,11 @@ class DiagonalVelocityGoal(BaseVelocityGoal):
self.sub_name = str(self.goals[0])
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
if self.goal_start_time is None:
self.goal_start_time = sim_data.sim_time
self.count = int((sim_data.sim_time - self.goal_start_time) / self.cmd_duration)
self.update_runtime_count(sim_data)
if self.count >= self.total:
return None
self.current_goal = self.goals[self.count]
if sim_data.sim_time - self.count * self.cmd_duration >= self.cmd_duration / 2:
if self.goal_runtime - self.count * self.cmd_duration >= self.cmd_duration / 2:
self.current_goal = self.current_goal.invert()
self.sub_name = str(self.current_goal)
return GoalData(

22
robogauge/tasks/pipeline/base_pipeline.py
View File

@@ -9,8 +9,9 @@
'''
import yaml
import random
from copy import deepcopy
import numpy as np
from pathlib import Path
from copy import deepcopy
from robogauge.utils.logger import logger
from robogauge.tasks.simulator import MujocoSimulator, MujocoConfig, SimData
@@ -18,6 +19,7 @@ from robogauge.tasks.robots import (
BaseRobot, RobotConfig, Go2Config, Go2, Go2MoEConfig, Go2MoE
)
from robogauge.tasks.gauge import BaseGauge, BaseGaugeConfig
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
from robogauge.utils.helpers import class_to_dict
class BasePipeline:
@@ -36,6 +38,8 @@ class BasePipeline:
self.robot: BaseRobot = eval(robot_cfg.robot_class)(robot_cfg)
self.gauge: BaseGauge = eval(gauge_cfg.gauge_class)(gauge_cfg, robot_cfg)
self.last_reset_time = 0.0
# save configs
cfg = {}
for name in ['sim_cfg', 'robot_cfg', 'gauge_cfg']:
@@ -57,11 +61,25 @@ class BasePipeline:
logger.info(f"🚀 Starting single run: {self.run_name}")
try:
self.load()
first_reset = True
sim_data = self.sim.step()
frame_skip = int(self.robot_cfg.control.control_dt / self.sim_cfg.physics.simulation_dt)
assert frame_skip * self.sim_cfg.physics.simulation_dt == self.robot_cfg.control.control_dt, \
"Control dt must be multiple of simulation dt."
logger.info(f"Sim FPS: {1.0 / self.sim_cfg.physics.simulation_dt:.2f}, Control FPS: {1.0 / self.robot_cfg.control.control_dt:.2f}, Frame Skip: {frame_skip:d}")
logger.info("Running pipeline...")
while not self.gauge.is_done():
if first_reset: # wait for robot to be still
goal_data = GoalData(
goal_type=self.robot_cfg.control.support_goal,
velocity_goal=VelocityGoal(), # zero velocity
position_goal=PositionGoal(), # current position
)
lin_vel = np.linalg.norm(sim_data.proprio.base.lin_vel)
# print(lin_vel, sim_data.sim_time - self.last_reset_time)
if np.linalg.norm(sim_data.proprio.base.lin_vel) < 0.05 and sim_data.sim_time - self.last_reset_time > 0.1:
first_reset = False
else:
goal_data = self.gauge.get_goal(sim_data)
if goal_data is None:
continue
@@ -79,6 +97,8 @@ class BasePipeline:
self.gauge.update_metrics(sim_data, goal_data)
if self.gauge.is_reset(sim_data):
self.sim.reset()
first_reset = True
self.last_reset_time = sim_data.sim_time
sim_data = self.sim.step()
finally:
self.sim.close_viewer()

58
robogauge/tasks/pipeline/multi_pipeline.py
View File

@@ -8,6 +8,7 @@
@Desc : Multiprocessing Pipeline for Robogauge
'''
import yaml
import traceback
import functools
import numpy as np
from tqdm import tqdm
@@ -26,6 +27,8 @@ def run_single_process(args, data):
seed, base_mass, friction = data
local_args = deepcopy(args)
local_args.seed = seed
local_args.friction = friction
local_args.base_mass = base_mass
run_name = f"{local_args.run_name}_{seed}_baseMass{base_mass}_friction{friction}"
logger.create(
experiment_name=local_args.experiment_name,
@@ -33,8 +36,22 @@ def run_single_process(args, data):
console_output=False
)
pipeline = task_register.make_pipeline(args=local_args, create_logger=False)
try:
log_dir = pipeline.run()
return log_dir
ret = {
'status': 'success',
'log_dir': log_dir,
'model_path': pipeline.robot_cfg.control.model_path,
}
except Exception as e:
logger.error(f"❌ Process with seed={seed}, base_mass={base_mass}, friction={friction} failed with error: {e}")
ret = {
'status': 'error',
'data': data,
'error_msg': str(e),
'traceback': traceback.format_exc()
}
return ret
class MultiPipeline:
def __init__(self, args):
@@ -43,26 +60,52 @@ class MultiPipeline:
self.frictions = args.frictions
self.base_masses = args.base_masses
self.num_processes = args.num_processes
self.model_path = None
logger.create(args.experiment_name+'_multi', args.run_name+'_multi')
def run(self):
logger.info(f"🚀 Starting Multi-Process Evaluation with {self.num_processes} processes.")
logger.info(f"🔢 Seeds: {self.seeds}, Frictions: {self.frictions}, Base masses: {self.base_masses}")
process_args = list(product(self.seeds, self.base_masses, self.frictions))
workers_data = list(product(self.seeds, self.base_masses, self.frictions))
ctx = multiprocessing.get_context('spawn')
worker_func = functools.partial(run_single_process, self.args)
result_log_dirs = []
success_flags = []
with ctx.Pool(processes=self.num_processes) as pool:
iterator = pool.imap_unordered(worker_func, process_args)
for log_dir in tqdm(iterator, total=len(process_args), desc="Evaluation"):
result_log_dirs.append(log_dir)
iterator = pool.imap_unordered(worker_func, workers_data)
for results in tqdm(iterator, total=len(workers_data), desc="Evaluation"):
success_flags.append(results['status'] == 'success')
if results['status'] == 'success':
result_log_dirs.append(results['log_dir'])
if self.model_path is None:
self.model_path = results['model_path']
else:
assert self.model_path == results['model_path'], "Model paths do not match across runs."
else:
data = results['data']
logger.error(f"❌ Process with seed={data[0]}, base_mass={data[1]}, friction={data[2]} failed with error: {results['error_msg']}")
logger.info("✅ Multi-Process Evaluation Completed.")
self.aggregate_results(result_log_dirs)
self.aggregate_results(result_log_dirs, success_flags, workers_data)
def aggregate_results(self, log_dirs):
def aggregate_results(self, log_dirs, success_flags, workers_data):
""" Process results.yaml from each log_dir """
logger.info("📊 Aggregating Results from all runs...")
summary = {'model_path': self.model_path, 'success': {}}
finish_msg = (
f"""\n{'='*20} Run Finish Summary {'='*20}\n"""
f"""{'Seed':^10}{'Base Mass':^15}{'Friction':^15}{'Status':^10}\n"""
)
for success, data in zip(success_flags, workers_data):
seed, base_mass, friction = data
status_str = "✅" if success else "❌"
finish_msg += f"{seed:^10}{base_mass:^15}{friction:^15}{status_str:^10}\n"
summary['success'][f"Seed_{seed}_BaseMass_{base_mass}_Friction_{friction}"] = True if success else False
finish_msg += f"""{'='*88}"""
logger.info(finish_msg)
all_results = []
all_yaml_paths = []
for path in log_dirs:
@@ -94,7 +137,6 @@ class MultiPipeline:
for mean_name, mean_value in means.items():
value_collections[metric][mean_name].append(float(mean_value.split(' ')[0]))
summary = {}
for metric, means in value_collections.items():
summary[metric] = {}
for mean_name, values in means.items():

3
robogauge/tasks/robots/base_robot_config.py
View File

@@ -8,7 +8,7 @@
@Desc : Base Robot Configuration
'''
from dataclasses import dataclass
from typing import Optional, List
from typing import Optional, List, Literal
from robogauge.utils.config import Config
class RobotConfig(Config):
@@ -24,6 +24,7 @@ class RobotConfig(Config):
model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_83501.pt"
control_dt = 0.02 # 50 Hz
control_type = 'P' # Position control
support_goal: Literal['velocity', 'position'] = 'velocity'
# Mujoco joint PD gains
p_gains = [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] # [N*m/rad]

1
robogauge/tasks/robots/go2/go2_config.py
View File

@@ -24,6 +24,7 @@ class Go2Config(RobotConfig):
# model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_cmd-1,1_38k.pt"
control_dt = 0.02 # 50 Hz
control_type = 'P' # Position control
support_goal: Literal['velocity', 'position'] = 'velocity'
# Mujoco joint PD gains
p_gains = [20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0] # [N*m/rad]

4
robogauge/tasks/simulator/mujoco_config.py
View File

@@ -25,8 +25,10 @@ class MujocoConfig(Config):
class render:
save_video = False
video_fps = 30
height = 480
width = 640
height = 480
# width = 1920
# height = 1080
class domain_rand:
# With randomization

2
robogauge/tasks/simulator/mujoco_simulator.py
View File

@@ -145,7 +145,7 @@ class MujocoSimulator:
vid_path,
fps=self.cfg.render.video_fps,
)
self.vid_frame_skip = int(1 / (self.cfg.render.video_fps * self.sim_dt * 2))
self.vid_frame_skip = int(1 / (self.cfg.render.video_fps * self.sim_dt))
logger.info(f"Simulation video saved at: {vid_path}")
self.vid_count += 1

4
robogauge/utils/helpers.py
View File

@@ -76,8 +76,8 @@ def parse_args():
{"name": "--multi", "action": "store_true", "default": False, "help": "Enable multiprocessing."},
{"name": "--num-processes", "type": int, "default": 2, "help": "Number of parallel processes."},
{"name": "--seeds", "type": int, "nargs": "+", "default": [0], "help": "List of random seeds for multiple runs."},
{"name": "--base-masses", "type": float, "nargs": "+", "default": [-1, 0, 1], "help": "List of base masses for the model."},
{"name": "--frictions", "type": float, "nargs": "+", "default": [0.5, 1.0, 1.5], "help": "List of friction coefficients for the model."}
{"name": "--base-masses", "type": float, "nargs": "+", "default": [0], "help": "List of base masses for the model."},
{"name": "--frictions", "type": float, "nargs": "+", "default": [0.4, 0.7, 1.0, 1.3, 1.6], "help": "List of friction coefficients for the model."}
]
for param in parameters:
parser.add_argument(param['name'], **{k: v for k, v in param.items() if k != 'name'})

4
robogauge/utils/task_register.py
View File

@@ -63,5 +63,9 @@ class TaskRegister():
sim_cfg.viewer.headless = args.headless
if args.save_video is not None:
sim_cfg.render.save_video = args.save_video
if hasattr(args, 'friction') and args.friction is not None:
sim_cfg.domain_rand.friction = args.friction
if hasattr(args, 'base_mass') and args.base_mass is not None:
sim_cfg.domain_rand.base_mass = args.base_mass
task_register = TaskRegister()