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

v0.1.4

Browse Source
This commit is contained in:
wty-yy
2025-12-01 00:08:01 +08:00
parent c58ac0ab89
commit a91ea5b62e
21 changed files with 299 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -8,3 +8,4 @@ __pycache__/
# Logger
logs/
.aim/

4
UPDATE.md
View File

@@ -1,4 +1,8 @@
# UPDATE
## 20251130
### v0.1.4
1. 加入velocity_goals中的MaxVelocityGoal, 依次执行每种维度上的极值
2. 加入metrics中的dof_limits_metric, 计算关节扭矩超过soft_dof_limit的比例
## 20251128
### v0.1.3
1. 完成go2模型预测

BIN
resources/models/go2/go2_cts_cmd-1,1_38k.pt Normal file
View File

Binary file not shown.

3
robogauge/tasks/gauge/__init__.py
View File

@@ -1,4 +1,3 @@
from .base_gauge import BaseGauge
from .base_gauge_config import BaseGaugeConfig
from .flat.flat_gauge import FlatGauge
from .flat.flat_gauge_config import FlatGaugeConfig
from .gauge_configs.flat_gauge_config import FlatGaugeConfig

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

@@ -7,30 +7,82 @@
@Blog : https://wty-yy.github.io/
@Desc : Base Gauge for Robogauge
'''
from robogauge.tasks.robots.base_robot_config import RobotConfig
from typing import List
from functools import partial
from robogauge.utils.logger import logger
from robogauge.utils.helpers import class_to_dict
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal, PositionGoal
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.utils.logger import logger
from robogauge.tasks.gauge.goals import BaseGoal, MaxVelocityGoal
from robogauge.tasks.gauge.metrics import dof_limits_metric
class BaseGauge:
def __init__(self, cfg: BaseGaugeConfig):
def __init__(self, cfg: BaseGaugeConfig, robot_cfg: RobotConfig):
self.cfg = cfg
self.goals_cfg = class_to_dict(self.cfg.goals)
self.metrics_cfg = class_to_dict(self.cfg.metrics)
self.goal_str = ""
self.goal_idx = 0
self.goals: List[BaseGoal] = []
self.metrics: List[function] = []
self.info = {
'goal': [],
'metric': [],
}
log_str = "Initialized Gauge with Goals:\n"
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))
log_str += f" - Max Velocity Goal: {kwargs}\n"
else:
raise NotImplementedError(f"Goal '{name}' is not implemented in BaseGauge.")
self.info['goal'].append(name)
for name, enabled in self.metrics_cfg.items():
if not enabled: continue
metric_func = eval(f"{name}_metric")
self.metrics.append(partial(metric_func, robot_cfg=robot_cfg, **self.metrics_cfg[name]))
log_str += f" - Metric: {name}\n"
self.info['metric'].append(name)
logger.info(log_str.strip())
def is_reset(self) -> bool:
return False
def is_reset(self, sim_data: SimData) -> bool:
if self.goal_idx >= len(self.goals):
return False
return self.goals[self.goal_idx].is_reset(sim_data)
def is_done(self) -> bool:
if self.goal_idx >= len(self.goals):
return True
return False
def get_goal(self) -> GoalData:
goal = GoalData(
goal_type='velocity',
velocity_goal=VelocityGoal(
lin_vel=[5.0, 0.0, 0.0],
ang_vel=[0.0, 0.0, 0.0]
)
)
def get_goal(self, sim_data: SimData) -> GoalData:
# goal = GoalData(
# goal_type='velocity',
# velocity_goal=VelocityGoal(
# ang_vel_yaw=-5.0,
# )
# )
if self.goal_idx >= len(self.goals):
logger.error("All goals have been exhausted.")
return None
goal_instance = self.goals[self.goal_idx]
goal = goal_instance.get_goal(sim_data)
if goal is None:
self.goal_idx += 1
return None
now_goal_str = str(goal_instance)
if now_goal_str != self.goal_str:
self.goal_str = now_goal_str
logger.info(f"New Goal [{self.goal_idx+1}/{len(self.goals)}] [{goal_instance.count+1}/{goal_instance.total}]: {self.goal_str}")
return goal
def update_metrics(self, sim_data: SimData):
@@ -38,4 +90,6 @@ class BaseGauge:
return
for i in range(len(sim_data.proprio.joint.force)):
logger.log(sim_data.proprio.joint.force[i], f'dof/force_{i}', step=sim_data.n_step)
for metric_func in self.metrics:
metric_func(sim_data)

9
robogauge/tasks/gauge/base_gauge_config.py
View File

@@ -15,9 +15,16 @@ class BaseGaugeConfig(Config):
class assets:
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_spawn_xy = [0, 0] # x y [m]
class goals:
class max_velocity: # goal with maximum velocity
enabled = True
cmd_duration = 3.0 # duration for each velocity command [s]
class metrics:
dof_limits = True
class dof_limits:
enabled = True
soft_dof_limit_ratio = 0.9
class commands:
stance = True

13
robogauge/tasks/gauge/flat/flat_gauge.py
View File

@@ -1,13 +0,0 @@
# -*- coding: utf-8 -*-
'''
@File : flat_gauge.py
@Time : 2025/11/27 16:03:11
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Flat Gauge Implementation
'''
from robogauge.tasks.gauge.base_gauge import BaseGauge
class FlatGauge(BaseGauge):
...

13
robogauge/tasks/gauge/flat/flat_gauge_config.py
View File

@@ -1,13 +0,0 @@
# -*- coding: utf-8 -*-
'''
@File : flat_gauge_config.py
@Time : 2025/11/27 16:03:02
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Flat Gauge Configuration
'''
from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
class FlatGaugeConfig(BaseGaugeConfig):
...

30
robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py Normal file
View File

@@ -0,0 +1,30 @@
# -*- coding: utf-8 -*-
'''
@File : flat_gauge_config.py
@Time : 2025/11/27 16:03:02
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Flat Gauge Configuration
'''
from robogauge.tasks.gauge.base_gauge_config import BaseGaugeConfig
class FlatGaugeConfig(BaseGaugeConfig):
gauge_class = 'BaseGauge'
class assets:
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
terrain_spawn_xy = [0, 0] # x y [m]
class goals:
max_velocity = True # goal with maximum velocity
class metrics:
dof_limits = True
class commands:
stance = True
max_lin_vel = True
diagonal_lin_vel = True

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

@@ -4,8 +4,19 @@ from typing import List, Optional, Literal
@dataclass
class VelocityGoal:
lin_vel: List[float] # x, y, z [m/s], z is ignored for ground robots
ang_vel: List[float] # roll, pitch, yaw [rad/s], roll and pitch are ignored for ground robots
lin_vel_x: float = 0.0 # [m/s]
lin_vel_y: float = 0.0 # [m/s]
lin_vel_z: float = 0.0 # [m/s]
ang_vel_roll: float = 0.0 # [rad/s]
ang_vel_pitch: float = 0.0 # [rad/s]
ang_vel_yaw: float = 0.0 # [rad/s]
def __repr__(self):
s = ""
for field in self.__dataclass_fields__:
if getattr(self, field) != 0.0:
s += f"{field}={getattr(self, field):.1f}, "
return s[:-2] if s else "stance"
@dataclass
class PositionGoal:

2
robogauge/tasks/gauge/goals/__init__.py Normal file
View File

@@ -0,0 +1,2 @@
from robogauge.tasks.gauge.goals.base_goal import BaseGoal
from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal

25
robogauge/tasks/gauge/goals/base_goal.py Normal file
View File

@@ -0,0 +1,25 @@
# -*- coding: utf-8 -*-
'''
@File : base_goals.py
@Time : 2025/11/30 21:52:59
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Base Goal Class
'''
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData
class BaseGoal:
count = 0
total = 0
def is_done(self) -> bool:
raise NotImplementedError
def is_reset(self, sim_data: SimData) -> bool:
raise NotImplementedError
def get_goal(self, sim_data: SimData) -> GoalData:
raise NotImplementedError

57
robogauge/tasks/gauge/goals/velocity_goals.py Normal file
View File

@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
'''
@File : velocity_goals.py
@Time : 2025/11/30 21:44:13
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Velocity Goals Implementation
'''
from typing import Optional
from robogauge.tasks.gauge.goals import BaseGoal
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData, VelocityGoal
from robogauge.utils.helpers import class_to_dict
from robogauge.utils.logger import logger
class MaxVelocityGoal(BaseGoal):
""" Goal class for maximizing velocity commands. """
def __init__(self, max_velocity: RobotConfig.commands, cmd_duration: float = 5, **kwargs):
kwargs.pop('enabled', None)
if kwargs:
logger.warning(f"Unused kwargs in MaxVelocityGoal: {kwargs}")
self.max_velocity = class_to_dict(max_velocity)
self.cmd_duration = cmd_duration
self.last_reset_time = 0.0
self.goals = []
for key, min_max in self.max_velocity.items():
if min_max is None: continue
for value in min_max:
if value != 0:
self.goals.append(VelocityGoal(**{key: value}))
self.count = 0
self.total = len(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
return True
return False
def get_goal(self, sim_data: SimData) -> Optional[GoalData]:
self.count = int(sim_data.sim_time / self.cmd_duration)
if self.count >= self.total:
return None
self.current_goal = self.goals[self.count]
return GoalData(
goal_type='velocity',
velocity_goal=self.current_goal
)
def __repr__(self):
return f"{self.current_goal}"

43
robogauge/tasks/gauge/metrics/__init__.py Normal file
View File

@@ -0,0 +1,43 @@
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.utils.logger import logger
def example_metric(
sim_data: SimData,
robot_cfg: RobotConfig,
**kwargs
) -> float:
""" An example metric function. """
value = 0.0
# Compute some metric value based on sim_data and robot_cfg
logger.log(value, 'example_metric', step=sim_data.n_step)
return value
def dof_limits_metric(
sim_data: SimData,
robot_cfg: RobotConfig,
soft_dof_limit_ratio: float = 0.9,
**kwargs
) -> float:
""" Metric to log DOF limit violations. """
mean_value = 0.0
for i in range(len(sim_data.proprio.joint.limits)):
lower_limit = sim_data.proprio.joint.limits[i, 0]
upper_limit = sim_data.proprio.joint.limits[i, 1]
dof_range = upper_limit - lower_limit
soft_lower_limit = lower_limit + soft_dof_limit_ratio * dof_range
soft_upper_limit = upper_limit - soft_dof_limit_ratio * dof_range
pos = sim_data.proprio.joint.pos[i]
value = 0
if pos < soft_lower_limit:
value = soft_lower_limit - pos
elif pos > soft_upper_limit:
value = pos - soft_upper_limit
value /= dof_range # Normalize by DOF range
logger.log(value, f'dof_limits/{i}', step=sim_data.n_step)
mean_value += value
mean_value /= len(sim_data.proprio.joint.limits)
logger.log(mean_value, f'dof_limits/mean', step=sim_data.n_step)
return mean_value

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

@@ -27,7 +27,7 @@ class BasePipeline:
self.sim: MujocoSimulator = eval(simulator_cfg.simulator_class)(simulator_cfg)
self.robot: BaseRobot = eval(robot_cfg.robot_class)(robot_cfg)
self.gauge: BaseGauge = eval(gauge_cfg.gauge_class)(gauge_cfg)
self.gauge: BaseGauge = eval(gauge_cfg.gauge_class)(gauge_cfg, robot_cfg)
def load(self):
logger.create_tensorboard(self.run_name)
@@ -47,14 +47,16 @@ class BasePipeline:
logger.info(f"Sim FPS: {1.0 / self.simulator_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():
goal = self.gauge.get_goal()
goal = self.gauge.get_goal(sim_data)
if goal is None:
continue
obs = self.robot.build_observation(sim_data, goal)
action, p_gains, d_gains, control_type = self.robot.get_action(obs)
self.sim.setup_action(action, p_gains, d_gains, control_type)
for _ in range(frame_skip):
sim_data = self.sim.step()
self.gauge.update_metrics(sim_data)
if self.gauge.is_reset():
if self.gauge.is_reset(sim_data):
self.sim.reset()
sim_data = self.sim.step()
finally:

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

@@ -7,7 +7,8 @@
@Blog : https://wty-yy.github.io/
@Desc : Base Robot Configuration
'''
from typing_extensions import Literal
from dataclasses import dataclass
from typing import Optional, List
from robogauge.utils.config import Config
class RobotConfig(Config):
@@ -30,7 +31,6 @@ class RobotConfig(Config):
num_observations = 45
num_actions = 12
max_velocity_cmd = [1.5, 1.0, 2.0]
default_dof_pos = [0.1, 0.8, -1.5, -0.1, 0.8, -1.5,
0.1, 1.0, -1.5, -0.1, 1.0, -1.5]
@@ -47,5 +47,7 @@ class RobotConfig(Config):
class commands:
lin_vel_x = [-1, 1] # min max [m/s]
lin_vel_y = [-1, 1] # min max [m/s]
lin_vel_z = None # min max [m/s]
ang_vel_roll = None # min max [rad/s]
ang_vel_pitch = None # min max [rad/s]
ang_vel_yaw = [-1, 1] # min max [rad/s]

9
robogauge/tasks/robots/go2/go2.py
View File

@@ -19,7 +19,7 @@ from robogauge.utils.logger import logger
class Go2(BaseRobot):
def __init__(self, cfg: Go2Config):
super().__init__(cfg)
self.max_velocity_cmd = np.array(cfg.control.max_velocity_cmd, dtype=np.float32)
self.cmd_range = cfg.commands
self.default_dof_pos = np.array(cfg.control.default_dof_pos, dtype=np.float32)
self.last_action = np.zeros(self.num_action, dtype=np.float32)
self.action_scale = cfg.control.scales.action
@@ -35,8 +35,11 @@ class Go2(BaseRobot):
dof_pos = (sim_proprio.joint.pos - self.default_dof_pos) * self.cfg.control.scales.dof_pos
dof_vel = sim_proprio.joint.vel * self.cfg.control.scales.dof_vel
cmd = np.array(goal_data.velocity_goal.lin_vel[:2] + goal_data.velocity_goal.ang_vel[2:3], np.float32)
cmd = np.minimum(np.maximum(cmd, -self.max_velocity_cmd), self.max_velocity_cmd)
goal = goal_data.velocity_goal
cmd = np.array([goal.lin_vel_x, goal.lin_vel_y, goal.ang_vel_yaw], dtype=np.float32)
cmd = np.minimum(np.maximum(cmd, np.array([self.cmd_range.lin_vel_x[0], self.cmd_range.lin_vel_y[0], self.cmd_range.ang_vel_yaw[0]], dtype=np.float32)),
np.array([self.cmd_range.lin_vel_x[1], self.cmd_range.lin_vel_y[1], self.cmd_range.ang_vel_yaw[1]], dtype=np.float32))
cmd *= self.cfg.control.scales.cmd
obs[:3] = ang_vel

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

