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

v0.1.10

Browse Source
This commit is contained in:
wty-yy
2025-12-18 23:14:18 +08:00
parent 1e1a04b4c0
commit 939dc69b7e
25 changed files with 411 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : go2_flat_task.py
@Time : 2025/12/18 20:19:25
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Go2 Flat Task Configuration
'''
from robogauge.tasks.robots import Go2Config, Go2MoEConfig
from robogauge.tasks.gauge import FlatGaugeConfig
from robogauge.tasks.simulator.mujoco_config import MujocoConfig
@@ -22,15 +31,15 @@ class Go2FlatGaugeConfig(FlatGaugeConfig):
class Go2FlatConfig(Go2Config):
class commands(Go2Config.commands):
lin_vel_x = [-2.0, 2.0] # min max [m/s]
lin_vel_y = [-2.0, 2.0] # min max [m/s]
ang_vel_yaw = [-2.0, 2.0] # min max [rad/s]
lin_vel_x = [-1.8, 1.8] # min max [m/s]
lin_vel_y = [-1.8, 1.8] # min max [m/s]
ang_vel_yaw = [-1.8, 1.8] # min max [rad/s]
class Go2MoEFlatConfig(Go2MoEConfig):
class commands(Go2Config.commands):
lin_vel_x = [-2.0, 2.0] # min max [m/s]
lin_vel_y = [-2.0, 2.0] # min max [m/s]
ang_vel_yaw = [-2.0, 2.0] # min max [rad/s]
lin_vel_x = [-1.8, 1.8] # min max [m/s]
lin_vel_y = [-1.8, 1.8] # min max [m/s]
ang_vel_yaw = [-1.8, 1.8] # min max [rad/s]
class control(Go2Config.control):
# model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_moe_cts_124k.pt"

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

@@ -26,7 +26,10 @@ from robogauge.tasks.gauge.goals import BaseGoal, MaxVelocityGoal, DiagonalVeloc
from robogauge.tasks.gauge.metrics import *
class BaseGauge:
def __init__(self, cfg: BaseGaugeConfig, robot_cfg: RobotConfig):
def __init__(self,
cfg: BaseGaugeConfig,
robot_cfg: RobotConfig,
):
self.cfg = cfg
self.robot_cfg = robot_cfg
self.goals_cfg = class_to_dict(self.cfg.goals)
@@ -79,7 +82,8 @@ class BaseGauge:
def create_new_goal_logger(self):
""" Create a new logger for new goal to metrics. """
if self.goal_idx >= len(self.goals): return
if self.goal_idx >= len(self.goals) or not self.cfg.write_tensorboard:
return
logger.create_tensorboard(
self.robot_cfg.robot_name,
Path(self.robot_cfg.control.model_path).stem,

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

@@ -11,6 +11,7 @@ from robogauge.utils.config import Config
class BaseGaugeConfig(Config):
gauge_class = 'BaseGauge'
write_tensorboard = False # Whether to write tensorboard logs
class assets:
terrain_xml = '{ROBOGAUGE_ROOT_DIR}/resources/terrains/flat.xml'
@@ -34,7 +35,7 @@ class BaseGaugeConfig(Config):
class visualization:
enabled = True
dof_force = True
dof_torque = True
dof_pos = True
class lin_vel_err:
@@ -42,3 +43,14 @@ class BaseGaugeConfig(Config):
class ang_vel_err:
enabled = True
class dof_power:
enabled = True
scaling_factor = 100.0 # [W] scaling factor for power metric
class orientation_stability:
enabled = True
class torque_smoothness:
enabled = True
scaling_factor = 30.0 # [Nm] scaling factor for torque smoothness metric

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

@@ -36,7 +36,7 @@ class FlatGaugeConfig(BaseGaugeConfig):
class visualization:
enabled = True
dof_force = True
dof_torque = True
dof_pos = True
class lin_vel_err:

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

@@ -1,4 +1,5 @@
from .base_metric import BaseMetric
from .dof_metrics import DofLimitsMetric
from .dof_metrics import DofLimitsMetric, DofPowerMetric
from .visualization import VisualizationMetric
from .vel_metrics import LinVelErrMetric, AngVelErrMetric
from .stable_metric import OrientationStabilityMetric, TorqueSmoothnessMetric

12
robogauge/tasks/gauge/metrics/base_metric.py
View File

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : base_metric.py
@Time : 2025/12/18 20:18:45
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Base Metric Implementation
'''
from robogauge.utils.logger import logger
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
@@ -9,6 +18,9 @@ class BaseMetric:
def __init__(self, robot_cfg: RobotConfig, **kwargs):
self.robot_cfg = robot_cfg
def reset(self):
pass
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
value = 0.0

