diff --git a/.gitignore b/.gitignore index 0d94886..e3bdbf8 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,4 @@ __pycache__/ # Logger logs/ +.aim/ \ No newline at end of file diff --git a/UPDATE.md b/UPDATE.md index 8f671ff..ee51819 100644 --- a/UPDATE.md +++ b/UPDATE.md @@ -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模型预测 diff --git a/resources/models/go2/go2_cts_cmd-1,1_38k.pt b/resources/models/go2/go2_cts_cmd-1,1_38k.pt new file mode 100644 index 0000000..e14df59 Binary files /dev/null and b/resources/models/go2/go2_cts_cmd-1,1_38k.pt differ diff --git a/robogauge/tasks/gauge/__init__.py b/robogauge/tasks/gauge/__init__.py index 05c7939..c40b81b 100644 --- a/robogauge/tasks/gauge/__init__.py +++ b/robogauge/tasks/gauge/__init__.py @@ -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 diff --git a/robogauge/tasks/gauge/base_gauge.py b/robogauge/tasks/gauge/base_gauge.py index 9d6ab24..c8994ad 100644 --- a/robogauge/tasks/gauge/base_gauge.py +++ b/robogauge/tasks/gauge/base_gauge.py @@ -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) \ No newline at end of file diff --git a/robogauge/tasks/gauge/base_gauge_config.py b/robogauge/tasks/gauge/base_gauge_config.py index 9bdb3ec..e3a42b6 100644 --- a/robogauge/tasks/gauge/base_gauge_config.py +++ b/robogauge/tasks/gauge/base_gauge_config.py @@ -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 diff --git a/robogauge/tasks/gauge/flat/flat_gauge.py b/robogauge/tasks/gauge/flat/flat_gauge.py deleted file mode 100644 index 3de824f..0000000 --- a/robogauge/tasks/gauge/flat/flat_gauge.py +++ /dev/null @@ -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): - ... diff --git a/robogauge/tasks/gauge/flat/flat_gauge_config.py b/robogauge/tasks/gauge/flat/flat_gauge_config.py deleted file mode 100644 index 6880a5a..0000000 --- a/robogauge/tasks/gauge/flat/flat_gauge_config.py +++ /dev/null @@ -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): - ... diff --git a/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py new file mode 100644 index 0000000..567e118 --- /dev/null +++ b/robogauge/tasks/gauge/gauge_configs/flat_gauge_config.py @@ -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 + + diff --git a/robogauge/tasks/gauge/goal_data.py b/robogauge/tasks/gauge/goal_data.py index 29fad09..767052e 100644 --- a/robogauge/tasks/gauge/goal_data.py +++ b/robogauge/tasks/gauge/goal_data.py @@ -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: diff --git a/robogauge/tasks/gauge/goals/__init__.py b/robogauge/tasks/gauge/goals/__init__.py new file mode 100644 index 0000000..1bef328 --- /dev/null +++ b/robogauge/tasks/gauge/goals/__init__.py @@ -0,0 +1,2 @@ +from robogauge.tasks.gauge.goals.base_goal import BaseGoal +from robogauge.tasks.gauge.goals.velocity_goals import MaxVelocityGoal diff --git a/robogauge/tasks/gauge/goals/base_goal.py b/robogauge/tasks/gauge/goals/base_goal.py new file mode 100644 index 0000000..98178da --- /dev/null +++ b/robogauge/tasks/gauge/goals/base_goal.py @@ -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 diff --git a/robogauge/tasks/gauge/goals/velocity_goals.py b/robogauge/tasks/gauge/goals/velocity_goals.py new file mode 100644 index 0000000..276fe25 --- /dev/null +++ b/robogauge/tasks/gauge/goals/velocity_goals.py @@ -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}" diff --git a/robogauge/tasks/gauge/metrics/__init__.py b/robogauge/tasks/gauge/metrics/__init__.py new file mode 100644 index 0000000..5c89953 --- /dev/null +++ b/robogauge/tasks/gauge/metrics/__init__.py @@ -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 diff --git a/robogauge/tasks/pipeline/base_pipeline.py b/robogauge/tasks/pipeline/base_pipeline.py index dbf2701..efb10e7 100644 --- a/robogauge/tasks/pipeline/base_pipeline.py +++ b/robogauge/tasks/pipeline/base_pipeline.py @@ -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: diff --git a/robogauge/tasks/robots/base_robot_config.py b/robogauge/tasks/robots/base_robot_config.py index ff5a6ed..adf52d5 100644 --- a/robogauge/tasks/robots/base_robot_config.py +++ b/robogauge/tasks/robots/base_robot_config.py @@ -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] - diff --git a/robogauge/tasks/robots/go2/go2.py b/robogauge/tasks/robots/go2/go2.py index 7fa46e8..89c1bdb 100644 --- a/robogauge/tasks/robots/go2/go2.py +++ b/robogauge/tasks/robots/go2/go2.py @@ -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 diff --git a/robogauge/tasks/robots/go2/go2_config.py b/robogauge/tasks/robots/go2/go2_config.py index 96252fa..210601b 100644 --- a/robogauge/tasks/robots/go2/go2_config.py +++ b/robogauge/tasks/robots/go2/go2_config.py @@ -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] + diff --git a/robogauge/tasks/simulator/mujoco_simulator.py b/robogauge/tasks/simulator/mujoco_simulator.py index abe30ea..a27018b 100644 --- a/robogauge/tasks/simulator/mujoco_simulator.py +++ b/robogauge/tasks/simulator/mujoco_simulator.py @@ -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) diff --git a/robogauge/tasks/simulator/sim_data.py b/robogauge/tasks/simulator/sim_data.py index 716c123..964fce3 100644 --- a/robogauge/tasks/simulator/sim_data.py +++ b/robogauge/tasks/simulator/sim_data.py @@ -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 diff --git a/robogauge/utils/helpers.py b/robogauge/utils/helpers.py index beb4df3..a096795 100644 --- a/robogauge/utils/helpers.py +++ b/robogauge/utils/helpers.py @@ -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 = {}