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

v0.1.3

Browse Source
This commit is contained in:
wty-yy
2025-11-28 22:08:32 +08:00
parent 4cc0687114
commit c58ac0ab89
39 changed files with 406 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
robogauge/tasks/robots/__init__.py
View File

@@ -1,4 +1,4 @@
from .base_robot_config import RobotConfig
from .base_robot import BaseRobot
# from .go2.go2_config import Go2Config
# from .go2.go2_controller import Go2Controller
from .go2.go2_config import Go2Config
from .go2.go2 import Go2

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

@@ -9,21 +9,55 @@
'''
import torch
import numpy as np
from robogauge.utils.helpers import parse_path
from robogauge.utils.logger import logger
from robogauge.tasks.robots.base_robot_config import RobotConfig
from robogauge.tasks.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData
class BaseRobot:
def __init__(self, cfg: RobotConfig):
self.num_act = cfg.mdp.num_actions
self.num_obs = cfg.mdp.num_observations
self.model = None
self.cfg = cfg
self.device = self.cfg.control.device
self.num_obs = cfg.control.num_observations
self.num_action = cfg.control.num_actions
self.control_type = cfg.control.control_type
self.p_gains = np.array(cfg.control.p_gains)
self.d_gains = np.array(cfg.control.d_gains)
script_model_path = parse_path(cfg.control.torch_script_model_path)
logger.info(f"Loading robot model from '{script_model_path}'")
self.model = torch.jit.load(script_model_path).to(self.device)
self.model.eval()
def load_model(self):
...
def build_observation(self, sim_info: dict, goal_info: dict) -> np.ndarray:
obs = np.zeros(self.num_obs)
def build_observation(self, sim_data: SimData, goal_data: GoalData) -> np.ndarray:
obs = np.zeros(self.num_obs, dtype=np.float32)
return obs
def get_action(self, obs) -> np.ndarray:
action = np.zeros_like(self.num_act)
return action
def get_action(self, obs: np.ndarray):
"""
Returns:
action: (num_action,) target joint positions/velocities/torques
p_gains: (num_action,) proportional gains for Mujoco PD controller
d_gains: (num_action,) derivative gains for Mujoco PD controller
control_type: 'P', 'V', or 'T' for position/velocity/torque control
"""
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

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

@@ -7,6 +7,7 @@
@Blog : https://wty-yy.github.io/
@Desc : Base Robot Configuration
'''
from typing_extensions import Literal
from robogauge.utils.config import Config
class RobotConfig(Config):
@@ -17,16 +18,32 @@ class RobotConfig(Config):
robot_spawn_height = 0.1 # z [m]
class control:
torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_61500.pt"
device = 'cpu'
torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_83501.pt"
control_dt = 0.02 # 50 Hz
action_scale = 0.25 # target pos = action_scale * action * default_pos
stiffness = 20.0 # [N*m/rad]
damping = 0.5 # [N*m*s/rad]
control_type = 'P' # Position control
# 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]
d_gains = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5] # [N*m*s/rad]
class mdp:
num_observations = 46
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]
mj2model_dof_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
class scales:
lin_vel = 2.0
ang_vel = 0.25
dof_pos = 1.0
dof_vel = 0.05
action = 0.25 # target pos = action_scale * action * default_pos
cmd = [2.0, 2.0, 0.25]
class commands:
lin_vel_x = [-1, 1] # min max [m/s]
lin_vel_y = [-1, 1] # min max [m/s]

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

@@ -0,0 +1,57 @@
# -*- coding: utf-8 -*-
'''
@File : go2.py
@Time : 2025/11/28 15:27:07
@Author : wty-yy
@Version : 1.0
@Blog : https://wty-yy.github.io/
@Desc : None
'''
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.simulator.sim_data import SimData
from robogauge.tasks.gauge.goal_data import GoalData
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.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
self.mj2model_idx = self.cfg.control.mj2model_dof_indices
self.model2mj_idx = [self.mj2model_idx.index(i) for i in range(len(self.mj2model_idx))]
def build_observation(self, sim_data: SimData, goal_data: GoalData) -> np.ndarray:
sim_proprio = sim_data.proprio
obs = np.zeros(self.num_obs)
if goal_data.goal_type == 'velocity':
ang_vel = sim_proprio.imu.ang_vel * self.cfg.control.scales.ang_vel
projected_gravity = get_projected_gravity(sim_proprio.imu.quat)
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)
cmd *= self.cfg.control.scales.cmd
obs[:3] = ang_vel
obs[3:6] = projected_gravity
obs[6:9] = cmd
obs[9:9+self.num_action] = dof_pos[self.mj2model_idx]
obs[9+self.num_action:9+2*self.num_action] = dof_vel[self.mj2model_idx]
obs[9+2*self.num_action:9+3*self.num_action] = self.last_action[self.mj2model_idx]
else:
raise NotImplementedError(f"Goal type '{goal_data.goal_type}' not implemented in Go2 robot.")
return obs
def get_action(self, obs: np.ndarray):
obs_tensor = torch.tensor(obs, dtype=torch.float32).unsqueeze(0).to(self.device)
action = self.model(obs_tensor).detach().cpu().numpy().squeeze(0)[self.model2mj_idx]
self.last_action = action
target_dof_pos = action * self.action_scale + self.default_dof_pos
return target_dof_pos, self.p_gains, self.d_gains, self.control_type

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

@@ -7,14 +7,44 @@
@Blog : https://wty-yy.github.io/
@Desc : Go2 Robot Configuration
'''
from typing_extensions import Literal
from robogauge.tasks.robots import RobotConfig
class Go2Config(RobotConfig):
robot_class = 'Go2'
class assets:
robot_xml = "{ROBOGAUGE_ROOT_DIR}/resources/robots/go2/go2.xml"
robot_spawn_height = 0.1 # z [m]
class control:
class control(RobotConfig.control):
device = 'cpu'
torch_script_model_path = "{ROBOGAUGE_ROOT_DIR}/resources/models/go2/go2_cts_83501.pt"
control_dt = 0.02 # 50 Hz
action_scale = 0.25 # scale for normalized actions
control_type = 'P' # Position control
# 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]
d_gains = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5] # [N*m*s/rad]
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]
mj2model_dof_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
class scales(RobotConfig.control.scales):
lin_vel = 2.0
ang_vel = 0.25
dof_pos = 1.0
dof_vel = 0.05
action = 0.25 # target pos = action_scale * action * default_pos
cmd = [2.0, 2.0, 0.25]
class commands(RobotConfig.commands):
lin_vel_x = [-1, 1] # min max [m/s]
lin_vel_y = [-1, 1] # min max [m/s]
ang_vel_yaw = [-1, 1] # min max [rad/s]