35
robogauge/tasks/gauge/metrics/dof_metrics.py
View File

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : dof_metrics.py
@Time : 2025/12/18 20:18:24
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : DOF Limits Metric Implementation
'''
import numpy as np
from robogauge.tasks.robots import RobotConfig
@@ -47,3 +56,29 @@ class DofLimitsMetric(BaseMetric):
rms_value = 1 - np.sqrt(np.mean(np.square(values)))
logger.log(1 - rms_value, f'dof_limits/rms', step=sim_data.n_step)
return rms_value
class DofPowerMetric(BaseMetric):
""" Metric to log DOF power efficiency. """
name = 'dof_power_metric'
def __init__(self,
robot_cfg: RobotConfig,
scaling_factor: float = 100.0,
**kwargs
):
super().__init__(robot_cfg)
self.scaling_factor = scaling_factor
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
values = []
for i in range(len(sim_data.proprio.joint.torque)):
torque = sim_data.proprio.joint.torque[i]
velocity = sim_data.proprio.joint.vel[i]
power = abs(torque * velocity)
values.append(power)
dof_name = sim_data.proprio.joint.names[i]
logger.log(power, f'dof_power/{dof_name}', step=sim_data.n_step)
rms_power = np.sqrt(np.mean(np.square(values)))
metric_power = 1 - rms_power / self.scaling_factor
logger.log(rms_power, f'dof_power/rms', step=sim_data.n_step)
return metric_power

61
robogauge/tasks/gauge/metrics/stable_metric.py Normal file
View File

@@ -0,0 +1,61 @@
# -*- coding: utf-8 -*-
'''
@File : height_metric.py
@Time : 2025/12/18 20:18:33
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Orientation Stability, Torque Smoothness Metric Implementation
'''
import numpy as np
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.gauge.metrics.base_metric import BaseMetric, GoalData, SimData
from robogauge.utils.math_utils import get_projected_gravity
from robogauge.utils.logger import logger
class OrientationStabilityMetric(BaseMetric):
""" Metric to log height stability. """
name = 'height_std_metric'
def __init__(self,
robot_cfg: RobotConfig,
**kwargs
):
super().__init__(robot_cfg)
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
projected_gravity = get_projected_gravity(sim_data.proprio.base.quat)
projected_x = projected_gravity[0]
metric_value = 1 - abs(projected_x) # consider roll only
logger.log(abs(projected_x), f'stable_metric/projected_x_abs', step=sim_data.n_step)
return metric_value
class TorqueSmoothnessMetric(BaseMetric):
""" Metric to log torque smoothness. """
name = 'torque_smoothness_metric'
def __init__(self,
robot_cfg: RobotConfig,
scaling_factor: float = 30.0,
**kwargs
):
super().__init__(robot_cfg)
self.last_torque = None
self.scaling_factor = scaling_factor
def reset(self):
self.last_torque = None
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
current_torque = np.array(sim_data.proprio.joint.torque, np.float32)
if self.last_torque is None:
self.last_torque = current_torque
return 1.0 # No change at first step
torque_diff = current_torque - self.last_torque
rms_value = np.sqrt(np.mean(np.square(torque_diff)))
metric_value = 1.0 - rms_value / self.scaling_factor
logger.log(rms_value, f'stable_metric/torque_rms_diff', step=sim_data.n_step)
return metric_value

9
robogauge/tasks/gauge/metrics/vel_metrics.py
View File

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : vel_metrics.py
@Time : 2025/12/18 20:18:56
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Velocity Metrics Implementation
'''
import numpy as np
from robogauge.tasks.robots import RobotConfig

21
robogauge/tasks/gauge/metrics/visualization.py
View File

@@ -1,3 +1,12 @@
# -*- coding: utf-8 -*-
'''
@File : visualization.py
@Time : 2025/12/18 20:19:09
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : Visualization Metric Implementation
'''
from robogauge.tasks.robots import RobotConfig
from robogauge.tasks.gauge.metrics.base_metric import BaseMetric, SimData, GoalData
@@ -9,20 +18,20 @@ class VisualizationMetric(BaseMetric):
def __init__(self,
robot_cfg: RobotConfig,
dof_force: bool = False,
dof_torque: bool = False,
dof_pos: bool = False,
**kwargs
):
super().__init__(robot_cfg)
self.dof_force = dof_force
self.dof_torque = dof_torque
self.dof_pos = dof_pos
def __call__(self, sim_data: SimData, goal_data: GoalData) -> float:
for i in range(len(sim_data.proprio.joint.force)):
for i in range(len(sim_data.proprio.joint.torque)):
name = sim_data.proprio.joint.names[i]
if self.dof_force:
force = sim_data.proprio.joint.force[i]
logger.log(force, f'dof_force/{name}', step=sim_data.n_step)
if self.dof_torque:
torque = sim_data.proprio.joint.torque[i]
logger.log(torque, f'dof_torque/{name}', step=sim_data.n_step)
if self.dof_pos:
pos = sim_data.proprio.joint.pos[i]
logger.log(pos, f'dof_pos/{name}', step=sim_data.n_step)

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

@@ -100,6 +100,9 @@ class BasePipeline:
first_reset = True
self.last_reset_time = sim_data.sim_time
sim_data = self.sim.step()
except Exception as e:
logger.error(f"❌ Pipeline execution failed with error: {e}")
raise e
finally:
self.sim.close_viewer()
self.sim.close_video_writer()

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

@@ -144,7 +144,7 @@ class MultiPipeline:
save_path = logger.log_dir / "aggregated_results.yaml"
with open(save_path, 'w') as file:
yaml.dump(summary, file, allow_unicode=True)
yaml.dump(summary, file, allow_unicode=True, sort_keys=False)
logger.info("✅ Aggregated execution finished.")
logger.info(f"📁 Aggregated results saved to: {save_path}")

16
robogauge/tasks/robots/base_robot.py
View File

@@ -45,19 +45,3 @@ class BaseRobot:
action = np.zeros(self.num_action, dtype=np.float32)
return action, self.p_gains, self.d_gains, self.control_type
def get_projected_gravity(quat):
""" Compute world frame gravity (0, 0, -1) projected into robot base frame.
Args:
quat: (4,) quaternion (w, x, y, z) from robot base to world frame
Returns:
projected_gravity: (3,) projected gravity vector in robot base frame
"""
qw, qx, qy, qz = quat
gravity_orientation = np.zeros(3)
gravity_orientation[0] = 2 * (-qz * qx + qw * qy)
gravity_orientation[1] = -2 * (qz * qy + qw * qx)
gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz)
return gravity_orientation

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

@@ -10,7 +10,8 @@
import torch
import numpy as np
from robogauge.tasks.robots.base_robot import BaseRobot, get_projected_gravity
from robogauge.tasks.robots.base_robot import BaseRobot
from robogauge.utils.math_utils import get_projected_gravity
from robogauge.tasks.robots.go2.go2_config import Go2Config
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData

5
robogauge/tasks/robots/go2/go2_moe.py
View File

@@ -10,12 +10,7 @@
import torch
import numpy as np
from robogauge.tasks.robots.base_robot import BaseRobot, get_projected_gravity
from robogauge.tasks.robots.go2.go2_config import Go2Config
from robogauge.tasks.robots.go2.go2 import Go2
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData
from robogauge.utils.logger import logger
class Go2MoE(Go2):
def get_action(self, obs: np.ndarray):

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

@@ -200,7 +200,7 @@ class MujocoSimulator:
joint=JointState(
pos=self.get_sensor_data('joint_pos'),
vel=self.get_sensor_data('joint_vel'),
force=self.get_sensor_data('joint_eff'),
torque=self.get_sensor_data('joint_eff'),
limits=self.dof_limits,
names=self.dof_names,
),
@@ -389,7 +389,7 @@ class MujocoSimulator:
logger.info("Proprioception shapes:")
logger.info(f" joint.pos: { _shape(jp.pos) }")
logger.info(f" joint.vel: { _shape(jp.vel) }")
logger.info(f" joint.force: { _shape(jp.force) }")
logger.info(f" joint.torque: { _shape(jp.torque) }")
logger.info(f" base.pos: { _shape(bs.pos) }")
logger.info(f" base.quat: { _shape(bs.quat) }")

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

@@ -5,7 +5,7 @@ from dataclasses import dataclass
class JointState:
pos: np.ndarray # [rad] shape (n_dof,)
vel: np.ndarray # [rad/s] shape (n_dof,)
force: np.ndarray # [N*m] shape (n_dof,)
torque: np.ndarray # [N*m] shape (n_dof,)
limits: np.ndarray # [rad] shape (n_dof, 2), lower and upper limits
names: list # list of joint names