@@ -20,6 +20,7 @@ class Go2Config(RobotConfig):
class control(RobotConfig.control):
device = 'cpu'
torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_83501.pt"
# torch_script_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
@@ -30,7 +31,6 @@ class Go2Config(RobotConfig):
num_observations = 45
num_actions = 12
max_velocity_cmd = [1.5, 1.0, 2.0]
default_dof_pos = [0.1, 0.8, -1.5, -0.1, 0.8, -1.5,
0.1, 1.0, -1.5, -0.1, 1.0, -1.5]
@@ -45,6 +45,10 @@ class Go2Config(RobotConfig):
cmd = [2.0, 2.0, 0.25]
class commands(RobotConfig.commands):
lin_vel_x = [-1, 1] # min max [m/s]
lin_vel_x = [-1.5, 1.5] # min max [m/s]
lin_vel_y = [-1, 1] # min max [m/s]
ang_vel_yaw = [-1, 1] # min max [rad/s]
lin_vel_z = None # min max [m/s]
ang_vel_roll = None # min max [rad/s]
ang_vel_pitch = None # min max [rad/s]
ang_vel_yaw = [-2, 2] # min max [rad/s]

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

@@ -117,6 +117,7 @@ class MujocoSimulator:
self._pause = False
self.n_step = 0
self.sim_time = 0.0
self.load_dof_limits()
self.preload_sensors()
@@ -159,6 +160,7 @@ class MujocoSimulator:
pos=self.get_sensor_data('joint_pos'),
vel=self.get_sensor_data('joint_vel'),
force=self.get_sensor_data('joint_eff'),
limits=self.dof_limits,
),
imu=IMUState(
pos=self.get_sensor_data('imu_pos'),
@@ -184,6 +186,7 @@ class MujocoSimulator:
sim_data = SimData(
n_step=self.n_step,
sim_dt=self.sim_dt,
sim_time=self.sim_time,
proprio=proprio
)
@@ -248,7 +251,9 @@ class MujocoSimulator:
self.imu_lin_vel = self.find_sensors(tag_name="framelinvel")
actuator_names = [mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(self.mj_model.nu)]
logger.info(
f"\n{'='*20} XML SENSOR NAMES {'='*20}\n"
f"""\nRobot XML: {self.robot_xml}\n"""
f"""Robot joint names: {[x.rsplit('/')[-1] for x in self.dof_names]}\n"""
f"""{'='*20} XML SENSOR NAMES {'='*20}\n"""
f"""Joint Position Sensors [{len(self.joint_pos_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_pos_sensor_names]}\n"""
f"""Joint Velocity Sensors [{len(self.joint_vel_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_vel_sensor_names]}\n"""
f"""Joint Effort Sensors [{len(self.joint_eff_sensor_names)}]: {[x.rsplit('/')[-1] for x in self.joint_eff_sensor_names]}\n"""
@@ -351,3 +356,16 @@ class MujocoSimulator:
logger.info(f" imu.acc: { _shape(imu.acc) }")
logger.info(f" imu.pos: { _shape(imu.pos) }")
logger.info(f" imu.lin_vel: { _shape(imu.lin_vel) }")
def load_dof_limits(self):
self.dof_limits = []
self.dof_names = []
for i in range(self.mj_model.njnt):
name = mujoco.mj_id2name(self.mj_model, mujoco.mjtObj.mjOBJ_JOINT, i)
jnt_type = self.mj_model.jnt_type[i]
if jnt_type == mujoco.mjtJoint.mjJNT_FREE:
continue
limits = self.mj_model.jnt_range[i]
self.dof_limits.append(limits)
self.dof_names.append(name)
self.dof_limits = np.array(self.dof_limits, np.float32)

8
robogauge/tasks/simulator/sim_data.py
View File

@@ -3,9 +3,10 @@ from dataclasses import dataclass
@dataclass
class JointState:
pos: np.ndarray
vel: np.ndarray
force: np.ndarray
pos: np.ndarray # [rad] shape (n_dof,)
vel: np.ndarray # [rad/s] shape (n_dof,)
force: np.ndarray # [N*m] shape (n_dof,)
limits: np.ndarray # [rad] shape (n_dof, 2), lower and upper limits
@dataclass
class BaseState:
@@ -32,4 +33,5 @@ class RobotProprioception:
class SimData:
n_step: int
sim_dt: float
sim_time: float
proprio: RobotProprioception

1
robogauge/utils/helpers.py
View File

@@ -20,6 +20,7 @@ def parse_path(path):
return path
def class_to_dict(obj) -> dict:
# From https://github.com/leggedrobotics/legged_gym/blob/master/legged_gym/utils/helpers.py
if not hasattr(obj, "__dict__"):
return obj
result = {}