diff --git a/README.md b/README.md index d70787a..b1f3d9c 100644 --- a/README.md +++ b/README.md @@ -168,7 +168,7 @@ xml_path: "{ROOT_DIR}/resources/go2/your-custom-scene.xml" ## TODO -- Try replacing ActionManager-level delay with `DelayedPDActuatorCfg` +- Try replacing ActionManager-level delay with `DelayedPDActuatorCfg` or `UnitreeActuatorCfg_Go2HV`, and make sure the randomization of motor parameters works correctly. --- diff --git a/source/robot_lab/robot_lab/assets/unitree.py b/source/robot_lab/robot_lab/assets/unitree.py index db7dfb9..5f7498c 100644 --- a/source/robot_lab/robot_lab/assets/unitree.py +++ b/source/robot_lab/robot_lab/assets/unitree.py @@ -5,17 +5,71 @@ Reference: https://github.com/unitreerobotics/unitree_ros """ +import os import isaaclab.sim as sim_utils from isaaclab.actuators import DCMotorCfg from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils import configclass from robot_lab.assets import ISAACLAB_ASSETS_DATA_DIR +from robot_lab.assets.unitree_actuator import UnitreeActuatorCfg_Go2HV ## # Configuration ## +@configclass +class UnitreeArticulationCfg(ArticulationCfg): + """Configuration for Unitree articulations.""" -UNITREE_GO2_CFG = ArticulationCfg( + joint_sdk_names: list[str] = None + + soft_joint_pos_limit_factor = 0.9 + + +@configclass +class UnitreeUrdfFileCfg(sim_utils.UrdfFileCfg): + fix_base: bool = False + activate_contact_sensors: bool = True + replace_cylinders_with_capsules = True + joint_drive = sim_utils.UrdfConverterCfg.JointDriveCfg( + gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0) + ) + articulation_props = sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=4, + ) + rigid_props = sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ) + + def replace_asset(self, meshes_dir, urdf_path): + """Replace the asset with a temporary copy to avoid modifying the original asset. + + When need to change the collisions, place the modified URDF file separately in this repository, + and let `meshes_dir` be provided by `unitree_ros`. + This function will auto construct a complete `robot_description` file structure in the `/tmp` directory. + Note: The mesh references inside the URDF should be in the same directory level as the URDF itself. + """ + tmp_meshes_dir = "/tmp/IsaacLab/unitree_rl_lab/meshes" + if os.path.exists(tmp_meshes_dir): + os.remove(tmp_meshes_dir) + os.makedirs("/tmp/IsaacLab/unitree_rl_lab", exist_ok=True) + os.symlink(meshes_dir, tmp_meshes_dir) + + self.asset_path = "/tmp/IsaacLab/unitree_rl_lab/robot.urdf" + if os.path.exists(self.asset_path): + os.remove(self.asset_path) + os.symlink(urdf_path, self.asset_path) + +# Go2 config from robot_lab [https://github.com/fan-ziqi/robot_lab] +GO2_CFG_ROBOTLAB = ArticulationCfg( spawn=sim_utils.UrdfFileCfg( fix_base=False, merge_fixed_joints=True, @@ -66,5 +120,39 @@ UNITREE_GO2_CFG = ArticulationCfg( ), }, ) -"""Configuration of Unitree Go2 using DC motor. -""" + + + +# Go2 config from unitree_rl_lab [https://github.com/unitreerobotics/unitree_rl_lab] +GO2_CFG_UNITREE = UnitreeArticulationCfg( + spawn=UnitreeUrdfFileCfg( + asset_path=f"{ISAACLAB_ASSETS_DATA_DIR}/go2/urdf/go2.urdf", + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.4), + joint_pos={ + ".*R_hip_joint": -0.1, + ".*L_hip_joint": 0.1, + "F[L,R]_thigh_joint": 0.8, + "R[L,R]_thigh_joint": 1.0, + ".*_calf_joint": -1.5, + }, + joint_vel={".*": 0.0}, + ), + actuators={ + "GO2HV": UnitreeActuatorCfg_Go2HV( + joint_names_expr=[".*"], + stiffness=25.0, + damping=0.5, + friction=0.01, + ), + }, + # fmt: off + joint_sdk_names=[ + "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", + "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", + "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", + "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint" + ], + # fmt: on +) diff --git a/source/robot_lab/robot_lab/assets/unitree_actuator.py b/source/robot_lab/robot_lab/assets/unitree_actuator.py new file mode 100644 index 0000000..bb894ec --- /dev/null +++ b/source/robot_lab/robot_lab/assets/unitree_actuator.py @@ -0,0 +1,130 @@ +# Unitree actuator class that implements a torque-speed curve for the actuators. +# source: https://github.com/unitreerobotics/unitree_rl_lab + +from __future__ import annotations + +import torch +from dataclasses import MISSING + +from isaaclab.actuators import DelayedPDActuator, DelayedPDActuatorCfg +from isaaclab.utils import configclass +from isaaclab.utils.types import ArticulationActions + + +class UnitreeActuator(DelayedPDActuator): + """Unitree actuator class that implements a torque-speed curve for the actuators. + + The torque-speed curve is defined as follows: + + Torque Limit, N·m + ^ + Y2──────────| + |──────────────Y1 + | │\ + | │ \ + | │ \ + | | \ + ------------+--------------|------> velocity: rad/s + X1 X2 + + - Y1: Peak Torque Test (Torque and Speed in the Same Direction) + - Y2: Peak Torque Test (Torque and Speed in the Opposite Direction) + - X1: Maximum Speed at Full Torque (T-N Curve Knee Point) + - X2: No-Load Speed Test + + - Fs: Static friction coefficient + - Fd: Dynamic friction coefficient + - Va: Velocity at which the friction is fully activated + """ + + cfg: UnitreeActuatorCfg + + armature: torch.Tensor + """The armature of the actuator joints. Shape is (num_envs, num_joints). + armature = J2 + J1 * i2 ^ 2 + Jr * (i1 * i2) ^ 2 + """ + + def __init__(self, cfg: UnitreeActuatorCfg, *args, **kwargs): + super().__init__(cfg, *args, **kwargs) + + self._joint_vel = torch.zeros_like(self.computed_effort) + self._effort_y1 = self._parse_joint_parameter(cfg.Y1, 1e9) + self._effort_y2 = self._parse_joint_parameter(cfg.Y2, cfg.Y1) + self._velocity_x1 = self._parse_joint_parameter(cfg.X1, 1e9) + self._velocity_x2 = self._parse_joint_parameter(cfg.X2, 1e9) + self._friction_static = self._parse_joint_parameter(cfg.Fs, 0.0) + self._friction_dynamic = self._parse_joint_parameter(cfg.Fd, 0.0) + self._activation_vel = self._parse_joint_parameter(cfg.Va, 0.01) + + def compute( + self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor + ) -> ArticulationActions: + # save current joint vel + self._joint_vel[:] = joint_vel + # calculate the desired joint torques + control_action = super().compute(control_action, joint_pos, joint_vel) + + # apply friction model on the torque + self.applied_effort -= ( + self._friction_static * torch.tanh(joint_vel / self._activation_vel) + self._friction_dynamic * joint_vel + ) + + control_action.joint_positions = None + control_action.joint_velocities = None + control_action.joint_efforts = self.applied_effort + + return control_action + + def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor: + # check if the effort is the same direction as the joint velocity + same_direction = (self._joint_vel * effort) > 0 + max_effort = torch.where(same_direction, self._effort_y1, self._effort_y2) + # check if the joint velocity is less than the max speed at full torque + max_effort = torch.where( + self._joint_vel.abs() < self._velocity_x1, max_effort, self._compute_effort_limit(max_effort) + ) + return torch.clip(effort, -max_effort, max_effort) + + def _compute_effort_limit(self, max_effort): + k = -max_effort / (self._velocity_x2 - self._velocity_x1) + limit = k * (self._joint_vel.abs() - self._velocity_x1) + max_effort + return limit.clip(min=0.0) + + +@configclass +class UnitreeActuatorCfg(DelayedPDActuatorCfg): + """ + Configuration for Unitree actuators. + """ + + class_type: type = UnitreeActuator + + X1: float = 1e9 + """Maximum Speed at Full Torque(T-N Curve Knee Point) Unit: rad/s""" + + X2: float = 1e9 + """No-Load Speed Test Unit: rad/s""" + + Y1: float = MISSING + """Peak Torque Test(Torque and Speed in the Same Direction) Unit: N*m""" + + Y2: float | None = None + """Peak Torque Test(Torque and Speed in the Opposite Direction) Unit: N*m""" + + Fs: float = 0.0 + """ Static friction coefficient """ + + Fd: float = 0.0 + """ Dynamic friction coefficient """ + + Va: float = 0.01 + """ Velocity at which the friction is fully activated """ + + +@configclass +class UnitreeActuatorCfg_Go2HV(UnitreeActuatorCfg): + X1 = 13.5 + X2 = 30 + Y1 = 20.2 + Y2 = 23.4 + diff --git a/source/robot_lab/robot_lab/tasks/go2/env_cfg.py b/source/robot_lab/robot_lab/tasks/go2/env_cfg.py index 011f34d..eaa40b1 100644 --- a/source/robot_lab/robot_lab/tasks/go2/env_cfg.py +++ b/source/robot_lab/robot_lab/tasks/go2/env_cfg.py @@ -18,7 +18,7 @@ from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise import isaaclab.terrains as terrain_gen import robot_lab.tasks.go2.mdp as mdp -from robot_lab.assets.unitree import UNITREE_GO2_CFG +from robot_lab.assets.unitree import GO2_CFG_ROBOTLAB, GO2_CFG_UNITREE JOINT_NAMES = [ "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", @@ -105,7 +105,7 @@ class Go2SceneCfg(InteractiveSceneCfg): debug_vis=False ) - robot: ArticulationCfg = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot: ArticulationCfg = GO2_CFG_ROBOTLAB.replace(prim_path="{ENV_REGEX_NS}/Robot") height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", @@ -331,7 +331,7 @@ class EventCfg: func=mdp.reset_joints_by_scale, mode="reset", params={ - "position_range": (0.8, 1.2), + "position_range": (0.5, 1.5), "velocity_range": (0.0, 0.0), }, ) @@ -387,15 +387,7 @@ class EventCfg: }, }, ) - randomize_apply_external_force_torque = EventTerm( - func=mdp.apply_external_force_torque, - mode="reset", - params={ - "asset_cfg": SceneEntityCfg("robot", body_names=BASE_LINK_NAME), - "force_range": (-5.0, 5.0), - "torque_range": (-5.0, 5.0), - }, - ) + @configclass class RewardsCfg: