From 85405f77076aa36a3386d1f8c5770d93d1491aa8 Mon Sep 17 00:00:00 2001 From: nathanjzhao Date: Wed, 3 Jul 2024 21:29:42 -0700 Subject: [PATCH 01/21] added stompyv2 --- .gitignore | 5 +- README.md | 1 + sim/deploy/run.py | 2 +- sim/env.py | 9 +- sim/humanoid_gym/envs/getup_config.py | 10 +- sim/humanoid_gym/envs/getup_env.py | 2 +- sim/humanoid_gym/envs/legs_config.py | 14 +- sim/humanoid_gym/envs/legs_env.py | 11 +- sim/humanoid_gym/envs/stompy_config.py | 16 +- sim/humanoid_gym/envs/stompy_env.py | 13 +- sim/scripts/create_fixed_torso.py | 2 +- sim/scripts/print_joints.py | 0 sim/scripts/simulate_mjcf.py | 0 sim/scripts/simulate_urdf.py | 7 +- sim/stompy2/joints.py | 514 +++++++++++++++++++++++++ 15 files changed, 574 insertions(+), 32 deletions(-) mode change 100644 => 100755 sim/deploy/run.py mode change 100644 => 100755 sim/scripts/create_fixed_torso.py mode change 100644 => 100755 sim/scripts/print_joints.py mode change 100644 => 100755 sim/scripts/simulate_mjcf.py mode change 100644 => 100755 sim/scripts/simulate_urdf.py create mode 100755 sim/stompy2/joints.py diff --git a/.gitignore b/.gitignore index 3defebe6..aa5ccec4 100755 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,8 @@ # .gitignore - +# Robot Parts +*.stl +*.dae +*.urdf # Local files .env # Python diff --git a/README.md b/README.md index 5a8113a4..dee107ba 100755 --- a/README.md +++ b/README.md @@ -80,6 +80,7 @@ python sim/humanoid_gym/train.py --task=stompy_ppo --num_envs=4096 --headless 3. Run evaluation with the following command: ```bash python sim/humanoid_gym/play.py --task legs_ppo --sim_device cpu + ``` See [this doc](https://docs.google.com/document/d/1YZzBqIXO7oq7vIKu-BZr5ssNsi3nKtxpRPnfSnTXojA/edit?usp=sharing) for more beginner tips. diff --git a/sim/deploy/run.py b/sim/deploy/run.py old mode 100644 new mode 100755 index 9f5e02fe..bd34f524 --- a/sim/deploy/run.py +++ b/sim/deploy/run.py @@ -21,7 +21,7 @@ from sim.deploy.config import RobotConfig from sim.env import stompy_mjcf_path -from sim.stompy.joints import StompyFixed +from sim.stompy2.joints import StompyFixed class Worlds(Enum): diff --git a/sim/env.py b/sim/env.py index b512d58d..e2745131 100755 --- a/sim/env.py +++ b/sim/env.py @@ -3,9 +3,16 @@ import os from pathlib import Path +from dotenv import load_dotenv + +load_dotenv() + def model_dir() -> Path: - return Path(os.environ.get("MODEL_DIR", "models")) + # return Path(os.environ.get("MODEL_DIR", "models")) + # return Path(os.environ.get("MODEL_DIR", "stompytherobot")) + # return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") + return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") def run_dir() -> Path: diff --git a/sim/humanoid_gym/envs/getup_config.py b/sim/humanoid_gym/envs/getup_config.py index 9cf0e6f8..6e16abc6 100755 --- a/sim/humanoid_gym/envs/getup_config.py +++ b/sim/humanoid_gym/envs/getup_config.py @@ -8,7 +8,7 @@ from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO from sim.env import stompy_urdf_path -from sim.stompy.joints import Stompy +from sim.stompy2.joints import Stompy NUM_JOINTS = len(Stompy.all_joints()) # 37 @@ -39,8 +39,12 @@ class asset(LeggedRobotCfg.asset): # noqa: N801 file = str(stompy_urdf_path()) name = "stompy" - foot_name = "_leg_1_x4_1_outer_1" # "foot" - knee_name = "belt_knee" # "knee" + foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" + knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" + + # old + # foot_name = "_leg_1_x4_1_outer_1" # "foot" + # knee_name = "belt_knee" terminate_after_contacts_on = [] # "link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter diff --git a/sim/humanoid_gym/envs/getup_env.py b/sim/humanoid_gym/envs/getup_env.py index 41434cd4..b5018660 100755 --- a/sim/humanoid_gym/envs/getup_env.py +++ b/sim/humanoid_gym/envs/getup_env.py @@ -8,7 +8,7 @@ from isaacgym import gymtorch from isaacgym.torch_utils import * -from sim.stompy.joints import Stompy +from sim.stompy2.joints import Stompy default_feet_height = 0.0 diff --git a/sim/humanoid_gym/envs/legs_config.py b/sim/humanoid_gym/envs/legs_config.py index 8be39bcd..6b91c071 100755 --- a/sim/humanoid_gym/envs/legs_config.py +++ b/sim/humanoid_gym/envs/legs_config.py @@ -4,7 +4,7 @@ from sim.env import stompy_urdf_path from sim.humanoid_gym.envs.stompy_config import StompyCfg -from sim.stompy.joints import StompyFixed +from sim.stompy2.joints import StompyFixed NUM_JOINTS = len(StompyFixed.default_standing()) # 17 @@ -33,13 +33,19 @@ class asset(LeggedRobotCfg.asset): name = "stompy" - foot_name = "_leg_1_x4_1_outer_1" # "foot" - knee_name = "belt_knee" + # foot_name = "_foot_1_foot_1" + foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" + knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" + + # old + # foot_name = "_leg_1_x4_1_outer_1" # "foot" + # knee_name = "belt_knee" termination_height = 0.23 default_feet_height = 0.0 - terminate_after_contacts_on = ["link_torso_1_top_torso_1"] + terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] + # terminate_after_contacts_on = ["link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter diff --git a/sim/humanoid_gym/envs/legs_env.py b/sim/humanoid_gym/envs/legs_env.py index f9fdddf9..e217c230 100755 --- a/sim/humanoid_gym/envs/legs_env.py +++ b/sim/humanoid_gym/envs/legs_env.py @@ -8,7 +8,7 @@ from isaacgym import gymtorch from isaacgym.torch_utils import * -from sim.stompy.joints import StompyFixed +from sim.stompy2.joints import StompyFixed class LegsFreeEnv(LeggedRobot): @@ -124,14 +124,14 @@ def compute_ref_state(self): sin_pos_l[sin_pos_l > 0] = 0 self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] = sin_pos_l * scale_1 - self.ref_dof_pos[:, self.legs_joints["left_knee"]] = sin_pos_l * scale_2 - self.ref_dof_pos[:, self.legs_joints["left_ankle"]] = sin_pos_l * scale_1 + self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] = sin_pos_l * scale_2 + self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] = sin_pos_l * scale_1 # right foot stance phase set to default joint pos sin_pos_r[sin_pos_r < 0] = 0 self.ref_dof_pos[:, self.legs_joints["right_hip_pitch"]] = sin_pos_r * scale_1 - self.ref_dof_pos[:, self.legs_joints["right_knee"]] = sin_pos_r * scale_2 - self.ref_dof_pos[:, self.legs_joints["right_ankle"]] = sin_pos_r * scale_1 + self.ref_dof_pos[:, self.legs_joints["right_knee_pitch"]] = sin_pos_r * scale_2 + self.ref_dof_pos[:, self.legs_joints["right_ankle_roll"]] = sin_pos_r * scale_1 # Double support phase self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0 @@ -302,6 +302,7 @@ def _reward_knee_distance(self): """ Calculates the reward based on the distance between the knee of the humanoid. """ + # breakpoint() foot_pos = self.rigid_state[:, self.knee_indices, :2] foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1) fd = self.cfg.rewards.min_dist diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 1833af37..5c8a1b2d 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -3,9 +3,9 @@ from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO from sim.env import stompy_urdf_path -from sim.stompy.joints import Stompy +from sim.stompy2.joints import Stompy -NUM_JOINTS = len(Stompy.all_joints()) # 37 +NUM_JOINTS = len(Stompy.all_joints()) # 33 class StompyCfg(LeggedRobotCfg): @@ -38,13 +38,17 @@ class asset(LeggedRobotCfg.asset): name = "stompy" - foot_name = "_leg_1_x4_1_outer_1" # "foot" - knee_name = "belt_knee" + foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" + knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" + + # old + # foot_name = "_leg_1_x4_1_outer_1" # "foot" + # knee_name = "belt_knee" termination_height = 0.23 default_feet_height = 0.0 - - terminate_after_contacts_on = ["link_torso_1_top_torso_1"] + terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] + # terminate_after_contacts_on = ["link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index 461c9a6a..dade8f79 100755 --- a/sim/humanoid_gym/envs/stompy_env.py +++ b/sim/humanoid_gym/envs/stompy_env.py @@ -8,7 +8,7 @@ from isaacgym import gymtorch from isaacgym.torch_utils import * -from sim.stompy.joints import Stompy +from sim.stompy2.joints import Stompy class StompyFreeEnv(LeggedRobot): @@ -56,6 +56,7 @@ def __init__(self, cfg: LeggedRobotCfg, sim_params, physics_engine, sim_device, self.legs_joints = {} for name, joint in Stompy.legs.left.joints_motors(): + print(name) joint_handle = self.gym.find_actor_dof_handle(env_handle, actor_handle, joint) self.legs_joints["left_" + name] = joint_handle @@ -125,14 +126,14 @@ def compute_ref_state(self): sin_pos_l[sin_pos_l > 0] = 0 self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] = sin_pos_l * scale_1 - self.ref_dof_pos[:, self.legs_joints["left_knee"]] = sin_pos_l * scale_2 - self.ref_dof_pos[:, self.legs_joints["left_ankle"]] = sin_pos_l * scale_1 + self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] = sin_pos_l * scale_2 + self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] = sin_pos_l * scale_1 # right foot stance phase set to default joint pos sin_pos_r[sin_pos_r < 0] = 0 self.ref_dof_pos[:, self.legs_joints["right_hip_pitch"]] = sin_pos_r * scale_1 - self.ref_dof_pos[:, self.legs_joints["right_knee"]] = sin_pos_r * scale_2 - self.ref_dof_pos[:, self.legs_joints["right_ankle"]] = sin_pos_r * scale_1 + self.ref_dof_pos[:, self.legs_joints["right_knee_pitch"]] = sin_pos_r * scale_2 + self.ref_dof_pos[:, self.legs_joints["right_ankle_roll"]] = sin_pos_r * scale_1 # Double support phase self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0 @@ -207,7 +208,6 @@ def compute_observations(self): contact_mask = self.contact_forces[:, self.feet_indices, 2] > 5.0 self.command_input = torch.cat((sin_pos, cos_pos, self.commands[:, :3] * self.commands_scale), dim=1) - breakpoint() q = (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos dq = self.dof_vel * self.obs_scales.dof_vel @@ -389,6 +389,7 @@ def _reward_base_height(self): The reward is computed based on the height difference between the robot's base and the average height of its feet when they are in contact with the ground. """ + # breakpoint() stance_mask = self._get_gait_phase() measured_heights = torch.sum(self.rigid_state[:, self.feet_indices, 2] * stance_mask, dim=1) / torch.sum( stance_mask, dim=1 diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py old mode 100644 new mode 100755 index 0fa0be87..1b7226ed --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -3,7 +3,7 @@ import xml.etree.ElementTree as ET -from sim.stompy.joints import StompyFixed +from sim.stompy2.joints import StompyFixed STOMPY_URDF = "stompy/robot.urdf" diff --git a/sim/scripts/print_joints.py b/sim/scripts/print_joints.py old mode 100644 new mode 100755 diff --git a/sim/scripts/simulate_mjcf.py b/sim/scripts/simulate_mjcf.py old mode 100644 new mode 100755 diff --git a/sim/scripts/simulate_urdf.py b/sim/scripts/simulate_urdf.py old mode 100644 new mode 100755 index 068e6810..697b3620 --- a/sim/scripts/simulate_urdf.py +++ b/sim/scripts/simulate_urdf.py @@ -12,10 +12,9 @@ from typing import Any, Dict, Literal, NewType from isaacgym import gymapi, gymtorch, gymutil - from sim.env import stompy_urdf_path from sim.logging import configure_logging -from sim.stompy.joints import Stompy +from sim.stompy2.joints import Stompy logger = logging.getLogger(__name__) @@ -150,8 +149,9 @@ def load_gym() -> GymParams: # Resets the DOF positions to the starting positions. # dof_vel[:] = 0.0 - starting_positions = Stompy.default_positions() + starting_positions = Stompy.default_standing() dof_ids: Dict[str, int] = gym.get_actor_dof_dict(env, robot) + print(starting_positions) for joint_name, joint_position in starting_positions.items(): dof_pos[0, dof_ids[joint_name]] = joint_position env_ids_int32 = torch.zeros(1, dtype=torch.int32) @@ -161,6 +161,7 @@ def load_gym() -> GymParams: gymtorch.unwrap_tensor(env_ids_int32), 1, ) + print(dof_pos) return GymParams( gym=gym, diff --git a/sim/stompy2/joints.py b/sim/stompy2/joints.py new file mode 100755 index 00000000..d3b522bb --- /dev/null +++ b/sim/stompy2/joints.py @@ -0,0 +1,514 @@ +"""Defines a more Pythonic interface for specifying the joint names. + +The best way to re-generate this snippet for a new robot is to use the +`sim/scripts/print_joints.py` script. This script will print out a hierarchical +tree of the various joint names in the robot. +""" + +import textwrap +from abc import ABC +from typing import Dict, List, Tuple, Union + +import numpy as np + + +class Node(ABC): + @classmethod + def children(cls) -> List["Union[Node, str]"]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, (Node, str)) + ] + + @classmethod + def joints(cls) -> List[str]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, str) + ] + + @classmethod + def joints_motors(cls) -> List[Tuple[str, str]]: + joint_names: List[Tuple[str, str]] = [] + for attr in dir(cls): + if not attr.startswith("__"): + attr2 = getattr(cls, attr) + if isinstance(attr2, str): + joint_names.append((attr, attr2)) + + return joint_names + + @classmethod + def all_joints(cls) -> List[str]: + leaves = cls.joints() + for child in cls.children(): + if isinstance(child, Node): + leaves.extend(child.all_joints()) + return leaves + + def __str__(self) -> str: + parts = [str(child) for child in self.children()] + parts_str = textwrap.indent("\n" + "\n".join(parts), " ") + return f"[{self.__class__.__name__}]{parts_str}" + + +class Torso(Node): + roll = "torso roll" + + +class LeftHand(Node): + wrist_roll = "wrist roll" + wrist_pitch = "wrist pitch" + wrist_yaw = "wrist yaw" + left_finger = "left hand left finger" + right_finger = "left hand right finger" + + +class LeftArm(Node): + shoulder_yaw = "shoulder yaw" + shoulder_pitch = "shoulder pitch" + shoulder_roll = "shoulder roll" + elbow_pitch = "elbow pitch" + hand = LeftHand() + + +class RightHand(Node): + wrist_roll = "right wrist roll" + wrist_pitch = "right wrist pitch" + wrist_yaw = "right wrist yaw" + left_finger = "right hand left finger" + right_finger = "right hand right finger" + + +class RightArm(Node): + shoulder_yaw = "right shoulder yaw" + shoulder_pitch = "right shoulder pitch" + shoulder_roll = "right shoulder roll" + elbow_pitch = "right elbow pitch" + hand = RightHand() + + +class LeftLeg(Node): + hip_roll = "left hip roll" + hip_yaw = "left hip yaw" + hip_pitch = "left hip pitch" + knee_pitch = "left knee pitch" + ankle_pitch = "left ankle pitch" + ankle_roll = "left ankle roll" + + +class RightLeg(Node): + hip_roll = "right hip roll" + hip_yaw = "right hip yaw" + hip_pitch = "right hip pitch" + knee_pitch = "right knee pitch" + ankle_pitch = "right ankle pitch" + ankle_roll = "right ankle roll" + + +class Legs(Node): + left = LeftLeg() + right = RightLeg() + + +class Stompy(Node): + torso = Torso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + @classmethod + def default_positions(cls) -> Dict[str, float]: + return { + Stompy.torso.roll: 0.0, + Stompy.left_arm.shoulder_yaw: np.deg2rad(60), + Stompy.left_arm.shoulder_pitch: np.deg2rad(60), + Stompy.right_arm.shoulder_yaw: np.deg2rad(-60), + } + + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + Stompy.torso.roll: 0.0, + # arms + Stompy.left_arm.shoulder_pitch: -0.126, + Stompy.left_arm.shoulder_yaw: 2.12, + Stompy.left_arm.shoulder_roll: 1.89, + Stompy.right_arm.shoulder_pitch: -1.13, + Stompy.right_arm.shoulder_yaw: 2.1, + Stompy.right_arm.shoulder_roll: -1.23, + Stompy.left_arm.elbow_pitch: 3.0, + Stompy.right_arm.elbow_pitch: 3.0, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: -0.346, + Stompy.left_arm.hand.wrist_pitch: -0.251, + Stompy.left_arm.hand.wrist_yaw: 0.377, + Stompy.right_arm.hand.wrist_roll: -3.14, + Stompy.right_arm.hand.wrist_pitch: -0.437, + Stompy.right_arm.hand.wrist_yaw: 0.911, + # legs + Stompy.legs.left.hip_pitch: -1.55, + Stompy.legs.left.hip_roll: 1.46, + Stompy.legs.left.hip_yaw: 1.45, + Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.79, + Stompy.legs.right.hip_pitch: 1.55, + Stompy.legs.right.hip_roll: -1.67, + Stompy.legs.right.hip_yaw: 1.04, + Stompy.legs.right.knee_pitch: 2.01, + Stompy.legs.right.ankle_pitch: 0.44, + Stompy.legs.right.ankle_roll: 1.79, + } + + @classmethod + def default_sitting(cls) -> Dict[str, float]: + return { + Stompy.torso.roll: 0.0, + # arms + Stompy.left_arm.shoulder_pitch: -0.126, + Stompy.left_arm.shoulder_yaw: 2.12, + Stompy.left_arm.shoulder_roll: 1.89, + Stompy.right_arm.shoulder_pitch: -1.13, + Stompy.right_arm.shoulder_yaw: 2.1, + Stompy.right_arm.shoulder_roll: -1.23, + Stompy.left_arm.elbow_pitch: 3.0, + Stompy.right_arm.elbow_pitch: 3.0, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: -0.346, + Stompy.left_arm.hand.wrist_pitch: -0.251, + Stompy.left_arm.hand.wrist_yaw: 0.377, + Stompy.right_arm.hand.wrist_roll: -3.14, + Stompy.right_arm.hand.wrist_pitch: -0.437, + Stompy.right_arm.hand.wrist_yaw: 0.911, + # legs + Stompy.legs.left.hip_pitch: -1.55, + Stompy.legs.left.hip_roll: 1.46, + Stompy.legs.left.hip_yaw: 1.45, + Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.79, + Stompy.legs.right.hip_pitch: 1.55, + Stompy.legs.right.hip_roll: -1.67, + Stompy.legs.right.hip_yaw: 1.04, + Stompy.legs.right.knee_pitch: 2.01, + Stompy.legs.right.ankle_pitch: 0.44, + Stompy.legs.right.ankle_roll: 1.79, + } + + +class StompyFixed(Stompy): + torso = Torso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + Stompy.torso.roll: 0.0, + # arms + Stompy.left_arm.shoulder_pitch: -0.126, + Stompy.left_arm.shoulder_yaw: 2.12, + Stompy.left_arm.shoulder_roll: 1.89, + Stompy.right_arm.shoulder_pitch: -1.13, + Stompy.right_arm.shoulder_yaw: 2.1, + Stompy.right_arm.shoulder_roll: -1.23, + Stompy.left_arm.elbow_pitch: 3.0, + Stompy.right_arm.elbow_pitch: 3.0, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: -0.346, + Stompy.left_arm.hand.wrist_pitch: -0.251, + Stompy.left_arm.hand.wrist_yaw: 0.377, + Stompy.right_arm.hand.wrist_roll: -3.14, + Stompy.right_arm.hand.wrist_pitch: -0.437, + Stompy.right_arm.hand.wrist_yaw: 0.911, + # legs + Stompy.legs.left.hip_pitch: -1.55, + Stompy.legs.left.hip_roll: 1.46, + Stompy.legs.left.hip_yaw: 1.45, + Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.79, + Stompy.legs.right.hip_pitch: 1.55, + Stompy.legs.right.hip_roll: -1.67, + Stompy.legs.right.hip_yaw: 1.04, + Stompy.legs.right.knee_pitch: 2.01, + Stompy.legs.right.ankle_pitch: 0.44, + Stompy.legs.right.ankle_roll: 1.79, + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + # arms + Stompy.torso.roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.left_arm.shoulder_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.left_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.30580090, + }, + Stompy.left_arm.shoulder_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.right_arm.shoulder_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.right_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.3058009, + }, + Stompy.right_arm.shoulder_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + # hands + Stompy.left_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.left_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, + Stompy.right_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.right_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.left_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + Stompy.right_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + # legs + Stompy.legs.left.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.left.hip_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.left.hip_yaw: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.left.knee_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.left.ankle_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.left.ankle_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.right.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.right.hip_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.right.hip_yaw: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.right.knee_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.right.ankle_pitch: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + Stompy.legs.right.ankle_roll: { + "lower": -3.1415927, + "upper": 3.1415927, + }, + } + + +class ImuTorso(Torso): + pitch = "torso_1_x8_1_dof_x8" + pelvis_tx = "pelvis_tx" + pelvis_tz = "pelvis_tz" + pelvis_ty = "pelvis_ty" + tilt = "pelvis_tilt" + list = "pelvis_list" + rotation = "pelvis_rotation" + + +class MjcfStompy(Stompy): + torso = ImuTorso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + # test the model + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + MjcfStompy.head.left_right: 0.8526, + # arms + MjcfStompy.left_arm.shoulder_yaw: 1.57, + MjcfStompy.left_arm.shoulder_pitch: 1.59, + MjcfStompy.left_arm.elbow_roll: 0.0, + MjcfStompy.left_arm.elbow_yaw: 0.0, + MjcfStompy.right_arm.elbow_roll: 2.48, + MjcfStompy.right_arm.elbow_yaw: -0.09, + MjcfStompy.right_arm.shoulder_yaw: -1.6, + MjcfStompy.right_arm.shoulder_pitch: -1.64, + # left hand + MjcfStompy.left_arm.hand.hand_roll: 0.9, + # right hand + MjcfStompy.right_arm.hand.hand_roll: 0.64, + # left leg + MjcfStompy.legs.left.hip_roll: -0.52, + MjcfStompy.legs.left.hip_yaw: 0.6, + MjcfStompy.legs.left.hip_pitch: -0.8, + MjcfStompy.legs.right.hip_roll: 0.59, + MjcfStompy.legs.right.hip_yaw: 0.7, + MjcfStompy.legs.right.hip_pitch: 1.0, + # right leg + MjcfStompy.legs.left.knee: 0.2, + MjcfStompy.legs.right.knee: 0.0, + MjcfStompy.legs.left.ankle: 0.0, + MjcfStompy.legs.right.ankle: 0.0, + MjcfStompy.legs.left.foot_roll: 0.0, + MjcfStompy.legs.right.foot_roll: 0.0, + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + MjcfStompy.head.left_right: { + "lower": -3.14, + "upper": 3.14, + }, + MjcfStompy.right_arm.shoulder_yaw: { + "lower": -2.09, + "upper": 2.09, + }, + MjcfStompy.left_arm.shoulder_yaw: { + "lower": -2.09, + "upper": 2.09, + }, + MjcfStompy.right_arm.shoulder_pitch: { + "lower": -1.91, + "upper": 1.91, + }, + MjcfStompy.left_arm.shoulder_pitch: { + "lower": -1.91, + "upper": 1.91, + }, + MjcfStompy.legs.right.hip_roll: { + "lower": -1.04, + "upper": 1.04, + }, + MjcfStompy.legs.left.hip_roll: { + "lower": -1.04, + "upper": 1.04, + }, + MjcfStompy.legs.right.hip_yaw: { + "lower": -1.57, + "upper": 1.06, + }, + MjcfStompy.legs.left.hip_yaw: { + "lower": -1.57, + "upper": 1.06, + }, + MjcfStompy.legs.right.hip_pitch: { + "lower": -1.78, + "upper": 2.35, + }, + MjcfStompy.legs.left.hip_pitch: { + "lower": -1.78, + "upper": 2.35, + }, + MjcfStompy.legs.right.knee: { + "lower": 0, + "upper": 1.2, + }, + MjcfStompy.legs.left.knee: { + "lower": -1.2, + "upper": 0, + }, + MjcfStompy.legs.right.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + MjcfStompy.legs.left.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + MjcfStompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, + MjcfStompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, + } + + +def print_joints() -> None: + joints = Stompy.all_joints() + assert len(joints) == len(set(joints)), "Duplicate joint names found!" + print(Stompy()) + + +if __name__ == "__main__": + # python -m sim.stompy.joints + print_joints() From 4ab2f4d4a95c1f1c5604d4bfdf1c2ae1404fcde0 Mon Sep 17 00:00:00 2001 From: is2ac Date: Thu, 4 Jul 2024 10:57:19 -0700 Subject: [PATCH 02/21] stompy_v2 fixes --- sim/env.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/sim/env.py b/sim/env.py index e2745131..e0284c19 100755 --- a/sim/env.py +++ b/sim/env.py @@ -9,10 +9,13 @@ def model_dir() -> Path: - # return Path(os.environ.get("MODEL_DIR", "models")) - # return Path(os.environ.get("MODEL_DIR", "stompytherobot")) - # return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") - return Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") + # path = Path(os.environ.get("MODEL_DIR", "models")) + # path = Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") + # replace with your own file path + + path = Path("/path/to/stompy/folder") + + return path def run_dir() -> Path: From 815a9013d39fdf63f136d23649420f39f481bb9b Mon Sep 17 00:00:00 2001 From: is2ac Date: Fri, 5 Jul 2024 11:52:27 -0700 Subject: [PATCH 03/21] removing torques values the model doesn't fly away --- sim/humanoid_gym/envs/stompy_config.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 5c8a1b2d..b1e81e3a 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -74,7 +74,7 @@ class terrain(LeggedRobotCfg.terrain): restitution = 0.0 class noise: - add_noise = True + add_noise = False noise_level = 0.6 # scales other values class noise_scales: @@ -86,7 +86,7 @@ class noise_scales: height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): - pos = [0.0, 0.0, 1.05] + pos = [0.0, 0.0, 1.15] # THIS default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} @@ -95,7 +95,7 @@ class init_state(LeggedRobotCfg.init_state): default_joint_angles[joint] = default_positions[joint] class control(LeggedRobotCfg.control): - # PD Drive parameters: + # PD Drive parameters: THIS stiffness = { "x10": 200, "x8": 200, @@ -129,11 +129,11 @@ class sim(LeggedRobotCfg.sim): class physx(LeggedRobotCfg.sim.physx): num_threads = 12 - solver_type = 1 # 0: pgs, 1: tgs - num_position_iterations = 4 - num_velocity_iterations = 0 - contact_offset = 0.01 # [m] - rest_offset = -0.02 # [m] + solver_type = 1 # 0: pgs, 1: tgs THIS + num_position_iterations = 4 # THIS + num_velocity_iterations = 1 # THIS + contact_offset = 0.01 # [m] THIS + rest_offset = -0.02 # [m] THIS bounce_threshold_velocity = 0.1 # [m/s] max_depenetration_velocity = 1.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more @@ -142,12 +142,12 @@ class physx(LeggedRobotCfg.sim.physx): contact_collection = 2 class domain_rand: - randomize_friction = True + randomize_friction = False friction_range = [0.1, 2.0] - randomize_base_mass = True + randomize_base_mass = False added_mass_range = [-1.0, 1.0] - push_robots = True + push_robots = False push_interval_s = 4 max_push_vel_xy = 0.2 max_push_ang_vel = 0.4 From b0fc9e55f216ef7f155e94422a745fb8e0b94138 Mon Sep 17 00:00:00 2001 From: is2ac Date: Mon, 8 Jul 2024 22:02:17 -0700 Subject: [PATCH 04/21] updated standing position and joint limits --- sim/humanoid_gym/envs/stompy_config.py | 4 +- sim/stompy2/joints.py | 370 +++++++++++++++++-------- 2 files changed, 251 insertions(+), 123 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index b1e81e3a..5ec60424 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -130,8 +130,10 @@ class sim(LeggedRobotCfg.sim): class physx(LeggedRobotCfg.sim.physx): num_threads = 12 solver_type = 1 # 0: pgs, 1: tgs THIS + # num_position_iterations = 4 # THIS + # num_velocity_iterations = 1 # THIS num_position_iterations = 4 # THIS - num_velocity_iterations = 1 # THIS + num_velocity_iterations = 0 # THIS contact_offset = 0.01 # [m] THIS rest_offset = -0.02 # [m] THIS bounce_threshold_velocity = 0.1 # [m/s] diff --git a/sim/stompy2/joints.py b/sim/stompy2/joints.py index d3b522bb..8bfaf441 100755 --- a/sim/stompy2/joints.py +++ b/sim/stompy2/joints.py @@ -131,40 +131,40 @@ def default_positions(cls) -> Dict[str, float]: @classmethod def default_standing(cls) -> Dict[str, float]: return { - Stompy.torso.roll: 0.0, + Stompy.torso.roll: 2.58, # arms - Stompy.left_arm.shoulder_pitch: -0.126, - Stompy.left_arm.shoulder_yaw: 2.12, - Stompy.left_arm.shoulder_roll: 1.89, - Stompy.right_arm.shoulder_pitch: -1.13, - Stompy.right_arm.shoulder_yaw: 2.1, - Stompy.right_arm.shoulder_roll: -1.23, - Stompy.left_arm.elbow_pitch: 3.0, - Stompy.right_arm.elbow_pitch: 3.0, + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, # hands Stompy.left_arm.hand.left_finger: 0.0, Stompy.left_arm.hand.right_finger: 0.0, Stompy.right_arm.hand.left_finger: 0.0, Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: -0.346, - Stompy.left_arm.hand.wrist_pitch: -0.251, - Stompy.left_arm.hand.wrist_yaw: 0.377, - Stompy.right_arm.hand.wrist_roll: -3.14, - Stompy.right_arm.hand.wrist_pitch: -0.437, - Stompy.right_arm.hand.wrist_yaw: 0.911, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, # legs - Stompy.legs.left.hip_pitch: -1.55, - Stompy.legs.left.hip_roll: 1.46, - Stompy.legs.left.hip_yaw: 1.45, - Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.79, - Stompy.legs.right.hip_pitch: 1.55, - Stompy.legs.right.hip_roll: -1.67, - Stompy.legs.right.hip_yaw: 1.04, - Stompy.legs.right.knee_pitch: 2.01, - Stompy.legs.right.ankle_pitch: 0.44, - Stompy.legs.right.ankle_roll: 1.79, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod @@ -198,7 +198,7 @@ def default_sitting(cls) -> Dict[str, float]: Stompy.legs.left.knee_pitch: 2.17, Stompy.legs.left.ankle_pitch: 0.238, Stompy.legs.left.ankle_roll: 1.79, - Stompy.legs.right.hip_pitch: 1.55, + Stompy.legs.right.hip_pitch: -1.55, Stompy.legs.right.hip_roll: -1.67, Stompy.legs.right.hip_yaw: 1.04, Stompy.legs.right.knee_pitch: 2.01, @@ -206,6 +206,135 @@ def default_sitting(cls) -> Dict[str, float]: Stompy.legs.right.ankle_roll: 1.79, } + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + Stompy.torso.roll: { + "lower": -4.36332, + "upper": 4.36332, + }, + Stompy.left_arm.shoulder_pitch: { + "lower": -0.0, + "upper": 0.2, + }, + Stompy.left_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.3058009, + }, + Stompy.left_arm.shoulder_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.shoulder_pitch: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.3058009, + }, + Stompy.right_arm.shoulder_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": -3.66519, + "upper": -1.39626, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": 0, + "upper": 1.5708, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": -1.5708, + "upper": 0.523599, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -1.5708, + "upper": 0, + }, + Stompy.legs.left.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.left.hip_roll: { + "lower": -3.14159, + "upper": 0, + }, + Stompy.legs.left.hip_yaw: { + "lower": -1.0472, + "upper": 2.0944, + }, + Stompy.legs.left.knee_pitch: { + "lower": -4.18879, + "upper": 0, + }, + Stompy.legs.left.ankle_pitch: { + "lower": -1.5708, + "upper": 2.18166, + }, + Stompy.legs.left.ankle_roll: { + "lower": -2.26893, + "upper": -1.22173, + }, + Stompy.legs.right.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.right.hip_roll: { + "lower": 0, + "upper": 3.14159, + }, + Stompy.legs.right.hip_yaw: { + "lower": -1.0472, + "upper": 2.0944, + }, + Stompy.legs.right.knee_pitch: { + "lower": -4.18879, + "upper": 0, + }, + Stompy.legs.right.ankle_pitch: { + "lower": -1.5708, + "upper": 2.18166, + }, + Stompy.legs.right.ankle_roll: { + "lower": -2.26893, + "upper": -1.22173, + }, + Stompy.left_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + Stompy.right_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + Stompy.left_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.left_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, + Stompy.right_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.right_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, + } + class StompyFixed(Stompy): torso = Torso() @@ -216,171 +345,168 @@ class StompyFixed(Stompy): @classmethod def default_standing(cls) -> Dict[str, float]: return { - Stompy.torso.roll: 0.0, + Stompy.torso.roll: 2.58, # arms - Stompy.left_arm.shoulder_pitch: -0.126, - Stompy.left_arm.shoulder_yaw: 2.12, - Stompy.left_arm.shoulder_roll: 1.89, - Stompy.right_arm.shoulder_pitch: -1.13, - Stompy.right_arm.shoulder_yaw: 2.1, - Stompy.right_arm.shoulder_roll: -1.23, - Stompy.left_arm.elbow_pitch: 3.0, - Stompy.right_arm.elbow_pitch: 3.0, + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, # hands Stompy.left_arm.hand.left_finger: 0.0, Stompy.left_arm.hand.right_finger: 0.0, Stompy.right_arm.hand.left_finger: 0.0, Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: -0.346, - Stompy.left_arm.hand.wrist_pitch: -0.251, - Stompy.left_arm.hand.wrist_yaw: 0.377, - Stompy.right_arm.hand.wrist_roll: -3.14, - Stompy.right_arm.hand.wrist_pitch: -0.437, - Stompy.right_arm.hand.wrist_yaw: 0.911, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, # legs - Stompy.legs.left.hip_pitch: -1.55, - Stompy.legs.left.hip_roll: 1.46, - Stompy.legs.left.hip_yaw: 1.45, - Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.79, - Stompy.legs.right.hip_pitch: 1.55, - Stompy.legs.right.hip_roll: -1.67, - Stompy.legs.right.hip_yaw: 1.04, - Stompy.legs.right.knee_pitch: 2.01, - Stompy.legs.right.ankle_pitch: 0.44, - Stompy.legs.right.ankle_roll: 1.79, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - # arms Stompy.torso.roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 2.53, + "upper": 2.63, }, Stompy.left_arm.shoulder_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -0.584, + "upper": -0.484, }, Stompy.left_arm.shoulder_yaw: { - "lower": 0.97738438, - "upper": 5.30580090, + "lower": 2.49, + "upper": 2.59, }, Stompy.left_arm.shoulder_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -0.0814, + "upper": 0.0186, }, Stompy.right_arm.shoulder_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 2.40, + "upper": 2.50, }, Stompy.right_arm.shoulder_yaw: { - "lower": 0.97738438, - "upper": 5.3058009, + "lower": 3.72, + "upper": 3.82, }, Stompy.right_arm.shoulder_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -0.0814, + "upper": 0.0186, + }, + Stompy.left_arm.elbow_pitch: { + "lower": 2.30, + "upper": 2.40, + }, + Stompy.right_arm.elbow_pitch: { + "lower": 2.60, + "upper": 2.70, }, - # hands Stompy.left_arm.hand.left_finger: { - "lower": -0.051, - "upper": 0.0, + "lower": -0.05, + "upper": 0.05, }, Stompy.left_arm.hand.right_finger: { - "lower": 0, - "upper": 0.051, + "lower": -0.05, + "upper": 0.05, }, Stompy.right_arm.hand.left_finger: { - "lower": -0.051, - "upper": 0.0, + "lower": -0.05, + "upper": 0.05, }, Stompy.right_arm.hand.right_finger: { - "lower": 0, - "upper": 0.051, + "lower": -0.05, + "upper": 0.05, }, Stompy.left_arm.hand.wrist_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.74, + "upper": 1.84, }, Stompy.left_arm.hand.wrist_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.30, + "upper": 1.40, }, Stompy.left_arm.hand.wrist_yaw: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.02, + "upper": 1.12, }, Stompy.right_arm.hand.wrist_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -2.18, + "upper": -2.08, }, Stompy.right_arm.hand.wrist_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.74, + "upper": 1.84, }, Stompy.right_arm.hand.wrist_yaw: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -0.301, + "upper": -0.201, }, - Stompy.left_arm.elbow_pitch: { - "lower": 1.4486233, - "higher": 5.4454273, - }, - Stompy.right_arm.elbow_pitch: { - "lower": 1.4486233, - "higher": 5.4454273, - }, - # legs Stompy.legs.left.hip_pitch: { - "lower": -4.712389, - "upper": 4.712389, + "lower": -1.65, + "upper": -1.55, }, Stompy.legs.left.hip_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.36, + "upper": 1.46, }, Stompy.legs.left.hip_yaw: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -2.17, + "upper": -2.07, }, Stompy.legs.left.knee_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.96, + "upper": 2.06, }, Stompy.legs.left.ankle_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 0.188, + "upper": 0.288, }, Stompy.legs.left.ankle_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.80, + "upper": 1.90, }, Stompy.legs.right.hip_pitch: { - "lower": -4.712389, - "upper": 4.712389, + "lower": 1.71, + "upper": 1.81, }, Stompy.legs.right.hip_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": -1.59, + "upper": -1.49, }, Stompy.legs.right.hip_yaw: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 0.917, + "upper": 1.017, }, Stompy.legs.right.knee_pitch: { - "lower": -4.712389, - "upper": 4.712389, + "lower": 2.02, + "upper": 2.12, }, Stompy.legs.right.ankle_pitch: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 0.327, + "upper": 0.427, }, Stompy.legs.right.ankle_roll: { - "lower": -3.1415927, - "upper": 3.1415927, + "lower": 1.87, + "upper": 1.97, }, } From cb8592b86605c50cfe3ba81b9b263904b995db5c Mon Sep 17 00:00:00 2001 From: is2ac Date: Mon, 8 Jul 2024 22:06:59 -0700 Subject: [PATCH 05/21] initial params --- sim/humanoid_gym/envs/stompy_config.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 5ec60424..aa95a8ac 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -137,7 +137,8 @@ class physx(LeggedRobotCfg.sim.physx): contact_offset = 0.01 # [m] THIS rest_offset = -0.02 # [m] THIS bounce_threshold_velocity = 0.1 # [m/s] - max_depenetration_velocity = 1.0 + # max_depenetration_velocity = 1.0 + mx_depenetration_velocity = 0.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more default_buffer_size_multiplier = 5 # 0: never, 1: last sub-step, 2: all sub-steps (default=2) From 80b9289365933334cdf260082f879106ea6f6115 Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 13:21:33 -0700 Subject: [PATCH 06/21] cleanup --- sim/humanoid_gym/envs/stompy_config.py | 19 ++++++------------- sim/humanoid_gym/envs/stompy_env.py | 6 +++--- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index aa95a8ac..10f6776a 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -41,14 +41,9 @@ class asset(LeggedRobotCfg.asset): foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - # old - # foot_name = "_leg_1_x4_1_outer_1" # "foot" - # knee_name = "belt_knee" - termination_height = 0.23 default_feet_height = 0.0 terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] - # terminate_after_contacts_on = ["link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter @@ -74,7 +69,7 @@ class terrain(LeggedRobotCfg.terrain): restitution = 0.0 class noise: - add_noise = False + add_noise = True noise_level = 0.6 # scales other values class noise_scales: @@ -86,7 +81,7 @@ class noise_scales: height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): - pos = [0.0, 0.0, 1.15] # THIS + pos = [0.0, 0.0, 1.15] # THIS default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} @@ -96,6 +91,7 @@ class init_state(LeggedRobotCfg.init_state): class control(LeggedRobotCfg.control): # PD Drive parameters: THIS + # NOTE: these names are outdated, must be manually set in third party legged_robot.py stiffness = { "x10": 200, "x8": 200, @@ -130,15 +126,12 @@ class sim(LeggedRobotCfg.sim): class physx(LeggedRobotCfg.sim.physx): num_threads = 12 solver_type = 1 # 0: pgs, 1: tgs THIS - # num_position_iterations = 4 # THIS - # num_velocity_iterations = 1 # THIS num_position_iterations = 4 # THIS - num_velocity_iterations = 0 # THIS + num_velocity_iterations = 1 # THIS contact_offset = 0.01 # [m] THIS rest_offset = -0.02 # [m] THIS bounce_threshold_velocity = 0.1 # [m/s] - # max_depenetration_velocity = 1.0 - mx_depenetration_velocity = 0.0 + max_depenetration_velocity = 1.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more default_buffer_size_multiplier = 5 # 0: never, 1: last sub-step, 2: all sub-steps (default=2) @@ -150,7 +143,7 @@ class domain_rand: randomize_base_mass = False added_mass_range = [-1.0, 1.0] - push_robots = False + push_robots = True push_interval_s = 4 max_push_vel_xy = 0.2 max_push_ang_vel = 0.4 diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index dade8f79..f9502930 100755 --- a/sim/humanoid_gym/envs/stompy_env.py +++ b/sim/humanoid_gym/envs/stompy_env.py @@ -5,9 +5,9 @@ from humanoid.envs import LeggedRobot from humanoid.envs.base.legged_robot_config import LeggedRobotCfg from humanoid.utils.terrain import HumanoidTerrain -from isaacgym import gymtorch from isaacgym.torch_utils import * +from isaacgym import gymtorch from sim.stompy2.joints import Stompy @@ -78,7 +78,6 @@ def _push_robots(self): self.rand_push_torque = torch_rand_float( -max_push_angular, max_push_angular, (self.num_envs, 3), device=self.device ) - self.root_states[:, 10:13] = self.rand_push_torque self.gym.set_actor_root_state_tensor(self.sim, gymtorch.unwrap_tensor(self.root_states)) @@ -175,6 +174,7 @@ def _get_noise_scale_vec(self, cfg): num_actions = self.num_actions noise_vec = torch.zeros(self.cfg.env.num_single_obs, device=self.device) self.add_noise = self.cfg.noise.add_noise + # breakpoint() noise_scales = self.cfg.noise.noise_scales noise_vec[0:5] = 0.0 # commands noise_vec[5 : (num_actions + 5)] = noise_scales.dof_pos * self.obs_scales.dof_pos @@ -212,7 +212,7 @@ def compute_observations(self): dq = self.dof_vel * self.obs_scales.dof_vel diff = self.dof_pos - self.ref_dof_pos - + # breakpoint() self.privileged_obs_buf = torch.cat( ( self.command_input, # 2 + 3 From e52ffaf6022b556c9fff403ef4ebae84e9d36c7b Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 14:03:45 -0700 Subject: [PATCH 07/21] fighting mypy --- sim/humanoid_gym/envs/stompy_config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 10f6776a..ab75c221 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -1,9 +1,9 @@ """Defines the environment configuration for the Getting up task""" -from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO +from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO # type: ignore from sim.env import stompy_urdf_path -from sim.stompy2.joints import Stompy +from sim.stompy2.joints import Stompy # type: ignore[import] NUM_JOINTS = len(Stompy.all_joints()) # 33 @@ -45,7 +45,7 @@ class asset(LeggedRobotCfg.asset): default_feet_height = 0.0 terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] - penalize_contacts_on = [] + penalize_contacts_on = ["place_holder"] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter flip_visual_attachments = False replace_cylinder_with_capsule = False From c627bda97e30606dbca5141a87e7f069834275bc Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 14:07:57 -0700 Subject: [PATCH 08/21] linting --- sim/humanoid_gym/envs/stompy_config.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index ab75c221..db726f62 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -1,9 +1,12 @@ """Defines the environment configuration for the Getting up task""" -from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO # type: ignore +from humanoid.envs.base.legged_robot_config import ( # type: ignore + LeggedRobotCfg, + LeggedRobotCfgPPO, +) from sim.env import stompy_urdf_path -from sim.stompy2.joints import Stompy # type: ignore[import] +from sim.stompy2.joints import Stompy # type: ignore[import] NUM_JOINTS = len(Stompy.all_joints()) # 33 From 9a25cbff1931e1f3505f97da0e9dcd3406d124a3 Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 14:44:01 -0700 Subject: [PATCH 09/21] defeating mypy --- sim/humanoid_gym/envs/stompy_config.py | 2 +- sim/humanoid_gym/envs/stompy_env.py | 2 +- sim/scripts/simulate_urdf.py | 18 ++- sim/stompy2/__init__.py | 0 sim/stompy2/joints.py | 216 ++++++++++++++++--------- 5 files changed, 155 insertions(+), 83 deletions(-) create mode 100644 sim/stompy2/__init__.py diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index db726f62..3afe8ee4 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -6,7 +6,7 @@ ) from sim.env import stompy_urdf_path -from sim.stompy2.joints import Stompy # type: ignore[import] +from sim.stompy2.joints import Stompy NUM_JOINTS = len(Stompy.all_joints()) # 33 diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index f9502930..23c4fe58 100755 --- a/sim/humanoid_gym/envs/stompy_env.py +++ b/sim/humanoid_gym/envs/stompy_env.py @@ -1,7 +1,7 @@ # mypy: disable-error-code="valid-newtype" """Defines the environment for training the humanoid.""" -import torch +import torch # type: ignore[import] from humanoid.envs import LeggedRobot from humanoid.envs.base.legged_robot_config import LeggedRobotCfg from humanoid.utils.terrain import HumanoidTerrain diff --git a/sim/scripts/simulate_urdf.py b/sim/scripts/simulate_urdf.py index 697b3620..14b3c30a 100755 --- a/sim/scripts/simulate_urdf.py +++ b/sim/scripts/simulate_urdf.py @@ -26,7 +26,7 @@ Args = NewType("Args", Any) # Importing torch down here to avoid gymtorch issues. -import torch # noqa: E402 +import torch # noqa: E402 # type: ignore[import] # DRIVE_MODE = gymapi.DOF_MODE_EFFORT DRIVE_MODE = gymapi.DOF_MODE_POS @@ -77,9 +77,11 @@ def load_gym() -> GymParams: sim_params.physx.contact_collection = gymapi.CC_ALL_SUBSTEPS sim_params.physx.num_threads = args.num_threads - sim_params.physx.use_gpu = args.use_gpu + # sim_params.physx.use_gpu = args.use_gpu + # is2ac2 - disable GPU for now + sim_params.physx.use_gpu = False - # sim_params.use_gpu_pipeline = False + sim_params.use_gpu_pipeline = False # if args.use_gpu_pipeline: # warnings.warn("Forcing CPU pipeline.") @@ -115,14 +117,16 @@ def load_gym() -> GymParams: asset_options = gymapi.AssetOptions() asset_options.default_dof_drive_mode = DRIVE_MODE asset_options.collapse_fixed_joints = True - asset_options.disable_gravity = True - asset_options.fix_base_link = True + # is2ac2: disable gravity for now + # asset_options.disable_gravity = False + asset_options.disable_gravity = False + asset_options.fix_base_link = False asset_path = stompy_urdf_path() robot_asset = gym.load_urdf(sim, str(asset_path.parent), str(asset_path.name), asset_options) # Adds the robot to the environment. initial_pose = gymapi.Transform() - initial_pose.p = gymapi.Vec3(0.0, 5.0, 0.0) + initial_pose.p = gymapi.Vec3(0.0, 2.0, 0.0) initial_pose.r = gymapi.Quat(0.0, 0.0, 0.0, 1.0) robot = gym.create_actor(env, robot_asset, initial_pose, "robot") @@ -189,7 +193,7 @@ def run_gym(gym: GymParams, mode: Literal["one_at_a_time", "all_at_once"] = "all gym.gym.step_graphics(gym.sim) gym.gym.draw_viewer(gym.viewer, gym.sim, True) gym.gym.sync_frame_time(gym.sim) - + # breakpoint() # Print the joint forces. # print(gym.gym.get_actor_dof_forces(gym.env, gym.robot)) # print(gym.gym.get_env_rigid_contact_forces(gym.env)) diff --git a/sim/stompy2/__init__.py b/sim/stompy2/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sim/stompy2/joints.py b/sim/stompy2/joints.py index 8bfaf441..9f92fc98 100755 --- a/sim/stompy2/joints.py +++ b/sim/stompy2/joints.py @@ -531,101 +531,169 @@ class MjcfStompy(Stompy): @classmethod def default_standing(cls) -> Dict[str, float]: return { - MjcfStompy.head.left_right: 0.8526, + Stompy.torso.roll: 2.58, # arms - MjcfStompy.left_arm.shoulder_yaw: 1.57, - MjcfStompy.left_arm.shoulder_pitch: 1.59, - MjcfStompy.left_arm.elbow_roll: 0.0, - MjcfStompy.left_arm.elbow_yaw: 0.0, - MjcfStompy.right_arm.elbow_roll: 2.48, - MjcfStompy.right_arm.elbow_yaw: -0.09, - MjcfStompy.right_arm.shoulder_yaw: -1.6, - MjcfStompy.right_arm.shoulder_pitch: -1.64, - # left hand - MjcfStompy.left_arm.hand.hand_roll: 0.9, - # right hand - MjcfStompy.right_arm.hand.hand_roll: 0.64, - # left leg - MjcfStompy.legs.left.hip_roll: -0.52, - MjcfStompy.legs.left.hip_yaw: 0.6, - MjcfStompy.legs.left.hip_pitch: -0.8, - MjcfStompy.legs.right.hip_roll: 0.59, - MjcfStompy.legs.right.hip_yaw: 0.7, - MjcfStompy.legs.right.hip_pitch: 1.0, - # right leg - MjcfStompy.legs.left.knee: 0.2, - MjcfStompy.legs.right.knee: 0.0, - MjcfStompy.legs.left.ankle: 0.0, - MjcfStompy.legs.right.ankle: 0.0, - MjcfStompy.legs.left.foot_roll: 0.0, - MjcfStompy.legs.right.foot_roll: 0.0, + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, + # legs + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - MjcfStompy.head.left_right: { - "lower": -3.14, - "upper": 3.14, + Stompy.torso.roll: { + "lower": 2.53, + "upper": 2.63, }, - MjcfStompy.right_arm.shoulder_yaw: { - "lower": -2.09, - "upper": 2.09, + Stompy.left_arm.shoulder_pitch: { + "lower": -0.584, + "upper": -0.484, }, - MjcfStompy.left_arm.shoulder_yaw: { - "lower": -2.09, - "upper": 2.09, + Stompy.left_arm.shoulder_yaw: { + "lower": 2.49, + "upper": 2.59, }, - MjcfStompy.right_arm.shoulder_pitch: { - "lower": -1.91, - "upper": 1.91, + Stompy.left_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, - MjcfStompy.left_arm.shoulder_pitch: { - "lower": -1.91, - "upper": 1.91, + Stompy.right_arm.shoulder_pitch: { + "lower": 2.40, + "upper": 2.50, }, - MjcfStompy.legs.right.hip_roll: { - "lower": -1.04, - "upper": 1.04, + Stompy.right_arm.shoulder_yaw: { + "lower": 3.72, + "upper": 3.82, }, - MjcfStompy.legs.left.hip_roll: { - "lower": -1.04, - "upper": 1.04, + Stompy.right_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, - MjcfStompy.legs.right.hip_yaw: { - "lower": -1.57, - "upper": 1.06, + Stompy.left_arm.elbow_pitch: { + "lower": 2.30, + "upper": 2.40, }, - MjcfStompy.legs.left.hip_yaw: { - "lower": -1.57, - "upper": 1.06, + Stompy.right_arm.elbow_pitch: { + "lower": 2.60, + "upper": 2.70, }, - MjcfStompy.legs.right.hip_pitch: { - "lower": -1.78, - "upper": 2.35, + Stompy.left_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.left.hip_pitch: { - "lower": -1.78, - "upper": 2.35, + Stompy.left_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.right.knee: { - "lower": 0, - "upper": 1.2, + Stompy.right_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.left.knee: { - "lower": -1.2, - "upper": 0, + Stompy.right_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": 1.30, + "upper": 1.40, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": 1.02, + "upper": 1.12, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -2.18, + "upper": -2.08, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -0.301, + "upper": -0.201, + }, + Stompy.legs.left.hip_pitch: { + "lower": -1.65, + "upper": -1.55, + }, + Stompy.legs.left.hip_roll: { + "lower": 1.36, + "upper": 1.46, + }, + Stompy.legs.left.hip_yaw: { + "lower": -2.17, + "upper": -2.07, + }, + Stompy.legs.left.knee_pitch: { + "lower": 1.96, + "upper": 2.06, + }, + Stompy.legs.left.ankle_pitch: { + "lower": 0.188, + "upper": 0.288, + }, + Stompy.legs.left.ankle_roll: { + "lower": 1.80, + "upper": 1.90, + }, + Stompy.legs.right.hip_pitch: { + "lower": 1.71, + "upper": 1.81, }, - MjcfStompy.legs.right.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.hip_roll: { + "lower": -1.59, + "upper": -1.49, + }, + Stompy.legs.right.hip_yaw: { + "lower": 0.917, + "upper": 1.017, + }, + Stompy.legs.right.knee_pitch: { + "lower": 2.02, + "upper": 2.12, }, - MjcfStompy.legs.left.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.ankle_pitch: { + "lower": 0.327, + "upper": 0.427, + }, + Stompy.legs.right.ankle_roll: { + "lower": 1.87, + "upper": 1.97, }, - MjcfStompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, - MjcfStompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, } From bffbb58ea32779e8549452f3f2a3459bdf4a2a6f Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 17:04:44 -0700 Subject: [PATCH 10/21] fixes --- sim/env.py | 8 +- sim/humanoid_gym/envs/legs_env.py | 7 +- sim/humanoid_gym/envs/stompy_config.py | 2 +- sim/humanoid_gym/envs/stompy_env.py | 6 +- sim/scripts/simulate_urdf.py | 9 +- sim/stompy/__init__.py | 0 sim/stompy/joints.py | 730 +++++++++++++++++-------- 7 files changed, 512 insertions(+), 250 deletions(-) mode change 100755 => 100644 sim/stompy/__init__.py diff --git a/sim/env.py b/sim/env.py index e0284c19..daecc371 100755 --- a/sim/env.py +++ b/sim/env.py @@ -9,13 +9,7 @@ def model_dir() -> Path: - # path = Path(os.environ.get("MODEL_DIR", "models")) - # path = Path("/home/dpsh/isaacs_sim/sim/sim/stompy2") - # replace with your own file path - - path = Path("/path/to/stompy/folder") - - return path + return Path(os.environ.get("MODEL_DIR", "models")) def run_dir() -> Path: diff --git a/sim/humanoid_gym/envs/legs_env.py b/sim/humanoid_gym/envs/legs_env.py index e217c230..c3ce260e 100755 --- a/sim/humanoid_gym/envs/legs_env.py +++ b/sim/humanoid_gym/envs/legs_env.py @@ -1,14 +1,14 @@ # mypy: disable-error-code="valid-newtype" """Defines the environment for training the Stompy with fixed torso.""" -import torch +import torch # type: ignore[import] from humanoid.envs import LeggedRobot from humanoid.envs.base.legged_robot_config import LeggedRobotCfg from humanoid.utils.terrain import HumanoidTerrain -from isaacgym import gymtorch from isaacgym.torch_utils import * -from sim.stompy2.joints import StompyFixed +from isaacgym import gymtorch +from sim.old_stompy.joints import StompyFixed class LegsFreeEnv(LeggedRobot): @@ -302,7 +302,6 @@ def _reward_knee_distance(self): """ Calculates the reward based on the distance between the knee of the humanoid. """ - # breakpoint() foot_pos = self.rigid_state[:, self.knee_indices, :2] foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1) fd = self.cfg.rewards.min_dist diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 3afe8ee4..e70d5c54 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -6,7 +6,7 @@ ) from sim.env import stompy_urdf_path -from sim.stompy2.joints import Stompy +from sim.old_stompy.joints import Stompy NUM_JOINTS = len(Stompy.all_joints()) # 33 diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index 23c4fe58..96ce8649 100755 --- a/sim/humanoid_gym/envs/stompy_env.py +++ b/sim/humanoid_gym/envs/stompy_env.py @@ -8,7 +8,7 @@ from isaacgym.torch_utils import * from isaacgym import gymtorch -from sim.stompy2.joints import Stompy +from sim.old_stompy.joints import Stompy class StompyFreeEnv(LeggedRobot): @@ -56,7 +56,6 @@ def __init__(self, cfg: LeggedRobotCfg, sim_params, physics_engine, sim_device, self.legs_joints = {} for name, joint in Stompy.legs.left.joints_motors(): - print(name) joint_handle = self.gym.find_actor_dof_handle(env_handle, actor_handle, joint) self.legs_joints["left_" + name] = joint_handle @@ -174,7 +173,6 @@ def _get_noise_scale_vec(self, cfg): num_actions = self.num_actions noise_vec = torch.zeros(self.cfg.env.num_single_obs, device=self.device) self.add_noise = self.cfg.noise.add_noise - # breakpoint() noise_scales = self.cfg.noise.noise_scales noise_vec[0:5] = 0.0 # commands noise_vec[5 : (num_actions + 5)] = noise_scales.dof_pos * self.obs_scales.dof_pos @@ -212,7 +210,6 @@ def compute_observations(self): dq = self.dof_vel * self.obs_scales.dof_vel diff = self.dof_pos - self.ref_dof_pos - # breakpoint() self.privileged_obs_buf = torch.cat( ( self.command_input, # 2 + 3 @@ -389,7 +386,6 @@ def _reward_base_height(self): The reward is computed based on the height difference between the robot's base and the average height of its feet when they are in contact with the ground. """ - # breakpoint() stance_mask = self._get_gait_phase() measured_heights = torch.sum(self.rigid_state[:, self.feet_indices, 2] * stance_mask, dim=1) / torch.sum( stance_mask, dim=1 diff --git a/sim/scripts/simulate_urdf.py b/sim/scripts/simulate_urdf.py index 14b3c30a..495b6693 100755 --- a/sim/scripts/simulate_urdf.py +++ b/sim/scripts/simulate_urdf.py @@ -14,7 +14,7 @@ from isaacgym import gymapi, gymtorch, gymutil from sim.env import stompy_urdf_path from sim.logging import configure_logging -from sim.stompy2.joints import Stompy +from sim.old_stompy.joints import Stompy logger = logging.getLogger(__name__) @@ -77,9 +77,7 @@ def load_gym() -> GymParams: sim_params.physx.contact_collection = gymapi.CC_ALL_SUBSTEPS sim_params.physx.num_threads = args.num_threads - # sim_params.physx.use_gpu = args.use_gpu - # is2ac2 - disable GPU for now - sim_params.physx.use_gpu = False + sim_params.physx.use_gpu = args.use_gpu sim_params.use_gpu_pipeline = False # if args.use_gpu_pipeline: @@ -117,8 +115,6 @@ def load_gym() -> GymParams: asset_options = gymapi.AssetOptions() asset_options.default_dof_drive_mode = DRIVE_MODE asset_options.collapse_fixed_joints = True - # is2ac2: disable gravity for now - # asset_options.disable_gravity = False asset_options.disable_gravity = False asset_options.fix_base_link = False asset_path = stompy_urdf_path() @@ -193,7 +189,6 @@ def run_gym(gym: GymParams, mode: Literal["one_at_a_time", "all_at_once"] = "all gym.gym.step_graphics(gym.sim) gym.gym.draw_viewer(gym.viewer, gym.sim, True) gym.gym.sync_frame_time(gym.sim) - # breakpoint() # Print the joint forces. # print(gym.gym.get_actor_dof_forces(gym.env, gym.robot)) # print(gym.gym.get_env_rigid_contact_forces(gym.env)) diff --git a/sim/stompy/__init__.py b/sim/stompy/__init__.py old mode 100755 new mode 100644 diff --git a/sim/stompy/joints.py b/sim/stompy/joints.py index a87e9be7..9f92fc98 100755 --- a/sim/stompy/joints.py +++ b/sim/stompy/joints.py @@ -54,67 +54,58 @@ def __str__(self) -> str: return f"[{self.__class__.__name__}]{parts_str}" -class Head(Node): - left_right = "joint_head_1_x4_1_dof_x4" - up_down = "joint_head_1_x4_2_dof_x4" - - class Torso(Node): - pitch = "joint_torso_1_x8_1_dof_x8" + roll = "torso roll" class LeftHand(Node): - hand_roll = "joint_left_arm_2_hand_1_x4_1_dof_x4" - hand_grip = "joint_left_arm_2_hand_1_x4_2_dof_x4" - slider_a = "joint_left_arm_2_hand_1_slider_1" - slider_b = "joint_left_arm_2_hand_1_slider_2" + wrist_roll = "wrist roll" + wrist_pitch = "wrist pitch" + wrist_yaw = "wrist yaw" + left_finger = "left hand left finger" + right_finger = "left hand right finger" class LeftArm(Node): - shoulder_yaw = "joint_left_arm_2_x8_1_dof_x8" - shoulder_pitch = "joint_left_arm_2_x8_2_dof_x8" - shoulder_roll = "joint_left_arm_2_x6_1_dof_x6" - elbow_yaw = "joint_left_arm_2_x6_2_dof_x6" - elbow_roll = "joint_left_arm_2_x4_1_dof_x4" + shoulder_yaw = "shoulder yaw" + shoulder_pitch = "shoulder pitch" + shoulder_roll = "shoulder roll" + elbow_pitch = "elbow pitch" hand = LeftHand() class RightHand(Node): - hand_roll = "joint_right_arm_1_hand_1_x4_1_dof_x4" - hand_grip = "joint_right_arm_1_hand_1_x4_2_dof_x4" - slider_a = "joint_right_arm_1_hand_1_slider_1" - slider_b = "joint_right_arm_1_hand_1_slider_2" + wrist_roll = "right wrist roll" + wrist_pitch = "right wrist pitch" + wrist_yaw = "right wrist yaw" + left_finger = "right hand left finger" + right_finger = "right hand right finger" class RightArm(Node): - shoulder_yaw = "joint_right_arm_1_x8_1_dof_x8" - shoulder_pitch = "joint_right_arm_1_x8_2_dof_x8" - shoulder_roll = "joint_right_arm_1_x6_1_dof_x6" - elbow_yaw = "joint_right_arm_1_x6_2_dof_x6" - elbow_roll = "joint_right_arm_1_x4_1_dof_x4" + shoulder_yaw = "right shoulder yaw" + shoulder_pitch = "right shoulder pitch" + shoulder_roll = "right shoulder roll" + elbow_pitch = "right elbow pitch" hand = RightHand() class LeftLeg(Node): - hip_roll = "joint_legs_1_x8_2_dof_x8" - hip_yaw = "joint_legs_1_left_leg_1_x8_1_dof_x8" - hip_pitch = "joint_legs_1_left_leg_1_x10_1_dof_x10" - knee_motor = "joint_legs_1_left_leg_1_x10_2_dof_x10" - knee = "joint_legs_1_left_leg_1_knee_revolute" - ankle_motor = "joint_legs_1_left_leg_1_x6_1_dof_x6" - ankle = "joint_legs_1_left_leg_1_ankle_revolute" - foot_roll = "joint_legs_1_left_leg_1_x4_1_dof_x4" + hip_roll = "left hip roll" + hip_yaw = "left hip yaw" + hip_pitch = "left hip pitch" + knee_pitch = "left knee pitch" + ankle_pitch = "left ankle pitch" + ankle_roll = "left ankle roll" class RightLeg(Node): - hip_roll = "joint_legs_1_x8_1_dof_x8" - hip_yaw = "joint_legs_1_right_leg_1_x8_1_dof_x8" - hip_pitch = "joint_legs_1_right_leg_1_x10_2_dof_x10" - knee_motor = "joint_legs_1_right_leg_1_x10_1_dof_x10" - knee = "joint_legs_1_right_leg_1_knee_revolute" - ankle_motor = "joint_legs_1_right_leg_1_x6_1_dof_x6" - ankle = "joint_legs_1_right_leg_1_ankle_revolute" - foot_roll = "joint_legs_1_right_leg_1_x4_1_dof_x4" + hip_roll = "right hip roll" + hip_yaw = "right hip yaw" + hip_pitch = "right hip pitch" + knee_pitch = "right knee pitch" + ankle_pitch = "right ankle pitch" + ankle_roll = "right ankle roll" class Legs(Node): @@ -123,7 +114,6 @@ class Legs(Node): class Stompy(Node): - head = Head() torso = Torso() left_arm = LeftArm() right_arm = RightArm() @@ -132,9 +122,7 @@ class Stompy(Node): @classmethod def default_positions(cls) -> Dict[str, float]: return { - Stompy.head.left_right: np.deg2rad(-54), - Stompy.head.up_down: 0.0, - Stompy.torso.pitch: 0.0, + Stompy.torso.roll: 0.0, Stompy.left_arm.shoulder_yaw: np.deg2rad(60), Stompy.left_arm.shoulder_pitch: np.deg2rad(60), Stompy.right_arm.shoulder_yaw: np.deg2rad(-60), @@ -143,60 +131,212 @@ def default_positions(cls) -> Dict[str, float]: @classmethod def default_standing(cls) -> Dict[str, float]: return { - Stompy.head.left_right: np.deg2rad(-2), # -0.03 + Stompy.torso.roll: 2.58, # arms - Stompy.left_arm.shoulder_yaw: np.deg2rad(-69.5), # -1.21 - Stompy.left_arm.shoulder_pitch: np.deg2rad(-93), # 1.61 - Stompy.right_arm.shoulder_yaw: np.deg2rad(85), # 1.48 - Stompy.right_arm.shoulder_pitch: np.deg2rad(104), # 1.81 + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, # legs - Stompy.legs.left.hip_roll: np.deg2rad(29), # 0.5 - Stompy.legs.left.hip_yaw: np.deg2rad(-29), # -0.5 - Stompy.legs.left.hip_pitch: np.deg2rad(56), # 0.97 - Stompy.legs.right.hip_roll: np.deg2rad(-29), # -0.5 - Stompy.legs.right.hip_yaw: np.deg2rad(-29), # -0.5 - Stompy.legs.right.hip_pitch: np.deg2rad(-56), # -0.97 - Stompy.legs.left.knee: np.deg2rad(-6), # -0.1 - Stompy.legs.right.knee: np.deg2rad(6), # 0.1 - Stompy.legs.left.ankle: np.deg2rad(0), # 0 - Stompy.legs.right.ankle: np.deg2rad(0), # 0 - Stompy.legs.left.foot_roll: np.deg2rad(0), # 0 - Stompy.legs.right.foot_roll: np.deg2rad(0), # 0 + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod def default_sitting(cls) -> Dict[str, float]: return { - Stompy.head.left_right: np.deg2rad(-3), - Stompy.head.up_down: 0.0, - Stompy.torso.pitch: 0.0, + Stompy.torso.roll: 0.0, # arms - Stompy.left_arm.shoulder_yaw: np.deg2rad(-88), - Stompy.left_arm.shoulder_pitch: np.deg2rad(-30), - Stompy.left_arm.shoulder_roll: np.deg2rad(-190), - Stompy.left_arm.elbow_yaw: np.deg2rad(-88), - Stompy.right_arm.shoulder_yaw: np.deg2rad(88), - Stompy.right_arm.shoulder_pitch: np.deg2rad(30), - Stompy.right_arm.shoulder_roll: np.deg2rad(190), - Stompy.right_arm.elbow_yaw: np.deg2rad(88), + Stompy.left_arm.shoulder_pitch: -0.126, + Stompy.left_arm.shoulder_yaw: 2.12, + Stompy.left_arm.shoulder_roll: 1.89, + Stompy.right_arm.shoulder_pitch: -1.13, + Stompy.right_arm.shoulder_yaw: 2.1, + Stompy.right_arm.shoulder_roll: -1.23, + Stompy.left_arm.elbow_pitch: 3.0, + Stompy.right_arm.elbow_pitch: 3.0, # hands - Stompy.left_arm.hand.hand_roll: np.deg2rad(-60), - Stompy.right_arm.hand.hand_roll: np.deg2rad(-60), + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: -0.346, + Stompy.left_arm.hand.wrist_pitch: -0.251, + Stompy.left_arm.hand.wrist_yaw: 0.377, + Stompy.right_arm.hand.wrist_roll: -3.14, + Stompy.right_arm.hand.wrist_pitch: -0.437, + Stompy.right_arm.hand.wrist_yaw: 0.911, # legs - Stompy.legs.left.hip_roll: np.deg2rad(29), - Stompy.legs.left.hip_yaw: np.deg2rad(-29), - Stompy.legs.left.hip_pitch: np.deg2rad(56), - Stompy.legs.right.hip_roll: np.deg2rad(-29), - Stompy.legs.right.hip_yaw: np.deg2rad(-29), - # check this - Stompy.legs.right.hip_pitch: np.deg2rad(-56), - Stompy.legs.left.knee: np.deg2rad(-6), - Stompy.legs.right.knee: np.deg2rad(6), + Stompy.legs.left.hip_pitch: -1.55, + Stompy.legs.left.hip_roll: 1.46, + Stompy.legs.left.hip_yaw: 1.45, + Stompy.legs.left.knee_pitch: 2.17, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.79, + Stompy.legs.right.hip_pitch: -1.55, + Stompy.legs.right.hip_roll: -1.67, + Stompy.legs.right.hip_yaw: 1.04, + Stompy.legs.right.knee_pitch: 2.01, + Stompy.legs.right.ankle_pitch: 0.44, + Stompy.legs.right.ankle_roll: 1.79, + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + Stompy.torso.roll: { + "lower": -4.36332, + "upper": 4.36332, + }, + Stompy.left_arm.shoulder_pitch: { + "lower": -0.0, + "upper": 0.2, + }, + Stompy.left_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.3058009, + }, + Stompy.left_arm.shoulder_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.shoulder_pitch: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.shoulder_yaw: { + "lower": 0.97738438, + "upper": 5.3058009, + }, + Stompy.right_arm.shoulder_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": -3.66519, + "upper": -1.39626, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": 0, + "upper": 1.5708, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -4.71239, + "upper": 4.71239, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": -1.5708, + "upper": 0.523599, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -1.5708, + "upper": 0, + }, + Stompy.legs.left.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.left.hip_roll: { + "lower": -3.14159, + "upper": 0, + }, + Stompy.legs.left.hip_yaw: { + "lower": -1.0472, + "upper": 2.0944, + }, + Stompy.legs.left.knee_pitch: { + "lower": -4.18879, + "upper": 0, + }, + Stompy.legs.left.ankle_pitch: { + "lower": -1.5708, + "upper": 2.18166, + }, + Stompy.legs.left.ankle_roll: { + "lower": -2.26893, + "upper": -1.22173, + }, + Stompy.legs.right.hip_pitch: { + "lower": -4.712389, + "upper": 4.712389, + }, + Stompy.legs.right.hip_roll: { + "lower": 0, + "upper": 3.14159, + }, + Stompy.legs.right.hip_yaw: { + "lower": -1.0472, + "upper": 2.0944, + }, + Stompy.legs.right.knee_pitch: { + "lower": -4.18879, + "upper": 0, + }, + Stompy.legs.right.ankle_pitch: { + "lower": -1.5708, + "upper": 2.18166, + }, + Stompy.legs.right.ankle_roll: { + "lower": -2.26893, + "upper": -1.22173, + }, + Stompy.left_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + Stompy.right_arm.elbow_pitch: { + "lower": 1.4486233, + "higher": 5.4454273, + }, + Stompy.left_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.left_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, + Stompy.right_arm.hand.left_finger: { + "lower": -0.051, + "upper": 0.0, + }, + Stompy.right_arm.hand.right_finger: { + "lower": 0, + "upper": 0.051, + }, } class StompyFixed(Stompy): - head = Head() torso = Torso() left_arm = LeftArm() right_arm = RightArm() @@ -205,94 +345,171 @@ class StompyFixed(Stompy): @classmethod def default_standing(cls) -> Dict[str, float]: return { - Stompy.head.left_right: np.deg2rad(-2), # -0.03 + Stompy.torso.roll: 2.58, # arms - Stompy.left_arm.shoulder_yaw: np.deg2rad(-69.5), # -1.21 - Stompy.left_arm.shoulder_pitch: np.deg2rad(-93), # 1.61 - Stompy.right_arm.shoulder_yaw: np.deg2rad(85), # 1.48 - Stompy.right_arm.shoulder_pitch: np.deg2rad(104), # 1.81 + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, # legs - Stompy.legs.left.hip_roll: np.deg2rad(29), # 0.5 - Stompy.legs.left.hip_yaw: np.deg2rad(-29), # -0.5 - Stompy.legs.left.hip_pitch: np.deg2rad(56), # 0.97 - Stompy.legs.right.hip_roll: np.deg2rad(-29), # -0.5 - Stompy.legs.right.hip_yaw: np.deg2rad(-29), # -0.5 - Stompy.legs.right.hip_pitch: np.deg2rad(-56), # -0.97 - Stompy.legs.left.knee: np.deg2rad(-6), # -0.1 - Stompy.legs.right.knee: np.deg2rad(6), # 0.1 - Stompy.legs.left.ankle: np.deg2rad(0), # 0 - Stompy.legs.right.ankle: np.deg2rad(0), # 0 - Stompy.legs.left.foot_roll: np.deg2rad(0), # 0 - Stompy.legs.right.foot_roll: np.deg2rad(0), # 0 + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - Stompy.head.left_right: { - "lower": -0.1, - "upper": 0.0, + Stompy.torso.roll: { + "lower": 2.53, + "upper": 2.63, }, - Stompy.right_arm.shoulder_yaw: { - "lower": 1.47, - "upper": 1.50, + Stompy.left_arm.shoulder_pitch: { + "lower": -0.584, + "upper": -0.484, }, Stompy.left_arm.shoulder_yaw: { - "lower": -1.23, - "upper": -1.20, + "lower": 2.49, + "upper": 2.59, + }, + Stompy.left_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, Stompy.right_arm.shoulder_pitch: { - "lower": 1.8, - "upper": 1.83, + "lower": 2.40, + "upper": 2.50, }, - Stompy.left_arm.shoulder_pitch: { - "lower": -1.63, - "upper": -1.60, + Stompy.right_arm.shoulder_yaw: { + "lower": 3.72, + "upper": 3.82, }, - Stompy.legs.right.hip_roll: { - "lower": -0.75, - "upper": -0.15, + Stompy.right_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, - Stompy.legs.left.hip_roll: { - "lower": 0.2, - "upper": 0.75, + Stompy.left_arm.elbow_pitch: { + "lower": 2.30, + "upper": 2.40, }, - Stompy.legs.right.hip_yaw: { - "lower": -1, - "upper": 0.0, + Stompy.right_arm.elbow_pitch: { + "lower": 2.60, + "upper": 2.70, + }, + Stompy.left_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.left_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.right_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.right_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": 1.30, + "upper": 1.40, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": 1.02, + "upper": 1.12, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -2.18, + "upper": -2.08, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -0.301, + "upper": -0.201, + }, + Stompy.legs.left.hip_pitch: { + "lower": -1.65, + "upper": -1.55, + }, + Stompy.legs.left.hip_roll: { + "lower": 1.36, + "upper": 1.46, }, Stompy.legs.left.hip_yaw: { - "lower": -0.71, - "upper": -0.3, + "lower": -2.17, + "upper": -2.07, + }, + Stompy.legs.left.knee_pitch: { + "lower": 1.96, + "upper": 2.06, + }, + Stompy.legs.left.ankle_pitch: { + "lower": 0.188, + "upper": 0.288, + }, + Stompy.legs.left.ankle_roll: { + "lower": 1.80, + "upper": 1.90, }, Stompy.legs.right.hip_pitch: { - "lower": -1.1, - "upper": -0.4, + "lower": 1.71, + "upper": 1.81, }, - Stompy.legs.left.hip_pitch: { - "lower": -0.1, - "upper": 1.2, + Stompy.legs.right.hip_roll: { + "lower": -1.59, + "upper": -1.49, }, - Stompy.legs.right.knee: { - "lower": 0, - "upper": 1.2, + Stompy.legs.right.hip_yaw: { + "lower": 0.917, + "upper": 1.017, }, - Stompy.legs.left.knee: { - "lower": -1.2, - "upper": 0, + Stompy.legs.right.knee_pitch: { + "lower": 2.02, + "upper": 2.12, }, - Stompy.legs.right.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.ankle_pitch: { + "lower": 0.327, + "upper": 0.427, }, - Stompy.legs.left.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.ankle_roll: { + "lower": 1.87, + "upper": 1.97, }, - Stompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, - Stompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, } - + class ImuTorso(Torso): pitch = "torso_1_x8_1_dof_x8" @@ -305,7 +522,6 @@ class ImuTorso(Torso): class MjcfStompy(Stompy): - head = Head() torso = ImuTorso() left_arm = LeftArm() right_arm = RightArm() @@ -315,107 +531,169 @@ class MjcfStompy(Stompy): @classmethod def default_standing(cls) -> Dict[str, float]: return { - MjcfStompy.head.left_right: 0.8526, + Stompy.torso.roll: 2.58, # arms - MjcfStompy.left_arm.shoulder_yaw: 1.57, - MjcfStompy.left_arm.shoulder_pitch: 1.59, - MjcfStompy.left_arm.elbow_roll: 0.0, - MjcfStompy.left_arm.elbow_yaw: 0.0, - - MjcfStompy.right_arm.elbow_roll: 2.48, - MjcfStompy.right_arm.elbow_yaw: -0.09, - - MjcfStompy.right_arm.shoulder_yaw: -1.6, - MjcfStompy.right_arm.shoulder_pitch: -1.64, - - # left hand - MjcfStompy.left_arm.hand.hand_roll: 0.9, - - # right hand - MjcfStompy.right_arm.hand.hand_roll: 0.64, - - # left leg - MjcfStompy.legs.left.hip_roll: -0.52, - MjcfStompy.legs.left.hip_yaw: 0.6, - MjcfStompy.legs.left.hip_pitch: -0.8, - MjcfStompy.legs.right.hip_roll: 0.59, - MjcfStompy.legs.right.hip_yaw: 0.7, - MjcfStompy.legs.right.hip_pitch: 1.0, - - # right leg - MjcfStompy.legs.left.knee: 0.2, - MjcfStompy.legs.right.knee: 0.0, - MjcfStompy.legs.left.ankle: 0.0, - MjcfStompy.legs.right.ankle: 0.0, - MjcfStompy.legs.left.foot_roll: 0.0, - MjcfStompy.legs.right.foot_roll: 0.0, + Stompy.left_arm.shoulder_pitch: -0.534, + Stompy.left_arm.shoulder_yaw: 2.54, + Stompy.left_arm.shoulder_roll: -0.0314, + Stompy.right_arm.shoulder_pitch: 2.45, + Stompy.right_arm.shoulder_yaw: 3.77, + Stompy.right_arm.shoulder_roll: -0.0314, + Stompy.left_arm.elbow_pitch: 2.35, + Stompy.right_arm.elbow_pitch: 2.65, + # hands + Stompy.left_arm.hand.left_finger: 0.0, + Stompy.left_arm.hand.right_finger: 0.0, + Stompy.right_arm.hand.left_finger: 0.0, + Stompy.right_arm.hand.right_finger: 0.0, + Stompy.left_arm.hand.wrist_roll: 1.79, + Stompy.left_arm.hand.wrist_pitch: 1.35, + Stompy.left_arm.hand.wrist_yaw: 1.07, + Stompy.right_arm.hand.wrist_roll: -2.13, + Stompy.right_arm.hand.wrist_pitch: 1.79, + Stompy.right_arm.hand.wrist_yaw: -0.251, + # legs + Stompy.legs.left.hip_pitch: -1.6, + Stompy.legs.left.hip_roll: 1.41, + Stompy.legs.left.hip_yaw: -2.12, + Stompy.legs.left.knee_pitch: 2.01, + Stompy.legs.left.ankle_pitch: 0.238, + Stompy.legs.left.ankle_roll: 1.85, + Stompy.legs.right.hip_pitch: 1.76, + Stompy.legs.right.hip_roll: -1.54, + Stompy.legs.right.hip_yaw: 0.967, + Stompy.legs.right.knee_pitch: 2.07, + Stompy.legs.right.ankle_pitch: 0.377, + Stompy.legs.right.ankle_roll: 1.92, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - MjcfStompy.head.left_right: { - "lower": -3.14, - "upper": 3.14, + Stompy.torso.roll: { + "lower": 2.53, + "upper": 2.63, }, - MjcfStompy.right_arm.shoulder_yaw: { - "lower": -2.09, - "upper": 2.09, + Stompy.left_arm.shoulder_pitch: { + "lower": -0.584, + "upper": -0.484, }, - MjcfStompy.left_arm.shoulder_yaw: { - "lower": -2.09, - "upper": 2.09, + Stompy.left_arm.shoulder_yaw: { + "lower": 2.49, + "upper": 2.59, }, - MjcfStompy.right_arm.shoulder_pitch: { - "lower": -1.91, - "upper": 1.91, + Stompy.left_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, - MjcfStompy.left_arm.shoulder_pitch: { - "lower": -1.91, - "upper": 1.91, + Stompy.right_arm.shoulder_pitch: { + "lower": 2.40, + "upper": 2.50, }, - MjcfStompy.legs.right.hip_roll: { - "lower": -1.04, - "upper": 1.04, + Stompy.right_arm.shoulder_yaw: { + "lower": 3.72, + "upper": 3.82, }, - MjcfStompy.legs.left.hip_roll: { - "lower": -1.04, - "upper": 1.04, + Stompy.right_arm.shoulder_roll: { + "lower": -0.0814, + "upper": 0.0186, }, - MjcfStompy.legs.right.hip_yaw: { - "lower": -1.57, - "upper": 1.06, + Stompy.left_arm.elbow_pitch: { + "lower": 2.30, + "upper": 2.40, }, - MjcfStompy.legs.left.hip_yaw: { - "lower": -1.57, - "upper": 1.06, + Stompy.right_arm.elbow_pitch: { + "lower": 2.60, + "upper": 2.70, }, - MjcfStompy.legs.right.hip_pitch: { - "lower": -1.78, - "upper": 2.35, + Stompy.left_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.left.hip_pitch: { - "lower": -1.78, - "upper": 2.35, + Stompy.left_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.right.knee: { - "lower": 0, - "upper": 1.2, + Stompy.right_arm.hand.left_finger: { + "lower": -0.05, + "upper": 0.05, }, - MjcfStompy.legs.left.knee: { - "lower": -1.2, - "upper": 0, + Stompy.right_arm.hand.right_finger: { + "lower": -0.05, + "upper": 0.05, + }, + Stompy.left_arm.hand.wrist_roll: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.left_arm.hand.wrist_pitch: { + "lower": 1.30, + "upper": 1.40, + }, + Stompy.left_arm.hand.wrist_yaw: { + "lower": 1.02, + "upper": 1.12, + }, + Stompy.right_arm.hand.wrist_roll: { + "lower": -2.18, + "upper": -2.08, + }, + Stompy.right_arm.hand.wrist_pitch: { + "lower": 1.74, + "upper": 1.84, + }, + Stompy.right_arm.hand.wrist_yaw: { + "lower": -0.301, + "upper": -0.201, + }, + Stompy.legs.left.hip_pitch: { + "lower": -1.65, + "upper": -1.55, + }, + Stompy.legs.left.hip_roll: { + "lower": 1.36, + "upper": 1.46, + }, + Stompy.legs.left.hip_yaw: { + "lower": -2.17, + "upper": -2.07, + }, + Stompy.legs.left.knee_pitch: { + "lower": 1.96, + "upper": 2.06, + }, + Stompy.legs.left.ankle_pitch: { + "lower": 0.188, + "upper": 0.288, + }, + Stompy.legs.left.ankle_roll: { + "lower": 1.80, + "upper": 1.90, + }, + Stompy.legs.right.hip_pitch: { + "lower": 1.71, + "upper": 1.81, + }, + Stompy.legs.right.hip_roll: { + "lower": -1.59, + "upper": -1.49, + }, + Stompy.legs.right.hip_yaw: { + "lower": 0.917, + "upper": 1.017, + }, + Stompy.legs.right.knee_pitch: { + "lower": 2.02, + "upper": 2.12, }, - MjcfStompy.legs.right.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.ankle_pitch: { + "lower": 0.327, + "upper": 0.427, }, - MjcfStompy.legs.left.ankle: { - "lower": -0.3, - "upper": 0.3, + Stompy.legs.right.ankle_roll: { + "lower": 1.87, + "upper": 1.97, }, - MjcfStompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, - MjcfStompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, } From 5098bf1dae47accb00bb39468bade66106784efc Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 17:37:04 -0700 Subject: [PATCH 11/21] final fixes --- sim/humanoid_gym/envs/stompy_config.py | 54 ++++++++++++++------------ 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index e70d5c54..44de5332 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -6,7 +6,7 @@ ) from sim.env import stompy_urdf_path -from sim.old_stompy.joints import Stompy +from sim.stompy.joints import Stompy NUM_JOINTS = len(Stompy.all_joints()) # 33 @@ -48,7 +48,7 @@ class asset(LeggedRobotCfg.asset): default_feet_height = 0.0 terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] - penalize_contacts_on = ["place_holder"] + penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter flip_visual_attachments = False replace_cylinder_with_capsule = False @@ -84,7 +84,7 @@ class noise_scales: height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): - pos = [0.0, 0.0, 1.15] # THIS + pos = [0.0, 0.0, 1.15] default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} @@ -93,29 +93,33 @@ class init_state(LeggedRobotCfg.init_state): default_joint_angles[joint] = default_positions[joint] class control(LeggedRobotCfg.control): - # PD Drive parameters: THIS + # PD Drive parameters: # NOTE: these names are outdated, must be manually set in third party legged_robot.py stiffness = { - "x10": 200, - "x8": 200, - "x6": 200, - "x4": 200, - "foot": 200, - "forward": 200, - "knee": 200, + "shoulder": 200, + "elbow": 200, + "wrist": 200, + "hand": 200, + "torso": 200, + "hip": 200, "ankle": 200, + "knee": 200, } damping = { - "x10": 10, - "x8": 10, - "x6": 10, - "x4": 10, - "foot": 10, - "forward": 10, - "knee": 10, + "shoulder": 10, + "elbow": 10, + "wrist": 10, + "hand": 10, + "torso": 10, + "hip": 10, "ankle": 10, + "knee": 10, } - + # ['right shoulder pitch', 'right shoulder yaw', 'right shoulder roll', 'right elbow pitch', 'right wrist roll', 'right wrist pitch', 'right wrist yaw', + # 'right hand left finger', 'right hand right finger', 'shoulder pitch', 'shoulder yaw', 'shoulder roll', 'elbow pitch', 'wrist roll', 'wrist pitch', 'wrist yaw', + # 'left hand left finger', 'left hand right finger', 'torso roll', 'left hip pitch', 'left hip yaw', 'left hip roll', + # 'left knee pitch', 'left ankle pitch', 'left ankle roll', 'right hip pitch', 'right hip yaw', 'right hip roll', 'right knee pitch', + # 'right ankle pitch', 'right ankle roll'] # action scale: target angle = actionScale * action + defaultAngle action_scale = 0.25 # decimation: Number of control action updates @ sim DT per policy DT @@ -128,11 +132,11 @@ class sim(LeggedRobotCfg.sim): class physx(LeggedRobotCfg.sim.physx): num_threads = 12 - solver_type = 1 # 0: pgs, 1: tgs THIS - num_position_iterations = 4 # THIS - num_velocity_iterations = 1 # THIS - contact_offset = 0.01 # [m] THIS - rest_offset = -0.02 # [m] THIS + solver_type = 1 # 0: pgs, 1: tgs + num_position_iterations = 4 + num_velocity_iterations = 1 + contact_offset = 0.01 # [m] + rest_offset = -0.02 # [m] bounce_threshold_velocity = 0.1 # [m/s] max_depenetration_velocity = 1.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more @@ -141,7 +145,7 @@ class physx(LeggedRobotCfg.sim.physx): contact_collection = 2 class domain_rand: - randomize_friction = False + randomize_friction = True friction_range = [0.1, 2.0] randomize_base_mass = False From 635a12c18d78722491e9a106121687579972432a Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 18:00:44 -0700 Subject: [PATCH 12/21] more fixes --- sim/deploy/run.py | 4 +- sim/humanoid_gym/envs/getup_config.py | 8 +- sim/humanoid_gym/envs/getup_env.py | 4 +- sim/humanoid_gym/envs/stompy_config.py | 9 +- sim/scripts/create_fixed_torso.py | 13 +- sim/stompy2/__init__.py | 0 sim/stompy2/joints.py | 708 ------------------------- 7 files changed, 19 insertions(+), 727 deletions(-) delete mode 100644 sim/stompy2/__init__.py delete mode 100755 sim/stompy2/joints.py diff --git a/sim/deploy/run.py b/sim/deploy/run.py index bd34f524..18b79792 100755 --- a/sim/deploy/run.py +++ b/sim/deploy/run.py @@ -14,14 +14,14 @@ import numpy as np from humanoid.envs import * from humanoid.utils import task_registry -from isaacgym import gymapi from isaacgym.torch_utils import * from policy import SimPolicy from tqdm import tqdm +from isaacgym import gymapi from sim.deploy.config import RobotConfig from sim.env import stompy_mjcf_path -from sim.stompy2.joints import StompyFixed +from sim.stompy.joints import StompyFixed class Worlds(Enum): diff --git a/sim/humanoid_gym/envs/getup_config.py b/sim/humanoid_gym/envs/getup_config.py index 6e16abc6..410a8499 100755 --- a/sim/humanoid_gym/envs/getup_config.py +++ b/sim/humanoid_gym/envs/getup_config.py @@ -8,7 +8,7 @@ from humanoid.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO from sim.env import stompy_urdf_path -from sim.stompy2.joints import Stompy +from sim.stompy.joints import Stompy NUM_JOINTS = len(Stompy.all_joints()) # 37 @@ -42,9 +42,6 @@ class asset(LeggedRobotCfg.asset): # noqa: N801 foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - # old - # foot_name = "_leg_1_x4_1_outer_1" # "foot" - # knee_name = "belt_knee" terminate_after_contacts_on = [] # "link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter @@ -82,7 +79,8 @@ class noise_scales: # noqa: N801 height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): # noqa: N801 - pos = [0.0, 0.0, 0.2] + pos = [0.0, 0.0, 5.2] + # pos = [0.0, 0.0, 0.2] # Face up # rot = [-0.717107, 0.0, 0.0, 0.717107] diff --git a/sim/humanoid_gym/envs/getup_env.py b/sim/humanoid_gym/envs/getup_env.py index b5018660..182fc464 100755 --- a/sim/humanoid_gym/envs/getup_env.py +++ b/sim/humanoid_gym/envs/getup_env.py @@ -5,10 +5,10 @@ from humanoid.envs import LeggedRobot from humanoid.envs.base.legged_robot_config import LeggedRobotCfg from humanoid.utils.terrain import HumanoidTerrain -from isaacgym import gymtorch from isaacgym.torch_utils import * -from sim.stompy2.joints import Stompy +from isaacgym import gymtorch +from sim.stompy.joints import Stompy default_feet_height = 0.0 diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 44de5332..6aaaf66c 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -115,11 +115,6 @@ class control(LeggedRobotCfg.control): "ankle": 10, "knee": 10, } - # ['right shoulder pitch', 'right shoulder yaw', 'right shoulder roll', 'right elbow pitch', 'right wrist roll', 'right wrist pitch', 'right wrist yaw', - # 'right hand left finger', 'right hand right finger', 'shoulder pitch', 'shoulder yaw', 'shoulder roll', 'elbow pitch', 'wrist roll', 'wrist pitch', 'wrist yaw', - # 'left hand left finger', 'left hand right finger', 'torso roll', 'left hip pitch', 'left hip yaw', 'left hip roll', - # 'left knee pitch', 'left ankle pitch', 'left ankle roll', 'right hip pitch', 'right hip yaw', 'right hip roll', 'right knee pitch', - # 'right ankle pitch', 'right ankle roll'] # action scale: target angle = actionScale * action + defaultAngle action_scale = 0.25 # decimation: Number of control action updates @ sim DT per policy DT @@ -134,7 +129,7 @@ class physx(LeggedRobotCfg.sim.physx): num_threads = 12 solver_type = 1 # 0: pgs, 1: tgs num_position_iterations = 4 - num_velocity_iterations = 1 + num_velocity_iterations = 0 contact_offset = 0.01 # [m] rest_offset = -0.02 # [m] bounce_threshold_velocity = 0.1 # [m/s] @@ -148,7 +143,7 @@ class domain_rand: randomize_friction = True friction_range = [0.1, 2.0] - randomize_base_mass = False + randomize_base_mass = True added_mass_range = [-1.0, 1.0] push_robots = True push_interval_s = 4 diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py index 1b7226ed..2ba972de 100755 --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -3,9 +3,9 @@ import xml.etree.ElementTree as ET -from sim.stompy2.joints import StompyFixed +from sim.stompy.joints import StompyFixed -STOMPY_URDF = "stompy/robot.urdf" +STOMPY_URDF = "sim/stompy/robot.urdf" def update_urdf() -> None: @@ -30,8 +30,15 @@ def update_urdf() -> None: limit.set("upper", upper) # Save the modified URDF to a new file - tree.write("stompy/robot_fixed.urdf") + tree.write("sim/stompy/robot_fixed.urdf") if __name__ == "__main__": update_urdf() + + +# adjust default limits to be very constrained arouynd default standing pose +# run create fixed torso +# adjust legs_ppo.py to use the fixed torso +# python train.py --task=legs_ppo --num_envs=1 +# adjust torque valu third_party/humanoid-gym/humanoid/envs/base/legged_robot.py diff --git a/sim/stompy2/__init__.py b/sim/stompy2/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/sim/stompy2/joints.py b/sim/stompy2/joints.py deleted file mode 100755 index 9f92fc98..00000000 --- a/sim/stompy2/joints.py +++ /dev/null @@ -1,708 +0,0 @@ -"""Defines a more Pythonic interface for specifying the joint names. - -The best way to re-generate this snippet for a new robot is to use the -`sim/scripts/print_joints.py` script. This script will print out a hierarchical -tree of the various joint names in the robot. -""" - -import textwrap -from abc import ABC -from typing import Dict, List, Tuple, Union - -import numpy as np - - -class Node(ABC): - @classmethod - def children(cls) -> List["Union[Node, str]"]: - return [ - attr - for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) - if isinstance(attr, (Node, str)) - ] - - @classmethod - def joints(cls) -> List[str]: - return [ - attr - for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) - if isinstance(attr, str) - ] - - @classmethod - def joints_motors(cls) -> List[Tuple[str, str]]: - joint_names: List[Tuple[str, str]] = [] - for attr in dir(cls): - if not attr.startswith("__"): - attr2 = getattr(cls, attr) - if isinstance(attr2, str): - joint_names.append((attr, attr2)) - - return joint_names - - @classmethod - def all_joints(cls) -> List[str]: - leaves = cls.joints() - for child in cls.children(): - if isinstance(child, Node): - leaves.extend(child.all_joints()) - return leaves - - def __str__(self) -> str: - parts = [str(child) for child in self.children()] - parts_str = textwrap.indent("\n" + "\n".join(parts), " ") - return f"[{self.__class__.__name__}]{parts_str}" - - -class Torso(Node): - roll = "torso roll" - - -class LeftHand(Node): - wrist_roll = "wrist roll" - wrist_pitch = "wrist pitch" - wrist_yaw = "wrist yaw" - left_finger = "left hand left finger" - right_finger = "left hand right finger" - - -class LeftArm(Node): - shoulder_yaw = "shoulder yaw" - shoulder_pitch = "shoulder pitch" - shoulder_roll = "shoulder roll" - elbow_pitch = "elbow pitch" - hand = LeftHand() - - -class RightHand(Node): - wrist_roll = "right wrist roll" - wrist_pitch = "right wrist pitch" - wrist_yaw = "right wrist yaw" - left_finger = "right hand left finger" - right_finger = "right hand right finger" - - -class RightArm(Node): - shoulder_yaw = "right shoulder yaw" - shoulder_pitch = "right shoulder pitch" - shoulder_roll = "right shoulder roll" - elbow_pitch = "right elbow pitch" - hand = RightHand() - - -class LeftLeg(Node): - hip_roll = "left hip roll" - hip_yaw = "left hip yaw" - hip_pitch = "left hip pitch" - knee_pitch = "left knee pitch" - ankle_pitch = "left ankle pitch" - ankle_roll = "left ankle roll" - - -class RightLeg(Node): - hip_roll = "right hip roll" - hip_yaw = "right hip yaw" - hip_pitch = "right hip pitch" - knee_pitch = "right knee pitch" - ankle_pitch = "right ankle pitch" - ankle_roll = "right ankle roll" - - -class Legs(Node): - left = LeftLeg() - right = RightLeg() - - -class Stompy(Node): - torso = Torso() - left_arm = LeftArm() - right_arm = RightArm() - legs = Legs() - - @classmethod - def default_positions(cls) -> Dict[str, float]: - return { - Stompy.torso.roll: 0.0, - Stompy.left_arm.shoulder_yaw: np.deg2rad(60), - Stompy.left_arm.shoulder_pitch: np.deg2rad(60), - Stompy.right_arm.shoulder_yaw: np.deg2rad(-60), - } - - @classmethod - def default_standing(cls) -> Dict[str, float]: - return { - Stompy.torso.roll: 2.58, - # arms - Stompy.left_arm.shoulder_pitch: -0.534, - Stompy.left_arm.shoulder_yaw: 2.54, - Stompy.left_arm.shoulder_roll: -0.0314, - Stompy.right_arm.shoulder_pitch: 2.45, - Stompy.right_arm.shoulder_yaw: 3.77, - Stompy.right_arm.shoulder_roll: -0.0314, - Stompy.left_arm.elbow_pitch: 2.35, - Stompy.right_arm.elbow_pitch: 2.65, - # hands - Stompy.left_arm.hand.left_finger: 0.0, - Stompy.left_arm.hand.right_finger: 0.0, - Stompy.right_arm.hand.left_finger: 0.0, - Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: 1.79, - Stompy.left_arm.hand.wrist_pitch: 1.35, - Stompy.left_arm.hand.wrist_yaw: 1.07, - Stompy.right_arm.hand.wrist_roll: -2.13, - Stompy.right_arm.hand.wrist_pitch: 1.79, - Stompy.right_arm.hand.wrist_yaw: -0.251, - # legs - Stompy.legs.left.hip_pitch: -1.6, - Stompy.legs.left.hip_roll: 1.41, - Stompy.legs.left.hip_yaw: -2.12, - Stompy.legs.left.knee_pitch: 2.01, - Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.85, - Stompy.legs.right.hip_pitch: 1.76, - Stompy.legs.right.hip_roll: -1.54, - Stompy.legs.right.hip_yaw: 0.967, - Stompy.legs.right.knee_pitch: 2.07, - Stompy.legs.right.ankle_pitch: 0.377, - Stompy.legs.right.ankle_roll: 1.92, - } - - @classmethod - def default_sitting(cls) -> Dict[str, float]: - return { - Stompy.torso.roll: 0.0, - # arms - Stompy.left_arm.shoulder_pitch: -0.126, - Stompy.left_arm.shoulder_yaw: 2.12, - Stompy.left_arm.shoulder_roll: 1.89, - Stompy.right_arm.shoulder_pitch: -1.13, - Stompy.right_arm.shoulder_yaw: 2.1, - Stompy.right_arm.shoulder_roll: -1.23, - Stompy.left_arm.elbow_pitch: 3.0, - Stompy.right_arm.elbow_pitch: 3.0, - # hands - Stompy.left_arm.hand.left_finger: 0.0, - Stompy.left_arm.hand.right_finger: 0.0, - Stompy.right_arm.hand.left_finger: 0.0, - Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: -0.346, - Stompy.left_arm.hand.wrist_pitch: -0.251, - Stompy.left_arm.hand.wrist_yaw: 0.377, - Stompy.right_arm.hand.wrist_roll: -3.14, - Stompy.right_arm.hand.wrist_pitch: -0.437, - Stompy.right_arm.hand.wrist_yaw: 0.911, - # legs - Stompy.legs.left.hip_pitch: -1.55, - Stompy.legs.left.hip_roll: 1.46, - Stompy.legs.left.hip_yaw: 1.45, - Stompy.legs.left.knee_pitch: 2.17, - Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.79, - Stompy.legs.right.hip_pitch: -1.55, - Stompy.legs.right.hip_roll: -1.67, - Stompy.legs.right.hip_yaw: 1.04, - Stompy.legs.right.knee_pitch: 2.01, - Stompy.legs.right.ankle_pitch: 0.44, - Stompy.legs.right.ankle_roll: 1.79, - } - - @classmethod - def default_limits(cls) -> Dict[str, Dict[str, float]]: - return { - Stompy.torso.roll: { - "lower": -4.36332, - "upper": 4.36332, - }, - Stompy.left_arm.shoulder_pitch: { - "lower": -0.0, - "upper": 0.2, - }, - Stompy.left_arm.shoulder_yaw: { - "lower": 0.97738438, - "upper": 5.3058009, - }, - Stompy.left_arm.shoulder_roll: { - "lower": -4.71239, - "upper": 4.71239, - }, - Stompy.right_arm.shoulder_pitch: { - "lower": -4.71239, - "upper": 4.71239, - }, - Stompy.right_arm.shoulder_yaw: { - "lower": 0.97738438, - "upper": 5.3058009, - }, - Stompy.right_arm.shoulder_roll: { - "lower": -4.71239, - "upper": 4.71239, - }, - Stompy.left_arm.hand.wrist_roll: { - "lower": -4.71239, - "upper": 4.71239, - }, - Stompy.left_arm.hand.wrist_pitch: { - "lower": -3.66519, - "upper": -1.39626, - }, - Stompy.left_arm.hand.wrist_yaw: { - "lower": 0, - "upper": 1.5708, - }, - Stompy.right_arm.hand.wrist_roll: { - "lower": -4.71239, - "upper": 4.71239, - }, - Stompy.right_arm.hand.wrist_pitch: { - "lower": -1.5708, - "upper": 0.523599, - }, - Stompy.right_arm.hand.wrist_yaw: { - "lower": -1.5708, - "upper": 0, - }, - Stompy.legs.left.hip_pitch: { - "lower": -4.712389, - "upper": 4.712389, - }, - Stompy.legs.left.hip_roll: { - "lower": -3.14159, - "upper": 0, - }, - Stompy.legs.left.hip_yaw: { - "lower": -1.0472, - "upper": 2.0944, - }, - Stompy.legs.left.knee_pitch: { - "lower": -4.18879, - "upper": 0, - }, - Stompy.legs.left.ankle_pitch: { - "lower": -1.5708, - "upper": 2.18166, - }, - Stompy.legs.left.ankle_roll: { - "lower": -2.26893, - "upper": -1.22173, - }, - Stompy.legs.right.hip_pitch: { - "lower": -4.712389, - "upper": 4.712389, - }, - Stompy.legs.right.hip_roll: { - "lower": 0, - "upper": 3.14159, - }, - Stompy.legs.right.hip_yaw: { - "lower": -1.0472, - "upper": 2.0944, - }, - Stompy.legs.right.knee_pitch: { - "lower": -4.18879, - "upper": 0, - }, - Stompy.legs.right.ankle_pitch: { - "lower": -1.5708, - "upper": 2.18166, - }, - Stompy.legs.right.ankle_roll: { - "lower": -2.26893, - "upper": -1.22173, - }, - Stompy.left_arm.elbow_pitch: { - "lower": 1.4486233, - "higher": 5.4454273, - }, - Stompy.right_arm.elbow_pitch: { - "lower": 1.4486233, - "higher": 5.4454273, - }, - Stompy.left_arm.hand.left_finger: { - "lower": -0.051, - "upper": 0.0, - }, - Stompy.left_arm.hand.right_finger: { - "lower": 0, - "upper": 0.051, - }, - Stompy.right_arm.hand.left_finger: { - "lower": -0.051, - "upper": 0.0, - }, - Stompy.right_arm.hand.right_finger: { - "lower": 0, - "upper": 0.051, - }, - } - - -class StompyFixed(Stompy): - torso = Torso() - left_arm = LeftArm() - right_arm = RightArm() - legs = Legs() - - @classmethod - def default_standing(cls) -> Dict[str, float]: - return { - Stompy.torso.roll: 2.58, - # arms - Stompy.left_arm.shoulder_pitch: -0.534, - Stompy.left_arm.shoulder_yaw: 2.54, - Stompy.left_arm.shoulder_roll: -0.0314, - Stompy.right_arm.shoulder_pitch: 2.45, - Stompy.right_arm.shoulder_yaw: 3.77, - Stompy.right_arm.shoulder_roll: -0.0314, - Stompy.left_arm.elbow_pitch: 2.35, - Stompy.right_arm.elbow_pitch: 2.65, - # hands - Stompy.left_arm.hand.left_finger: 0.0, - Stompy.left_arm.hand.right_finger: 0.0, - Stompy.right_arm.hand.left_finger: 0.0, - Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: 1.79, - Stompy.left_arm.hand.wrist_pitch: 1.35, - Stompy.left_arm.hand.wrist_yaw: 1.07, - Stompy.right_arm.hand.wrist_roll: -2.13, - Stompy.right_arm.hand.wrist_pitch: 1.79, - Stompy.right_arm.hand.wrist_yaw: -0.251, - # legs - Stompy.legs.left.hip_pitch: -1.6, - Stompy.legs.left.hip_roll: 1.41, - Stompy.legs.left.hip_yaw: -2.12, - Stompy.legs.left.knee_pitch: 2.01, - Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.85, - Stompy.legs.right.hip_pitch: 1.76, - Stompy.legs.right.hip_roll: -1.54, - Stompy.legs.right.hip_yaw: 0.967, - Stompy.legs.right.knee_pitch: 2.07, - Stompy.legs.right.ankle_pitch: 0.377, - Stompy.legs.right.ankle_roll: 1.92, - } - - @classmethod - def default_limits(cls) -> Dict[str, Dict[str, float]]: - return { - Stompy.torso.roll: { - "lower": 2.53, - "upper": 2.63, - }, - Stompy.left_arm.shoulder_pitch: { - "lower": -0.584, - "upper": -0.484, - }, - Stompy.left_arm.shoulder_yaw: { - "lower": 2.49, - "upper": 2.59, - }, - Stompy.left_arm.shoulder_roll: { - "lower": -0.0814, - "upper": 0.0186, - }, - Stompy.right_arm.shoulder_pitch: { - "lower": 2.40, - "upper": 2.50, - }, - Stompy.right_arm.shoulder_yaw: { - "lower": 3.72, - "upper": 3.82, - }, - Stompy.right_arm.shoulder_roll: { - "lower": -0.0814, - "upper": 0.0186, - }, - Stompy.left_arm.elbow_pitch: { - "lower": 2.30, - "upper": 2.40, - }, - Stompy.right_arm.elbow_pitch: { - "lower": 2.60, - "upper": 2.70, - }, - Stompy.left_arm.hand.left_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.left_arm.hand.right_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.right_arm.hand.left_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.right_arm.hand.right_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.left_arm.hand.wrist_roll: { - "lower": 1.74, - "upper": 1.84, - }, - Stompy.left_arm.hand.wrist_pitch: { - "lower": 1.30, - "upper": 1.40, - }, - Stompy.left_arm.hand.wrist_yaw: { - "lower": 1.02, - "upper": 1.12, - }, - Stompy.right_arm.hand.wrist_roll: { - "lower": -2.18, - "upper": -2.08, - }, - Stompy.right_arm.hand.wrist_pitch: { - "lower": 1.74, - "upper": 1.84, - }, - Stompy.right_arm.hand.wrist_yaw: { - "lower": -0.301, - "upper": -0.201, - }, - Stompy.legs.left.hip_pitch: { - "lower": -1.65, - "upper": -1.55, - }, - Stompy.legs.left.hip_roll: { - "lower": 1.36, - "upper": 1.46, - }, - Stompy.legs.left.hip_yaw: { - "lower": -2.17, - "upper": -2.07, - }, - Stompy.legs.left.knee_pitch: { - "lower": 1.96, - "upper": 2.06, - }, - Stompy.legs.left.ankle_pitch: { - "lower": 0.188, - "upper": 0.288, - }, - Stompy.legs.left.ankle_roll: { - "lower": 1.80, - "upper": 1.90, - }, - Stompy.legs.right.hip_pitch: { - "lower": 1.71, - "upper": 1.81, - }, - Stompy.legs.right.hip_roll: { - "lower": -1.59, - "upper": -1.49, - }, - Stompy.legs.right.hip_yaw: { - "lower": 0.917, - "upper": 1.017, - }, - Stompy.legs.right.knee_pitch: { - "lower": 2.02, - "upper": 2.12, - }, - Stompy.legs.right.ankle_pitch: { - "lower": 0.327, - "upper": 0.427, - }, - Stompy.legs.right.ankle_roll: { - "lower": 1.87, - "upper": 1.97, - }, - } - - -class ImuTorso(Torso): - pitch = "torso_1_x8_1_dof_x8" - pelvis_tx = "pelvis_tx" - pelvis_tz = "pelvis_tz" - pelvis_ty = "pelvis_ty" - tilt = "pelvis_tilt" - list = "pelvis_list" - rotation = "pelvis_rotation" - - -class MjcfStompy(Stompy): - torso = ImuTorso() - left_arm = LeftArm() - right_arm = RightArm() - legs = Legs() - - # test the model - @classmethod - def default_standing(cls) -> Dict[str, float]: - return { - Stompy.torso.roll: 2.58, - # arms - Stompy.left_arm.shoulder_pitch: -0.534, - Stompy.left_arm.shoulder_yaw: 2.54, - Stompy.left_arm.shoulder_roll: -0.0314, - Stompy.right_arm.shoulder_pitch: 2.45, - Stompy.right_arm.shoulder_yaw: 3.77, - Stompy.right_arm.shoulder_roll: -0.0314, - Stompy.left_arm.elbow_pitch: 2.35, - Stompy.right_arm.elbow_pitch: 2.65, - # hands - Stompy.left_arm.hand.left_finger: 0.0, - Stompy.left_arm.hand.right_finger: 0.0, - Stompy.right_arm.hand.left_finger: 0.0, - Stompy.right_arm.hand.right_finger: 0.0, - Stompy.left_arm.hand.wrist_roll: 1.79, - Stompy.left_arm.hand.wrist_pitch: 1.35, - Stompy.left_arm.hand.wrist_yaw: 1.07, - Stompy.right_arm.hand.wrist_roll: -2.13, - Stompy.right_arm.hand.wrist_pitch: 1.79, - Stompy.right_arm.hand.wrist_yaw: -0.251, - # legs - Stompy.legs.left.hip_pitch: -1.6, - Stompy.legs.left.hip_roll: 1.41, - Stompy.legs.left.hip_yaw: -2.12, - Stompy.legs.left.knee_pitch: 2.01, - Stompy.legs.left.ankle_pitch: 0.238, - Stompy.legs.left.ankle_roll: 1.85, - Stompy.legs.right.hip_pitch: 1.76, - Stompy.legs.right.hip_roll: -1.54, - Stompy.legs.right.hip_yaw: 0.967, - Stompy.legs.right.knee_pitch: 2.07, - Stompy.legs.right.ankle_pitch: 0.377, - Stompy.legs.right.ankle_roll: 1.92, - } - - @classmethod - def default_limits(cls) -> Dict[str, Dict[str, float]]: - return { - Stompy.torso.roll: { - "lower": 2.53, - "upper": 2.63, - }, - Stompy.left_arm.shoulder_pitch: { - "lower": -0.584, - "upper": -0.484, - }, - Stompy.left_arm.shoulder_yaw: { - "lower": 2.49, - "upper": 2.59, - }, - Stompy.left_arm.shoulder_roll: { - "lower": -0.0814, - "upper": 0.0186, - }, - Stompy.right_arm.shoulder_pitch: { - "lower": 2.40, - "upper": 2.50, - }, - Stompy.right_arm.shoulder_yaw: { - "lower": 3.72, - "upper": 3.82, - }, - Stompy.right_arm.shoulder_roll: { - "lower": -0.0814, - "upper": 0.0186, - }, - Stompy.left_arm.elbow_pitch: { - "lower": 2.30, - "upper": 2.40, - }, - Stompy.right_arm.elbow_pitch: { - "lower": 2.60, - "upper": 2.70, - }, - Stompy.left_arm.hand.left_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.left_arm.hand.right_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.right_arm.hand.left_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.right_arm.hand.right_finger: { - "lower": -0.05, - "upper": 0.05, - }, - Stompy.left_arm.hand.wrist_roll: { - "lower": 1.74, - "upper": 1.84, - }, - Stompy.left_arm.hand.wrist_pitch: { - "lower": 1.30, - "upper": 1.40, - }, - Stompy.left_arm.hand.wrist_yaw: { - "lower": 1.02, - "upper": 1.12, - }, - Stompy.right_arm.hand.wrist_roll: { - "lower": -2.18, - "upper": -2.08, - }, - Stompy.right_arm.hand.wrist_pitch: { - "lower": 1.74, - "upper": 1.84, - }, - Stompy.right_arm.hand.wrist_yaw: { - "lower": -0.301, - "upper": -0.201, - }, - Stompy.legs.left.hip_pitch: { - "lower": -1.65, - "upper": -1.55, - }, - Stompy.legs.left.hip_roll: { - "lower": 1.36, - "upper": 1.46, - }, - Stompy.legs.left.hip_yaw: { - "lower": -2.17, - "upper": -2.07, - }, - Stompy.legs.left.knee_pitch: { - "lower": 1.96, - "upper": 2.06, - }, - Stompy.legs.left.ankle_pitch: { - "lower": 0.188, - "upper": 0.288, - }, - Stompy.legs.left.ankle_roll: { - "lower": 1.80, - "upper": 1.90, - }, - Stompy.legs.right.hip_pitch: { - "lower": 1.71, - "upper": 1.81, - }, - Stompy.legs.right.hip_roll: { - "lower": -1.59, - "upper": -1.49, - }, - Stompy.legs.right.hip_yaw: { - "lower": 0.917, - "upper": 1.017, - }, - Stompy.legs.right.knee_pitch: { - "lower": 2.02, - "upper": 2.12, - }, - Stompy.legs.right.ankle_pitch: { - "lower": 0.327, - "upper": 0.427, - }, - Stompy.legs.right.ankle_roll: { - "lower": 1.87, - "upper": 1.97, - }, - } - - -def print_joints() -> None: - joints = Stompy.all_joints() - assert len(joints) == len(set(joints)), "Duplicate joint names found!" - print(Stompy()) - - -if __name__ == "__main__": - # python -m sim.stompy.joints - print_joints() From e5e9eec9814511f59d35c0fbe0f8fc90278e13d3 Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 18:29:30 -0700 Subject: [PATCH 13/21] more fixes --- sim/env.py | 4 ---- sim/humanoid_gym/envs/getup_config.py | 3 +-- sim/humanoid_gym/envs/legs_config.py | 8 +------- sim/humanoid_gym/envs/legs_env.py | 2 +- sim/humanoid_gym/envs/stompy_config.py | 1 - sim/humanoid_gym/envs/stompy_env.py | 3 +-- sim/scripts/create_fixed_torso.py | 7 ------- sim/scripts/simulate_urdf.py | 4 +--- 8 files changed, 5 insertions(+), 27 deletions(-) diff --git a/sim/env.py b/sim/env.py index daecc371..b512d58d 100755 --- a/sim/env.py +++ b/sim/env.py @@ -3,10 +3,6 @@ import os from pathlib import Path -from dotenv import load_dotenv - -load_dotenv() - def model_dir() -> Path: return Path(os.environ.get("MODEL_DIR", "models")) diff --git a/sim/humanoid_gym/envs/getup_config.py b/sim/humanoid_gym/envs/getup_config.py index 410a8499..706b4fdc 100755 --- a/sim/humanoid_gym/envs/getup_config.py +++ b/sim/humanoid_gym/envs/getup_config.py @@ -79,8 +79,7 @@ class noise_scales: # noqa: N801 height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): # noqa: N801 - pos = [0.0, 0.0, 5.2] - # pos = [0.0, 0.0, 0.2] + pos = [0.0, 0.0, 0.2] # Face up # rot = [-0.717107, 0.0, 0.0, 0.717107] diff --git a/sim/humanoid_gym/envs/legs_config.py b/sim/humanoid_gym/envs/legs_config.py index 6b91c071..4574562f 100755 --- a/sim/humanoid_gym/envs/legs_config.py +++ b/sim/humanoid_gym/envs/legs_config.py @@ -4,7 +4,7 @@ from sim.env import stompy_urdf_path from sim.humanoid_gym.envs.stompy_config import StompyCfg -from sim.stompy2.joints import StompyFixed +from sim.stompy.joints import StompyFixed NUM_JOINTS = len(StompyFixed.default_standing()) # 17 @@ -33,19 +33,13 @@ class asset(LeggedRobotCfg.asset): name = "stompy" - # foot_name = "_foot_1_foot_1" foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - # old - # foot_name = "_leg_1_x4_1_outer_1" # "foot" - # knee_name = "belt_knee" - termination_height = 0.23 default_feet_height = 0.0 terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] - # terminate_after_contacts_on = ["link_torso_1_top_torso_1"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter diff --git a/sim/humanoid_gym/envs/legs_env.py b/sim/humanoid_gym/envs/legs_env.py index c3ce260e..dab514bc 100755 --- a/sim/humanoid_gym/envs/legs_env.py +++ b/sim/humanoid_gym/envs/legs_env.py @@ -8,7 +8,7 @@ from isaacgym.torch_utils import * from isaacgym import gymtorch -from sim.old_stompy.joints import StompyFixed +from sim.stompy.joints import StompyFixed class LegsFreeEnv(LeggedRobot): diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 6aaaf66c..ee6216bb 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -94,7 +94,6 @@ class init_state(LeggedRobotCfg.init_state): class control(LeggedRobotCfg.control): # PD Drive parameters: - # NOTE: these names are outdated, must be manually set in third party legged_robot.py stiffness = { "shoulder": 200, "elbow": 200, diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index 96ce8649..9429a6f7 100755 --- a/sim/humanoid_gym/envs/stompy_env.py +++ b/sim/humanoid_gym/envs/stompy_env.py @@ -8,7 +8,7 @@ from isaacgym.torch_utils import * from isaacgym import gymtorch -from sim.old_stompy.joints import Stompy +from sim.stompy.joints import Stompy class StompyFreeEnv(LeggedRobot): @@ -122,7 +122,6 @@ def compute_ref_state(self): scale_2 = 2 * scale_1 # left foot stance phase set to default joint pos sin_pos_l[sin_pos_l > 0] = 0 - self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] = sin_pos_l * scale_1 self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] = sin_pos_l * scale_2 self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] = sin_pos_l * scale_1 diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py index 2ba972de..861fbdf0 100755 --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -35,10 +35,3 @@ def update_urdf() -> None: if __name__ == "__main__": update_urdf() - - -# adjust default limits to be very constrained arouynd default standing pose -# run create fixed torso -# adjust legs_ppo.py to use the fixed torso -# python train.py --task=legs_ppo --num_envs=1 -# adjust torque valu third_party/humanoid-gym/humanoid/envs/base/legged_robot.py diff --git a/sim/scripts/simulate_urdf.py b/sim/scripts/simulate_urdf.py index 495b6693..0a0dcde0 100755 --- a/sim/scripts/simulate_urdf.py +++ b/sim/scripts/simulate_urdf.py @@ -14,7 +14,7 @@ from isaacgym import gymapi, gymtorch, gymutil from sim.env import stompy_urdf_path from sim.logging import configure_logging -from sim.old_stompy.joints import Stompy +from sim.stompy.joints import Stompy logger = logging.getLogger(__name__) @@ -151,7 +151,6 @@ def load_gym() -> GymParams: # dof_vel[:] = 0.0 starting_positions = Stompy.default_standing() dof_ids: Dict[str, int] = gym.get_actor_dof_dict(env, robot) - print(starting_positions) for joint_name, joint_position in starting_positions.items(): dof_pos[0, dof_ids[joint_name]] = joint_position env_ids_int32 = torch.zeros(1, dtype=torch.int32) @@ -161,7 +160,6 @@ def load_gym() -> GymParams: gymtorch.unwrap_tensor(env_ids_int32), 1, ) - print(dof_pos) return GymParams( gym=gym, From d346b62d9fb33c80de001fc3d29a7274877f8e95 Mon Sep 17 00:00:00 2001 From: is2ac Date: Tue, 9 Jul 2024 20:56:49 -0700 Subject: [PATCH 14/21] linting --- sim/humanoid_gym/envs/__init__.py | 7 +- sim/old_stompy/joints.py | 424 ++++++++++++++++++++++++++++++ 2 files changed, 428 insertions(+), 3 deletions(-) create mode 100755 sim/old_stompy/joints.py diff --git a/sim/humanoid_gym/envs/__init__.py b/sim/humanoid_gym/envs/__init__.py index 65c78a03..52ac3531 100755 --- a/sim/humanoid_gym/envs/__init__.py +++ b/sim/humanoid_gym/envs/__init__.py @@ -7,9 +7,10 @@ quickly. """ -import isaacgym -import torch - +# fmt: off +import isaacgym # isort:skip +import torch #isort:skip +# fmt: on from .getup_config import GetupCfg, GetupCfgPPO from .getup_env import GetupFreeEnv from .legs_config import LegsCfg, LegsCfgPPO diff --git a/sim/old_stompy/joints.py b/sim/old_stompy/joints.py new file mode 100755 index 00000000..69fe388d --- /dev/null +++ b/sim/old_stompy/joints.py @@ -0,0 +1,424 @@ +"""Defines a more Pythonic interface for specifying the joint names. + +The best way to re-generate this snippet for a new robot is to use the +`sim/scripts/print_joints.py` script. This script will print out a hierarchical +tree of the various joint names in the robot. +""" + +import textwrap +from abc import ABC +from typing import Dict, List, Tuple, Union + +import numpy as np + + +class Node(ABC): + @classmethod + def children(cls) -> List["Union[Node, str]"]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, (Node, str)) + ] + + @classmethod + def joints(cls) -> List[str]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, str) + ] + + @classmethod + def joints_motors(cls) -> List[Tuple[str, str]]: + joint_names: List[Tuple[str, str]] = [] + for attr in dir(cls): + if not attr.startswith("__"): + attr2 = getattr(cls, attr) + if isinstance(attr2, str): + joint_names.append((attr, attr2)) + + return joint_names + + @classmethod + def all_joints(cls) -> List[str]: + leaves = cls.joints() + for child in cls.children(): + if isinstance(child, Node): + leaves.extend(child.all_joints()) + return leaves + + def __str__(self) -> str: + parts = [str(child) for child in self.children()] + parts_str = textwrap.indent("\n" + "\n".join(parts), " ") + return f"[{self.__class__.__name__}]{parts_str}" + + +class Head(Node): + left_right = "joint_head_1_x4_1_dof_x4" + up_down = "joint_head_1_x4_2_dof_x4" + + +class Torso(Node): + pitch = "joint_torso_1_x8_1_dof_x8" + + +class LeftHand(Node): + hand_roll = "joint_left_arm_2_hand_1_x4_1_dof_x4" + hand_grip = "joint_left_arm_2_hand_1_x4_2_dof_x4" + slider_a = "joint_left_arm_2_hand_1_slider_1" + slider_b = "joint_left_arm_2_hand_1_slider_2" + + +class LeftArm(Node): + shoulder_yaw = "joint_left_arm_2_x8_1_dof_x8" + shoulder_pitch = "joint_left_arm_2_x8_2_dof_x8" + shoulder_roll = "joint_left_arm_2_x6_1_dof_x6" + elbow_yaw = "joint_left_arm_2_x6_2_dof_x6" + elbow_roll = "joint_left_arm_2_x4_1_dof_x4" + hand = LeftHand() + + +class RightHand(Node): + hand_roll = "joint_right_arm_1_hand_1_x4_1_dof_x4" + hand_grip = "joint_right_arm_1_hand_1_x4_2_dof_x4" + slider_a = "joint_right_arm_1_hand_1_slider_1" + slider_b = "joint_right_arm_1_hand_1_slider_2" + + +class RightArm(Node): + shoulder_yaw = "joint_right_arm_1_x8_1_dof_x8" + shoulder_pitch = "joint_right_arm_1_x8_2_dof_x8" + shoulder_roll = "joint_right_arm_1_x6_1_dof_x6" + elbow_yaw = "joint_right_arm_1_x6_2_dof_x6" + elbow_roll = "joint_right_arm_1_x4_1_dof_x4" + hand = RightHand() + + +class LeftLeg(Node): + hip_roll = "joint_legs_1_x8_2_dof_x8" + hip_yaw = "joint_legs_1_left_leg_1_x8_1_dof_x8" + hip_pitch = "joint_legs_1_left_leg_1_x10_1_dof_x10" + knee_motor = "joint_legs_1_left_leg_1_x10_2_dof_x10" + knee = "joint_legs_1_left_leg_1_knee_revolute" + ankle_motor = "joint_legs_1_left_leg_1_x6_1_dof_x6" + ankle = "joint_legs_1_left_leg_1_ankle_revolute" + foot_roll = "joint_legs_1_left_leg_1_x4_1_dof_x4" + + +class RightLeg(Node): + hip_roll = "joint_legs_1_x8_1_dof_x8" + hip_yaw = "joint_legs_1_right_leg_1_x8_1_dof_x8" + hip_pitch = "joint_legs_1_right_leg_1_x10_2_dof_x10" + knee_motor = "joint_legs_1_right_leg_1_x10_1_dof_x10" + knee = "joint_legs_1_right_leg_1_knee_revolute" + ankle_motor = "joint_legs_1_right_leg_1_x6_1_dof_x6" + ankle = "joint_legs_1_right_leg_1_ankle_revolute" + foot_roll = "joint_legs_1_right_leg_1_x4_1_dof_x4" + + +class Legs(Node): + left = LeftLeg() + right = RightLeg() + + +class Stompy(Node): + head = Head() + torso = Torso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + @classmethod + def default_positions(cls) -> Dict[str, float]: + return { + Stompy.head.left_right: np.deg2rad(-54), + Stompy.head.up_down: 0.0, + Stompy.torso.pitch: 0.0, + Stompy.left_arm.shoulder_yaw: np.deg2rad(60), + Stompy.left_arm.shoulder_pitch: np.deg2rad(60), + Stompy.right_arm.shoulder_yaw: np.deg2rad(-60), + } + + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + Stompy.head.left_right: np.deg2rad(-2), # -0.03 + # arms + Stompy.left_arm.shoulder_yaw: np.deg2rad(-69.5), # -1.21 + Stompy.left_arm.shoulder_pitch: np.deg2rad(-93), # 1.61 + Stompy.right_arm.shoulder_yaw: np.deg2rad(85), # 1.48 + Stompy.right_arm.shoulder_pitch: np.deg2rad(104), # 1.81 + # legs + Stompy.legs.left.hip_roll: np.deg2rad(29), # 0.5 + Stompy.legs.left.hip_yaw: np.deg2rad(-29), # -0.5 + Stompy.legs.left.hip_pitch: np.deg2rad(56), # 0.97 + Stompy.legs.right.hip_roll: np.deg2rad(-29), # -0.5 + Stompy.legs.right.hip_yaw: np.deg2rad(-29), # -0.5 + Stompy.legs.right.hip_pitch: np.deg2rad(-56), # -0.97 + Stompy.legs.left.knee: np.deg2rad(-6), # -0.1 + Stompy.legs.right.knee: np.deg2rad(6), # 0.1 + Stompy.legs.left.ankle: np.deg2rad(0), # 0 + Stompy.legs.right.ankle: np.deg2rad(0), # 0 + Stompy.legs.left.foot_roll: np.deg2rad(0), # 0 + Stompy.legs.right.foot_roll: np.deg2rad(0), # 0 + } + + @classmethod + def default_sitting(cls) -> Dict[str, float]: + return { + Stompy.head.left_right: np.deg2rad(-3), + Stompy.head.up_down: 0.0, + Stompy.torso.pitch: 0.0, + # arms + Stompy.left_arm.shoulder_yaw: np.deg2rad(-88), + Stompy.left_arm.shoulder_pitch: np.deg2rad(-30), + Stompy.left_arm.shoulder_roll: np.deg2rad(-190), + Stompy.left_arm.elbow_yaw: np.deg2rad(-88), + Stompy.right_arm.shoulder_yaw: np.deg2rad(88), + Stompy.right_arm.shoulder_pitch: np.deg2rad(30), + Stompy.right_arm.shoulder_roll: np.deg2rad(190), + Stompy.right_arm.elbow_yaw: np.deg2rad(88), + # hands + Stompy.left_arm.hand.hand_roll: np.deg2rad(-60), + Stompy.right_arm.hand.hand_roll: np.deg2rad(-60), + # legs + Stompy.legs.left.hip_roll: np.deg2rad(29), + Stompy.legs.left.hip_yaw: np.deg2rad(-29), + Stompy.legs.left.hip_pitch: np.deg2rad(56), + Stompy.legs.right.hip_roll: np.deg2rad(-29), + Stompy.legs.right.hip_yaw: np.deg2rad(-29), + # check this + Stompy.legs.right.hip_pitch: np.deg2rad(-56), + Stompy.legs.left.knee: np.deg2rad(-6), + Stompy.legs.right.knee: np.deg2rad(6), + } + + +class StompyFixed(Stompy): + head = Head() + torso = Torso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + Stompy.head.left_right: np.deg2rad(-2), # -0.03 + # arms + Stompy.left_arm.shoulder_yaw: np.deg2rad(-69.5), # -1.21 + Stompy.left_arm.shoulder_pitch: np.deg2rad(-93), # 1.61 + Stompy.right_arm.shoulder_yaw: np.deg2rad(85), # 1.48 + Stompy.right_arm.shoulder_pitch: np.deg2rad(104), # 1.81 + # legs + Stompy.legs.left.hip_roll: np.deg2rad(29), # 0.5 + Stompy.legs.left.hip_yaw: np.deg2rad(-29), # -0.5 + Stompy.legs.left.hip_pitch: np.deg2rad(56), # 0.97 + Stompy.legs.right.hip_roll: np.deg2rad(-29), # -0.5 + Stompy.legs.right.hip_yaw: np.deg2rad(-29), # -0.5 + Stompy.legs.right.hip_pitch: np.deg2rad(-56), # -0.97 + Stompy.legs.left.knee: np.deg2rad(-6), # -0.1 + Stompy.legs.right.knee: np.deg2rad(6), # 0.1 + Stompy.legs.left.ankle: np.deg2rad(0), # 0 + Stompy.legs.right.ankle: np.deg2rad(0), # 0 + Stompy.legs.left.foot_roll: np.deg2rad(0), # 0 + Stompy.legs.right.foot_roll: np.deg2rad(0), # 0 + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + Stompy.head.left_right: { + "lower": -0.1, + "upper": 0.0, + }, + Stompy.right_arm.shoulder_yaw: { + "lower": 1.47, + "upper": 1.50, + }, + Stompy.left_arm.shoulder_yaw: { + "lower": -1.23, + "upper": -1.20, + }, + Stompy.right_arm.shoulder_pitch: { + "lower": 1.8, + "upper": 1.83, + }, + Stompy.left_arm.shoulder_pitch: { + "lower": -1.63, + "upper": -1.60, + }, + Stompy.legs.right.hip_roll: { + "lower": -0.75, + "upper": -0.15, + }, + Stompy.legs.left.hip_roll: { + "lower": 0.2, + "upper": 0.75, + }, + Stompy.legs.right.hip_yaw: { + "lower": -1, + "upper": 0.0, + }, + Stompy.legs.left.hip_yaw: { + "lower": -0.71, + "upper": -0.3, + }, + Stompy.legs.right.hip_pitch: { + "lower": -1.1, + "upper": -0.4, + }, + Stompy.legs.left.hip_pitch: { + "lower": -0.1, + "upper": 1.2, + }, + Stompy.legs.right.knee: { + "lower": 0, + "upper": 1.2, + }, + Stompy.legs.left.knee: { + "lower": -1.2, + "upper": 0, + }, + Stompy.legs.right.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + Stompy.legs.left.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + Stompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, + Stompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, + } + + +class ImuTorso(Torso): + pitch = "torso_1_x8_1_dof_x8" + pelvis_tx = "pelvis_tx" + pelvis_tz = "pelvis_tz" + pelvis_ty = "pelvis_ty" + tilt = "pelvis_tilt" + list = "pelvis_list" + rotation = "pelvis_rotation" + + +class MjcfStompy(Stompy): + head = Head() + torso = ImuTorso() + left_arm = LeftArm() + right_arm = RightArm() + legs = Legs() + + # test the model + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + MjcfStompy.head.left_right: 0.8526, + # arms + MjcfStompy.left_arm.shoulder_yaw: 1.57, + MjcfStompy.left_arm.shoulder_pitch: 1.59, + MjcfStompy.left_arm.elbow_roll: 0.0, + MjcfStompy.left_arm.elbow_yaw: 0.0, + MjcfStompy.right_arm.elbow_roll: 2.48, + MjcfStompy.right_arm.elbow_yaw: -0.09, + MjcfStompy.right_arm.shoulder_yaw: -1.6, + MjcfStompy.right_arm.shoulder_pitch: -1.64, + # left hand + MjcfStompy.left_arm.hand.hand_roll: 0.9, + # right hand + MjcfStompy.right_arm.hand.hand_roll: 0.64, + # left leg + MjcfStompy.legs.left.hip_roll: -0.52, + MjcfStompy.legs.left.hip_yaw: 0.6, + MjcfStompy.legs.left.hip_pitch: -0.8, + MjcfStompy.legs.right.hip_roll: 0.59, + MjcfStompy.legs.right.hip_yaw: 0.7, + MjcfStompy.legs.right.hip_pitch: 1.0, + # right leg + MjcfStompy.legs.left.knee: 0.2, + MjcfStompy.legs.right.knee: 0.0, + MjcfStompy.legs.left.ankle: 0.0, + MjcfStompy.legs.right.ankle: 0.0, + MjcfStompy.legs.left.foot_roll: 0.0, + MjcfStompy.legs.right.foot_roll: 0.0, + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + MjcfStompy.head.left_right: { + "lower": -3.14, + "upper": 3.14, + }, + MjcfStompy.right_arm.shoulder_yaw: { + "lower": -2.09, + "upper": 2.09, + }, + MjcfStompy.left_arm.shoulder_yaw: { + "lower": -2.09, + "upper": 2.09, + }, + MjcfStompy.right_arm.shoulder_pitch: { + "lower": -1.91, + "upper": 1.91, + }, + MjcfStompy.left_arm.shoulder_pitch: { + "lower": -1.91, + "upper": 1.91, + }, + MjcfStompy.legs.right.hip_roll: { + "lower": -1.04, + "upper": 1.04, + }, + MjcfStompy.legs.left.hip_roll: { + "lower": -1.04, + "upper": 1.04, + }, + MjcfStompy.legs.right.hip_yaw: { + "lower": -1.57, + "upper": 1.06, + }, + MjcfStompy.legs.left.hip_yaw: { + "lower": -1.57, + "upper": 1.06, + }, + MjcfStompy.legs.right.hip_pitch: { + "lower": -1.78, + "upper": 2.35, + }, + MjcfStompy.legs.left.hip_pitch: { + "lower": -1.78, + "upper": 2.35, + }, + MjcfStompy.legs.right.knee: { + "lower": 0, + "upper": 1.2, + }, + MjcfStompy.legs.left.knee: { + "lower": -1.2, + "upper": 0, + }, + MjcfStompy.legs.right.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + MjcfStompy.legs.left.ankle: { + "lower": -0.3, + "upper": 0.3, + }, + MjcfStompy.legs.right.foot_roll: {"lower": -0.3, "upper": 0.3}, + MjcfStompy.legs.left.foot_roll: {"lower": -0.3, "upper": 0.3}, + } + + +def print_joints() -> None: + joints = Stompy.all_joints() + assert len(joints) == len(set(joints)), "Duplicate joint names found!" + print(Stompy()) + + +if __name__ == "__main__": + # python -m sim.stompy.joints + print_joints() From e2f9eb54dba4f836169def5ba715b1a353c33966 Mon Sep 17 00:00:00 2001 From: is2ac Date: Wed, 10 Jul 2024 16:45:51 -0700 Subject: [PATCH 15/21] save the walking policy, small issues with knees --- sim/humanoid_gym/envs/__init__.py | 3 + sim/humanoid_gym/envs/only_legs_config.py | 261 ++++++++++ sim/humanoid_gym/envs/only_legs_env.py | 550 ++++++++++++++++++++++ sim/humanoid_gym/envs/stompy_config.py | 2 + 4 files changed, 816 insertions(+) create mode 100755 sim/humanoid_gym/envs/only_legs_config.py create mode 100755 sim/humanoid_gym/envs/only_legs_env.py diff --git a/sim/humanoid_gym/envs/__init__.py b/sim/humanoid_gym/envs/__init__.py index 52ac3531..52c03558 100755 --- a/sim/humanoid_gym/envs/__init__.py +++ b/sim/humanoid_gym/envs/__init__.py @@ -15,6 +15,8 @@ from .getup_env import GetupFreeEnv from .legs_config import LegsCfg, LegsCfgPPO from .legs_env import LegsFreeEnv +from .only_legs_config import OnlyLegsCfg, OnlyLegsCfgPPO +from .only_legs_env import OnlyLegsFreeEnv from .stompy_config import StompyCfg, StompyCfgPPO from .stompy_env import StompyFreeEnv @@ -30,6 +32,7 @@ def register_tasks() -> None: task_registry.register("stompy_ppo", StompyFreeEnv, StompyCfg(), StompyCfgPPO()) task_registry.register("getup_ppo", GetupFreeEnv, GetupCfg(), GetupCfgPPO()) task_registry.register("legs_ppo", LegsFreeEnv, LegsCfg(), LegsCfgPPO()) + task_registry.register("only_legs_ppo", OnlyLegsFreeEnv, OnlyLegsCfg(), OnlyLegsCfgPPO()) register_tasks() diff --git a/sim/humanoid_gym/envs/only_legs_config.py b/sim/humanoid_gym/envs/only_legs_config.py new file mode 100755 index 00000000..9461839e --- /dev/null +++ b/sim/humanoid_gym/envs/only_legs_config.py @@ -0,0 +1,261 @@ +"""Defines the environment configuration for the Getting up task""" + +from humanoid.envs.base.legged_robot_config import ( # type: ignore + LeggedRobotCfg, + LeggedRobotCfgPPO, +) + +from sim.env import stompy_urdf_path +from sim.stompy_legs.joints import Stompy + +NUM_JOINTS = len(Stompy.all_joints()) # 33 + + +class OnlyLegsCfg(LeggedRobotCfg): + """ + Configuration class for the Legs humanoid robot. + """ + + class env(LeggedRobotCfg.env): + # change the observation dim + + frame_stack = 15 + c_frame_stack = 3 + num_single_obs = 11 + NUM_JOINTS * 3 + num_observations = int(frame_stack * num_single_obs) + single_num_privileged_obs = 25 + NUM_JOINTS * 4 + num_privileged_obs = int(c_frame_stack * single_num_privileged_obs) + num_actions = NUM_JOINTS + num_envs = 4096 + episode_length_s = 24 # episode length in seconds + use_ref_actions = False + + class safety: + # safety factors + pos_limit = 1.0 + vel_limit = 1.0 + torque_limit = 1.0 + + class asset(LeggedRobotCfg.asset): + file = str(stompy_urdf_path()) + + name = "stompy" + + foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" + knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" + + termination_height = 0.23 + default_feet_height = 0.0 + terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] + + penalize_contacts_on = [] + self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter + flip_visual_attachments = False + replace_cylinder_with_capsule = False + fix_base_link = False + + class terrain(LeggedRobotCfg.terrain): + mesh_type = "plane" + # mesh_type = 'trimesh' + curriculum = False + # rough terrain only: + measure_heights = False + static_friction = 0.6 + dynamic_friction = 0.6 + terrain_length = 8.0 + terrain_width = 8.0 + num_rows = 10 # number of terrain rows (levels) + num_cols = 10 # number of terrain cols (types) + max_init_terrain_level = 10 # starting curriculum state + # plane; obstacles; uniform; slope_up; slope_down, stair_up, stair_down + terrain_proportions = [0.2, 0.2, 0.4, 0.1, 0.1, 0, 0] + restitution = 0.0 + + class noise: + add_noise = True + noise_level = 0.6 # scales other values + + class noise_scales: + dof_pos = 0.05 + dof_vel = 0.5 + ang_vel = 0.1 + lin_vel = 0.05 + quat = 0.03 + height_measurements = 0.1 + + class init_state(LeggedRobotCfg.init_state): + pos = [0.0, 0.0, 1.15] + + default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} + + default_positions = Stompy.default_standing() + for joint in default_positions: + default_joint_angles[joint] = default_positions[joint] + + class control(LeggedRobotCfg.control): + # PD Drive parameters: + stiffness = { + "shoulder": 200, + "elbow": 200, + "wrist": 200, + "hand": 200, + "torso": 200, + "hip": 200, + "ankle": 200, + "knee": 200, + } + damping = { + "shoulder": 10, + "elbow": 10, + "wrist": 10, + "hand": 10, + "torso": 10, + "hip": 10, + "ankle": 10, + "knee": 10, + } + # action scale: target angle = actionScale * action + defaultAngle + action_scale = 0.25 + # decimation: Number of control action updates @ sim DT per policy DT + decimation = 10 # 100hz + + class sim(LeggedRobotCfg.sim): + dt = 0.001 # 1000 Hz + substeps = 1 # 2 + up_axis = 1 # 0 is y, 1 is z + + class physx(LeggedRobotCfg.sim.physx): + num_threads = 12 + solver_type = 1 # 0: pgs, 1: tgs + num_position_iterations = 4 + num_velocity_iterations = 0 + contact_offset = 0.01 # [m] + rest_offset = -0.02 # [m] + bounce_threshold_velocity = 0.1 # [m/s] + max_depenetration_velocity = 1.0 + max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more + default_buffer_size_multiplier = 5 + # 0: never, 1: last sub-step, 2: all sub-steps (default=2) + contact_collection = 2 + + class domain_rand: + randomize_friction = True + friction_range = [0.1, 2.0] + + randomize_base_mass = True + added_mass_range = [-1.0, 1.0] + push_robots = True + push_interval_s = 4 + max_push_vel_xy = 0.2 + max_push_ang_vel = 0.4 + dynamic_randomization = 0.02 + + class commands(LeggedRobotCfg.commands): + # Vers: lin_vel_x, lin_vel_y, ang_vel_yaw, heading (in heading mode ang_vel_yaw is recomputed from heading error) + num_commands = 4 + resampling_time = 8.0 # time before command are changed[s] + heading_command = True # if true: compute ang vel command from heading error + + class ranges: + lin_vel_x = [-0.3, 0.6] # min max [m/s] + lin_vel_y = [-0.3, 0.3] # min max [m/s] + ang_vel_yaw = [-0.3, 0.3] # min max [rad/s] + heading = [-3.14, 3.14] + + class rewards: + # quite important to keep it right + base_height_target = 0.97 + min_dist = 0.2 + max_dist = 0.5 + # put some settings here for LLM parameter tuning + target_joint_pos_scale = 0.17 # rad + target_feet_height = 0.06 # m + cycle_time = 0.64 # sec + # if true negative total rewards are clipped at zero (avoids early termination problems) + only_positive_rewards = True + # tracking reward = exp(error*sigma) + tracking_sigma = 5 + max_contact_force = 400 # forces above this value are penalized + + class scales: + # reference motion tracking + joint_pos = 1.6 + feet_clearance = 1.0 + feet_contact_number = 1.2 + # gait + feet_air_time = 1.0 + foot_slip = -0.05 + feet_distance = 0.2 + knee_distance = 0.2 + # contact + feet_contact_forces = -0.01 + # vel tracking + tracking_lin_vel = 1.2 + tracking_ang_vel = 1.1 + vel_mismatch_exp = 0.5 # lin_z; ang x,y + low_speed = 0.2 + track_vel_hard = 0.5 + + # above this was removed + # base pos + default_joint_pos = 0.5 + orientation = 1 + base_height = 0.2 + base_acc = 0.2 + # energy + action_smoothness = -0.002 + torques = -1e-5 + dof_vel = -5e-4 + dof_acc = -1e-7 + collision = -1.0 + + class normalization: + class obs_scales: + lin_vel = 2.0 + ang_vel = 1.0 + dof_pos = 1.0 + dof_vel = 0.05 + quat = 1.0 + height_measurements = 5.0 + + clip_observations = 18.0 + clip_actions = 18.0 + + class viewer: + ref_env = 0 + pos = [4, -4, 2] + lookat = [0, -2, 0] + + +class OnlyLegsCfgPPO(LeggedRobotCfgPPO): + seed = 5 + runner_class_name = "OnPolicyRunner" # DWLOnPolicyRunner + + class policy: + init_noise_std = 1.0 + actor_hidden_dims = [512, 256, 128] + critic_hidden_dims = [768, 256, 128] + + class algorithm(LeggedRobotCfgPPO.algorithm): + entropy_coef = 0.001 + learning_rate = 1e-5 + num_learning_epochs = 2 + gamma = 0.994 + lam = 0.9 + num_mini_batches = 4 + + class runner: + policy_class_name = "ActorCritic" + algorithm_class_name = "PPO" + num_steps_per_env = 60 # per iteration + max_iterations = 10001 # number of policy updates + + # logging + save_interval = 100 # check for potential saves every this many iterations + experiment_name = "Legs" + run_name = "" + # load and resume + resume = False + load_run = -1 # -1 = last run + checkpoint = -1 # -1 = last saved model + resume_path = None # updated from load_run and chkpt diff --git a/sim/humanoid_gym/envs/only_legs_env.py b/sim/humanoid_gym/envs/only_legs_env.py new file mode 100755 index 00000000..78f49008 --- /dev/null +++ b/sim/humanoid_gym/envs/only_legs_env.py @@ -0,0 +1,550 @@ +# mypy: disable-error-code="valid-newtype" +"""Defines the environment for training the humanoid.""" + +import torch # type: ignore[import] +from humanoid.envs import LeggedRobot +from humanoid.envs.base.legged_robot_config import LeggedRobotCfg +from humanoid.utils.terrain import HumanoidTerrain +from isaacgym.torch_utils import * + +from isaacgym import gymtorch +from sim.stompy_legs.joints import Stompy + + +class OnlyLegsFreeEnv(LeggedRobot): + """StompyFreeEnv is a class that represents a custom environment for a legged robot. + + Args: + cfg (LeggedRobotCfg): Configuration object for the legged robot. + sim_params: Parameters for the simulation. + physics_engine: Physics engine used in the simulation. + sim_device: Device used for the simulation. + headless: Flag indicating whether the simulation should be run in headless mode. + + Attributes: + last_feet_z (float): The z-coordinate of the last feet position. + feet_height (torch.Tensor): Tensor representing the height of the feet. + sim (gymtorch.GymSim): The simulation object. + terrain (HumanoidTerrain): The terrain object. + up_axis_idx (int): The index representing the up axis. + command_input (torch.Tensor): Tensor representing the command input. + privileged_obs_buf (torch.Tensor): Tensor representing the privileged observations buffer. + obs_buf (torch.Tensor): Tensor representing the observations buffer. + obs_history (collections.deque): Deque containing the history of observations. + critic_history (collections.deque): Deque containing the history of critic observations. + + Methods: + _push_robots(): Randomly pushes the robots by setting a randomized base velocity. + _get_phase(): Calculates the phase of the gait cycle. + _get_gait_phase(): Calculates the gait phase. + compute_ref_state(): Computes the reference state. + create_sim(): Creates the simulation, terrain, and environments. + _get_noise_scale_vec(cfg): Sets a vector used to scale the noise added to the observations. + step(actions): Performs a simulation step with the given actions. + compute_observations(): Computes the observations. + reset_idx(env_ids): Resets the environment for the specified environment IDs. + """ + + def __init__(self, cfg: LeggedRobotCfg, sim_params, physics_engine, sim_device, headless): + super().__init__(cfg, sim_params, physics_engine, sim_device, headless) + self.last_feet_z = self.cfg.asset.default_feet_height + self.feet_height = torch.zeros((self.num_envs, 2), device=self.device) + self.reset_idx(torch.tensor(range(self.num_envs), device=self.device)) + + env_handle = self.envs[0] + actor_handle = self.actor_handles[0] + + self.legs_joints = {} + for name, joint in Stompy.legs.left.joints_motors(): + joint_handle = self.gym.find_actor_dof_handle(env_handle, actor_handle, joint) + self.legs_joints["left_" + name] = joint_handle + + for name, joint in Stompy.legs.right.joints_motors(): + joint_handle = self.gym.find_actor_dof_handle(env_handle, actor_handle, joint) + self.legs_joints["right_" + name] = joint_handle + + self.compute_observations() + + def _push_robots(self): + """Random pushes the robots. Emulates an impulse by setting a randomized base velocity.""" + max_vel = self.cfg.domain_rand.max_push_vel_xy + max_push_angular = self.cfg.domain_rand.max_push_ang_vel + self.rand_push_force[:, :2] = torch_rand_float( + -max_vel, max_vel, (self.num_envs, 2), device=self.device + ) # lin vel x/y + self.root_states[:, 7:9] = self.rand_push_force[:, :2] + + self.rand_push_torque = torch_rand_float( + -max_push_angular, max_push_angular, (self.num_envs, 3), device=self.device + ) + self.root_states[:, 10:13] = self.rand_push_torque + + self.gym.set_actor_root_state_tensor(self.sim, gymtorch.unwrap_tensor(self.root_states)) + + def _get_phase(self): + cycle_time = self.cfg.rewards.cycle_time + phase = self.episode_length_buf * self.dt / cycle_time + return phase + + def _get_gait_phase(self): + # return float mask 1 is stance, 0 is swing + phase = self._get_phase() + sin_pos = torch.sin(2 * torch.pi * phase) + # Add double support phase + stance_mask = torch.zeros((self.num_envs, 2), device=self.device) + # left foot stance + stance_mask[:, 0] = sin_pos >= 0 + # right foot stance + stance_mask[:, 1] = sin_pos < 0 + # Double support phase + stance_mask[torch.abs(sin_pos) < 0.1] = 1 + + return stance_mask + + def check_termination(self): + """Check if environments need to be reset""" + self.reset_buf = torch.any( + torch.norm(self.contact_forces[:, self.termination_contact_indices, :], dim=-1) > 1.0, + dim=1, + ) + self.reset_buf |= self.root_states[:, 2] < self.cfg.asset.termination_height + self.time_out_buf = self.episode_length_buf > self.max_episode_length # no terminal reward for time-outs + self.reset_buf |= self.time_out_buf + + def compute_ref_state(self): + phase = self._get_phase() + sin_pos = torch.sin(2 * torch.pi * phase) + sin_pos_l = sin_pos.clone() + sin_pos_r = sin_pos.clone() + + self.ref_dof_pos = torch.zeros_like(self.dof_pos) + scale_1 = self.cfg.rewards.target_joint_pos_scale + scale_2 = 2 * scale_1 + # left foot stance phase set to default joint pos + sin_pos_l[sin_pos_l > 0] = 0 + self.ref_dof_pos[:, self.legs_joints["left_hip_pitch"]] = sin_pos_l * scale_1 + self.ref_dof_pos[:, self.legs_joints["left_knee_pitch"]] = sin_pos_l * scale_2 + self.ref_dof_pos[:, self.legs_joints["left_ankle_roll"]] = sin_pos_l * scale_1 + + # right foot stance phase set to default joint pos + sin_pos_r[sin_pos_r < 0] = 0 + self.ref_dof_pos[:, self.legs_joints["right_hip_pitch"]] = sin_pos_r * scale_1 + self.ref_dof_pos[:, self.legs_joints["right_knee_pitch"]] = sin_pos_r * scale_2 + self.ref_dof_pos[:, self.legs_joints["right_ankle_roll"]] = sin_pos_r * scale_1 + + # Double support phase + self.ref_dof_pos[torch.abs(sin_pos) < 0.1] = 0 + + self.ref_action = 2 * self.ref_dof_pos + + def create_sim(self): + """Creates simulation, terrain and evironments""" + self.up_axis_idx = 2 # 2 for z, 1 for y -> adapt gravity accordingly + self.sim = self.gym.create_sim( + self.sim_device_id, + self.graphics_device_id, + self.physics_engine, + self.sim_params, + ) + mesh_type = self.cfg.terrain.mesh_type + if mesh_type in ["heightfield", "trimesh"]: + self.terrain = HumanoidTerrain(self.cfg.terrain, self.num_envs) + if mesh_type == "plane": + self._create_ground_plane() + elif mesh_type == "heightfield": + self._create_heightfield() + elif mesh_type == "trimesh": + self._create_trimesh() + elif mesh_type is not None: + raise ValueError("Terrain mesh type not recognised. Allowed types are [None, plane, heightfield, trimesh]") + self._create_envs() + + def _get_noise_scale_vec(self, cfg): + """Sets a vector used to scale the noise added to the observations. + [NOTE]: Must be adapted when changing the observations structure + + Args: + cfg (Dict): Environment config file + + Returns: + [torch.Tensor]: Vector of scales used to multiply a uniform distribution in [-1, 1] + """ + num_actions = self.num_actions + noise_vec = torch.zeros(self.cfg.env.num_single_obs, device=self.device) + self.add_noise = self.cfg.noise.add_noise + noise_scales = self.cfg.noise.noise_scales + noise_vec[0:5] = 0.0 # commands + noise_vec[5 : (num_actions + 5)] = noise_scales.dof_pos * self.obs_scales.dof_pos + noise_vec[(num_actions + 5) : (2 * num_actions + 5)] = noise_scales.dof_vel * self.obs_scales.dof_vel + noise_vec[(2 * num_actions + 5) : (3 * num_actions + 5)] = 0.0 # previous actions + noise_vec[(3 * num_actions + 5) : (3 * num_actions + 5) + 3] = ( + noise_scales.ang_vel * self.obs_scales.ang_vel + ) # ang vel + noise_vec[(3 * num_actions + 5) + 3 : (3 * num_actions + 5)] = ( + noise_scales.quat * self.obs_scales.quat + ) # euler x,y + return noise_vec + + def step(self, actions): + if self.cfg.env.use_ref_actions: + actions += self.ref_action + # dynamic randomization + delay = torch.rand((self.num_envs, 1), device=self.device) + actions = (1 - delay) * actions + delay * self.actions + actions += self.cfg.domain_rand.dynamic_randomization * torch.randn_like(actions) * actions + return super().step(actions) + + def compute_observations(self): + phase = self._get_phase() + self.compute_ref_state() + + sin_pos = torch.sin(2 * torch.pi * phase).unsqueeze(1) + cos_pos = torch.cos(2 * torch.pi * phase).unsqueeze(1) + + stance_mask = self._get_gait_phase() + contact_mask = self.contact_forces[:, self.feet_indices, 2] > 5.0 + + self.command_input = torch.cat((sin_pos, cos_pos, self.commands[:, :3] * self.commands_scale), dim=1) + q = (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos + dq = self.dof_vel * self.obs_scales.dof_vel + + diff = self.dof_pos - self.ref_dof_pos + self.privileged_obs_buf = torch.cat( + ( + self.command_input, # 2 + 3 + (self.dof_pos - self.default_joint_pd_target) * self.obs_scales.dof_pos, # 12 + self.dof_vel * self.obs_scales.dof_vel, # 12 + self.actions, # 12 + diff, # 12 + self.base_lin_vel * self.obs_scales.lin_vel, # 3 + self.base_ang_vel * self.obs_scales.ang_vel, # 3 + self.base_euler_xyz * self.obs_scales.quat, # 3 + self.rand_push_force[:, :2], # 3 + self.rand_push_torque, # 3 + self.env_frictions, # 1 + self.body_mass / 30.0, # 1 + stance_mask, # 2 + contact_mask, # 2 + ), + dim=-1, + ) + + obs_buf = torch.cat( + ( + self.command_input, # 5 = 2D(sin cos) + 3D(vel_x, vel_y, aug_vel_yaw) + q, # 12D + dq, # 12D + self.actions, # 12D + self.base_ang_vel * self.obs_scales.ang_vel, # 3 + self.base_euler_xyz * self.obs_scales.quat, # 3 + ), + dim=-1, + ) + + if self.cfg.terrain.measure_heights: + heights = ( + torch.clip( + self.root_states[:, 2].unsqueeze(1) - 0.5 - self.measured_heights, + -1, + 1.0, + ) + * self.obs_scales.height_measurements + ) + self.privileged_obs_buf = torch.cat((self.obs_buf, heights), dim=-1) + + if self.add_noise: + obs_now = obs_buf.clone() + torch.randn_like(obs_buf) * self.noise_scale_vec * self.cfg.noise.noise_level + else: + obs_now = obs_buf.clone() + self.obs_history.append(obs_now) + self.critic_history.append(self.privileged_obs_buf) + + obs_buf_all = torch.stack([self.obs_history[i] for i in range(self.obs_history.maxlen)], dim=1) # N,T,K + + self.obs_buf = obs_buf_all.reshape(self.num_envs, -1) # N, T*K + self.privileged_obs_buf = torch.cat([self.critic_history[i] for i in range(self.cfg.env.c_frame_stack)], dim=1) + + def reset_idx(self, env_ids): + super().reset_idx(env_ids) + for i in range(self.obs_history.maxlen): + self.obs_history[i][env_ids] *= 0 + for i in range(self.critic_history.maxlen): + self.critic_history[i][env_ids] *= 0 + + # ================================================ Rewards ================================================== # + def _reward_joint_pos(self): + """ + Calculates the reward based on the difference between the current joint positions and the target joint positions. + """ + joint_pos = self.dof_pos.clone() + pos_target = self.ref_dof_pos.clone() + diff = joint_pos - pos_target + r = torch.exp(-2 * torch.norm(diff, dim=1)) - 0.2 * torch.norm(diff, dim=1).clamp(0, 0.5) + + return r + + def _reward_feet_distance(self): + """ + Calculates the reward based on the distance between the feet. Penilize feet get close to each other or too far away. + """ + foot_pos = self.rigid_state[:, self.feet_indices, :2] + foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1) + fd = self.cfg.rewards.min_dist + max_df = self.cfg.rewards.max_dist + d_min = torch.clamp(foot_dist - fd, -0.5, 0.0) + d_max = torch.clamp(foot_dist - max_df, 0, 0.5) + return (torch.exp(-torch.abs(d_min) * 100) + torch.exp(-torch.abs(d_max) * 100)) / 2 + + def _reward_knee_distance(self): + """ + Calculates the reward based on the distance between the knee of the humanoid. + """ + foot_pos = self.rigid_state[:, self.knee_indices, :2] + foot_dist = torch.norm(foot_pos[:, 0, :] - foot_pos[:, 1, :], dim=1) + fd = self.cfg.rewards.min_dist + max_df = self.cfg.rewards.max_dist / 2 + d_min = torch.clamp(foot_dist - fd, -0.5, 0.0) + d_max = torch.clamp(foot_dist - max_df, 0, 0.5) + return (torch.exp(-torch.abs(d_min) * 100) + torch.exp(-torch.abs(d_max) * 100)) / 2 + + def _reward_foot_slip(self): + """ + Calculates the reward for minimizing foot slip. The reward is based on the contact forces + and the speed of the feet. A contact threshold is used to determine if the foot is in contact + with the ground. The speed of the foot is calculated and scaled by the contact condition. + """ + contact = self.contact_forces[:, self.feet_indices, 2] > 5.0 + foot_speed_norm = torch.norm(self.rigid_state[:, self.feet_indices, 10:12], dim=2) + rew = torch.sqrt(foot_speed_norm) + rew *= contact + return torch.sum(rew, dim=1) + + def _reward_feet_air_time(self): + """ + Calculates the reward for feet air time, promoting longer steps. This is achieved by + checking the first contact with the ground after being in the air. The air time is + limited to a maximum value for reward calculation. + """ + contact = self.contact_forces[:, self.feet_indices, 2] > 5.0 + stance_mask = self._get_gait_phase() + self.contact_filt = torch.logical_or(torch.logical_or(contact, stance_mask), self.last_contacts) + self.last_contacts = contact + first_contact = (self.feet_air_time > 0.0) * self.contact_filt + self.feet_air_time += self.dt + air_time = self.feet_air_time.clamp(0, 0.5) * first_contact + self.feet_air_time *= ~self.contact_filt + return air_time.sum(dim=1) + + def _reward_feet_contact_number(self): + """ + Calculates a reward based on the number of feet contacts aligning with the gait phase. + Rewards or penalizes depending on whether the foot contact matches the expected gait phase. + """ + contact = self.contact_forces[:, self.feet_indices, 2] > 5.0 + stance_mask = self._get_gait_phase() + reward = torch.where(contact == stance_mask, 1, -0.3) + return torch.mean(reward, dim=1) + + def _reward_orientation(self): + """ + Calculates the reward for maintaining a flat base orientation. It penalizes deviation + from the desired base orientation using the base euler angles and the projected gravity vector. + """ + quat_mismatch = torch.exp(-torch.sum(torch.abs(self.base_euler_xyz[:, :2]), dim=1) * 10) + orientation = torch.exp(-torch.norm(self.projected_gravity[:, :2], dim=1) * 20) + + return (quat_mismatch + orientation) / 2.0 + + def _reward_feet_contact_forces(self): + """ + Calculates the reward for keeping contact forces within a specified range. Penalizes + high contact forces on the feet. + """ + return torch.sum( + ( + torch.norm(self.contact_forces[:, self.feet_indices, :], dim=-1) - self.cfg.rewards.max_contact_force + ).clip(0, 400), + dim=1, + ) + + def _reward_default_joint_pos(self): + """ + Calculates the reward for keeping joint positions close to default positions, with a focus + on penalizing deviation in yaw and roll directions. Excludes yaw and roll from the main penalty. + """ + joint_diff = self.dof_pos - self.default_joint_pd_target + left_yaw_roll = joint_diff[:, :2] + right_yaw_roll = joint_diff[:, 6:8] + yaw_roll = torch.norm(left_yaw_roll, dim=1) + torch.norm(right_yaw_roll, dim=1) + yaw_roll = torch.clamp(yaw_roll - 0.1, 0, 50) + return torch.exp(-yaw_roll * 100) - 0.01 * torch.norm(joint_diff, dim=1) + + def _reward_base_height(self): + """ + Calculates the reward based on the robot's base height. Penalizes deviation from a target base height. + The reward is computed based on the height difference between the robot's base and the average height + of its feet when they are in contact with the ground. + """ + stance_mask = self._get_gait_phase() + measured_heights = torch.sum(self.rigid_state[:, self.feet_indices, 2] * stance_mask, dim=1) / torch.sum( + stance_mask, dim=1 + ) + base_height = self.root_states[:, 2] - (measured_heights - self.cfg.asset.default_feet_height) + reward = torch.exp(-torch.abs(base_height - self.cfg.rewards.base_height_target) * 100) + return reward + + def _reward_base_acc(self): + """ + Computes the reward based on the base's acceleration. Penalizes high accelerations of the robot's base, + encouraging smoother motion. + """ + root_acc = self.last_root_vel - self.root_states[:, 7:13] + rew = torch.exp(-torch.norm(root_acc, dim=1) * 3) + return rew + + def _reward_vel_mismatch_exp(self): + """ + Computes a reward based on the mismatch in the robot's linear and angular velocities. + Encourages the robot to maintain a stable velocity by penalizing large deviations. + """ + lin_mismatch = torch.exp(-torch.square(self.base_lin_vel[:, 2]) * 10) + ang_mismatch = torch.exp(-torch.norm(self.base_ang_vel[:, :2], dim=1) * 5.0) + + c_update = (lin_mismatch + ang_mismatch) / 2.0 + + return c_update + + def _reward_track_vel_hard(self): + """ + Calculates a reward for accurately tracking both linear and angular velocity commands. + Penalizes deviations from specified linear and angular velocity targets. + """ + # Tracking of linear velocity commands (xy axes) + lin_vel_error = torch.norm(self.commands[:, :2] - self.base_lin_vel[:, :2], dim=1) + lin_vel_error_exp = torch.exp(-lin_vel_error * 10) + + # Tracking of angular velocity commands (yaw) + ang_vel_error = torch.abs(self.commands[:, 2] - self.base_ang_vel[:, 2]) + ang_vel_error_exp = torch.exp(-ang_vel_error * 10) + + linear_error = 0.2 * (lin_vel_error + ang_vel_error) + + return (lin_vel_error_exp + ang_vel_error_exp) / 2.0 - linear_error + + def _reward_tracking_lin_vel(self): + """ + Tracks linear velocity commands along the xy axes. + Calculates a reward based on how closely the robot's linear velocity matches the commanded values. + """ + lin_vel_error = torch.sum(torch.square(self.commands[:, :2] - self.base_lin_vel[:, :2]), dim=1) + return torch.exp(-lin_vel_error * self.cfg.rewards.tracking_sigma) + + def _reward_tracking_ang_vel(self): + """ + Tracks angular velocity commands for yaw rotation. + Computes a reward based on how closely the robot's angular velocity matches the commanded yaw values. + """ + + ang_vel_error = torch.square(self.commands[:, 2] - self.base_ang_vel[:, 2]) + return torch.exp(-ang_vel_error * self.cfg.rewards.tracking_sigma) + + def _reward_feet_clearance(self): + """ + Calculates reward based on the clearance of the swing leg from the ground during movement. + Encourages appropriate lift of the feet during the swing phase of the gait. + """ + + # Compute feet contact mask + contact = self.contact_forces[:, self.feet_indices, 2] > 5.0 + + # Get the z-position of the feet and compute the change in z-position + feet_z = self.rigid_state[:, self.feet_indices, 2] - self.cfg.asset.default_feet_height + delta_z = feet_z - self.last_feet_z + self.feet_height += delta_z + self.last_feet_z = feet_z + + # Compute swing mask + swing_mask = 1 - self._get_gait_phase() + + # feet height should be closed to target feet height at the peak + rew_pos = torch.abs(self.feet_height - self.cfg.rewards.target_feet_height) < 0.02 + rew_pos = torch.sum(rew_pos * swing_mask, dim=1) + self.feet_height *= ~contact + + return rew_pos + + def _reward_low_speed(self): + """ + Rewards or penalizes the robot based on its speed relative to the commanded speed. + This function checks if the robot is moving too slow, too fast, or at the desired speed, + and if the movement direction matches the command. + """ + # Calculate the absolute value of speed and command for comparison + absolute_speed = torch.abs(self.base_lin_vel[:, 0]) + absolute_command = torch.abs(self.commands[:, 0]) + + # Define speed criteria for desired range + speed_too_low = absolute_speed < 0.5 * absolute_command + speed_too_high = absolute_speed > 1.2 * absolute_command + speed_desired = ~(speed_too_low | speed_too_high) + + # Check if the speed and command directions are mismatched + sign_mismatch = torch.sign(self.base_lin_vel[:, 0]) != torch.sign(self.commands[:, 0]) + + # Initialize reward tensor + reward = torch.zeros_like(self.base_lin_vel[:, 0]) + + # Assign rewards based on conditions + # Speed too low + reward[speed_too_low] = -1.0 + # Speed too high + reward[speed_too_high] = 0.0 + # Speed within desired range + reward[speed_desired] = 1.2 + # Sign mismatch has the highest priority + reward[sign_mismatch] = -2.0 + return reward * (self.commands[:, 0].abs() > 0.1) + + def _reward_torques(self): + """ + Penalizes the use of high torques in the robot's joints. Encourages efficient movement by minimizing + the necessary force exerted by the motors. + """ + return torch.sum(torch.square(self.torques), dim=1) + + def _reward_dof_vel(self): + """ + Penalizes high velocities at the degrees of freedom (DOF) of the robot. This encourages smoother and + more controlled movements. + """ + return torch.sum(torch.square(self.dof_vel), dim=1) + + def _reward_dof_acc(self): + """ + Penalizes high accelerations at the robot's degrees of freedom (DOF). This is important for ensuring + smooth and stable motion, reducing wear on the robot's mechanical parts. + """ + return torch.sum(torch.square((self.last_dof_vel - self.dof_vel) / self.dt), dim=1) + + def _reward_collision(self): + """ + Penalizes collisions of the robot with the environment, specifically focusing on selected body parts. + This encourages the robot to avoid undesired contact with objects or surfaces. + """ + return torch.sum( + 1.0 * (torch.norm(self.contact_forces[:, self.penalised_contact_indices, :], dim=-1) > 0.1), + dim=1, + ) + + def _reward_action_smoothness(self): + """ + Encourages smoothness in the robot's actions by penalizing large differences between consecutive actions. + This is important for achieving fluid motion and reducing mechanical stress. + """ + term_1 = torch.sum(torch.square(self.last_actions - self.actions), dim=1) + term_2 = torch.sum( + torch.square(self.actions + self.last_last_actions - 2 * self.last_actions), + dim=1, + ) + term_3 = 0.05 * torch.sum(torch.abs(self.actions), dim=1) + return term_1 + term_2 + term_3 diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index ee6216bb..92496b24 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -195,6 +195,8 @@ class scales: vel_mismatch_exp = 0.5 # lin_z; ang x,y low_speed = 0.2 track_vel_hard = 0.5 + + # above this was removed # base pos default_joint_pos = 0.5 orientation = 1 From 54fbd28191ea9e5d72f47218178e364ea81fa6e0 Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Wed, 10 Jul 2024 17:20:37 -0700 Subject: [PATCH 16/21] linking latest humanoid-gym --- third_party/humanoid-gym | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/third_party/humanoid-gym b/third_party/humanoid-gym index 9315d813..dfb38549 160000 --- a/third_party/humanoid-gym +++ b/third_party/humanoid-gym @@ -1 +1 @@ -Subproject commit 9315d8135f24fa4a798249267991e797f895b43a +Subproject commit dfb38549f3f06d723fee2827c1877f1ca7179fab From d414cc3c620e0e7072ec83ff10ab6db2d0b99460 Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Wed, 10 Jul 2024 22:55:09 -0700 Subject: [PATCH 17/21] setup working for mjcf --- sim/stompy_legs/joints.py | 165 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100755 sim/stompy_legs/joints.py diff --git a/sim/stompy_legs/joints.py b/sim/stompy_legs/joints.py new file mode 100755 index 00000000..3f4ffdd8 --- /dev/null +++ b/sim/stompy_legs/joints.py @@ -0,0 +1,165 @@ +"""Defines a more Pythonic interface for specifying the joint names. + +The best way to re-generate this snippet for a new robot is to use the +`sim/scripts/print_joints.py` script. This script will print out a hierarchical +tree of the various joint names in the robot. +""" + +import textwrap +from abc import ABC +from typing import Dict, List, Tuple, Union + + +class Node(ABC): + @classmethod + def children(cls) -> List["Union[Node, str]"]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, (Node, str)) + ] + + @classmethod + def joints(cls) -> List[str]: + return [ + attr + for attr in (getattr(cls, attr) for attr in dir(cls) if not attr.startswith("__")) + if isinstance(attr, str) + ] + + @classmethod + def joints_motors(cls) -> List[Tuple[str, str]]: + joint_names: List[Tuple[str, str]] = [] + for attr in dir(cls): + if not attr.startswith("__"): + attr2 = getattr(cls, attr) + if isinstance(attr2, str): + joint_names.append((attr, attr2)) + + return joint_names + + @classmethod + def all_joints(cls) -> List[str]: + leaves = cls.joints() + for child in cls.children(): + if isinstance(child, Node): + leaves.extend(child.all_joints()) + return leaves + + def __str__(self) -> str: + parts = [str(child) for child in self.children()] + parts_str = textwrap.indent("\n" + "\n".join(parts), " ") + return f"[{self.__class__.__name__}]{parts_str}" + + +class LeftLeg(Node): + hip_roll = "left hip roll" + hip_yaw = "left hip yaw" + hip_pitch = "left hip pitch" + knee_pitch = "left knee pitch" + ankle_pitch = "left ankle pitch" + ankle_roll = "left ankle roll" + + +class RightLeg(Node): + hip_roll = "right hip roll" + hip_yaw = "right hip yaw" + hip_pitch = "right hip pitch" + knee_pitch = "right knee pitch" + ankle_pitch = "right ankle pitch" + ankle_roll = "right ankle roll" + + +class Legs(Node): + left = LeftLeg() + right = RightLeg() + + +class Stompy(Node): + legs = Legs() + + @classmethod + def default_positions(cls) -> Dict[str, float]: + return {} + + @classmethod + def default_standing(cls) -> Dict[str, float]: + return { + # legs + Stompy.legs.left.hip_pitch: -0.12, + Stompy.legs.left.hip_roll: 1.44, + Stompy.legs.left.hip_yaw: -1.19, + Stompy.legs.left.knee_pitch: -2.32, + Stompy.legs.left.ankle_pitch: 0.56, + Stompy.legs.left.ankle_roll: -2.64, + Stompy.legs.right.hip_pitch: -4.33, + Stompy.legs.right.hip_roll: 3.14, + Stompy.legs.right.hip_yaw: -1.13, + Stompy.legs.right.knee_pitch: -1.95, + Stompy.legs.right.ankle_pitch: 0.62, + Stompy.legs.right.ankle_roll: -2.64, + } + + @classmethod + def default_limits(cls) -> Dict[str, Dict[str, float]]: + return { + Stompy.legs.left.hip_pitch: { + "lower": -1.32, + "upper": 0.69, + }, + Stompy.legs.left.hip_roll: { + "lower": 1.13, + "upper": 2.14, + }, + Stompy.legs.left.hip_yaw: { + "lower": -2.2, + "upper": -1.19, + }, + Stompy.legs.left.knee_pitch: { + "lower": -3.14, + "upper": -2.2, + }, + Stompy.legs.left.ankle_pitch: { + "lower": 0.44, + "upper": 1.13, + }, + Stompy.legs.left.ankle_roll: { + "lower": -3.08, + "upper": -2.26, + }, + Stompy.legs.right.hip_pitch: { + "lower": -5.00, + "upper": -3.83, + }, + Stompy.legs.right.hip_roll: { + "lower": 2.39, + "upper": 3.33, + }, + Stompy.legs.right.hip_yaw: { + "lower": -1.13, + "upper": -0.69, + }, + Stompy.legs.right.knee_pitch: { + "lower": -1.95, + "upper": -1.00, + }, + Stompy.legs.right.ankle_pitch: { + "lower": 0.00, + "upper": 0.94, + }, + Stompy.legs.right.ankle_roll: { + "lower": -3.00, + "upper": -2.14, + }, + } + + +def print_joints() -> None: + joints = Stompy.all_joints() + assert len(joints) == len(set(joints)), "Duplicate joint names found!" + print(Stompy()) + + +if __name__ == "__main__": + # python -m sim.stompy.joints + print_joints() From 03bbe4701591c4e870f66b0ef47c0b9163deaed0 Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Thu, 11 Jul 2024 11:09:08 -0700 Subject: [PATCH 18/21] saving new modle with inertia, training works --- sim/humanoid_gym/envs/only_legs_config.py | 82 +- sim/humanoid_gym/envs/stompy_config.py | 34 +- sim/scripts/create_fixed_torso.py | 10 +- sim/stompy_legs/joints.py | 68 +- sim/stompy_legs/robot_fixed.urdf | 1263 +++++++++++++++++++++ 5 files changed, 1360 insertions(+), 97 deletions(-) create mode 100644 sim/stompy_legs/robot_fixed.urdf diff --git a/sim/humanoid_gym/envs/only_legs_config.py b/sim/humanoid_gym/envs/only_legs_config.py index 9461839e..8af5849d 100755 --- a/sim/humanoid_gym/envs/only_legs_config.py +++ b/sim/humanoid_gym/envs/only_legs_config.py @@ -32,24 +32,24 @@ class env(LeggedRobotCfg.env): class safety: # safety factors - pos_limit = 1.0 - vel_limit = 1.0 - torque_limit = 1.0 + pos_limit = .9 + vel_limit = .9 + torque_limit = .9 class asset(LeggedRobotCfg.asset): - file = str(stompy_urdf_path()) + file = str(stompy_urdf_path(legs_only=True)) name = "stompy" foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - termination_height = 0.23 + termination_height = 0.23 # TODO: find a suitable value default_feet_height = 0.0 - terminate_after_contacts_on = ["link_upper_limb_assembly_7_dof_1_torso_1_top_skeleton_2"] + terminate_after_contacts_on = [] # TODO: find a part for this penalize_contacts_on = [] - self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter + self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter flip_visual_attachments = False replace_cylinder_with_capsule = False fix_base_link = False @@ -72,7 +72,7 @@ class terrain(LeggedRobotCfg.terrain): restitution = 0.0 class noise: - add_noise = True + add_noise = False noise_level = 0.6 # scales other values class noise_scales: @@ -84,7 +84,7 @@ class noise_scales: height_measurements = 0.1 class init_state(LeggedRobotCfg.init_state): - pos = [0.0, 0.0, 1.15] + pos = [0.0, 0.0, .72] default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} @@ -95,14 +95,14 @@ class init_state(LeggedRobotCfg.init_state): class control(LeggedRobotCfg.control): # PD Drive parameters: stiffness = { - "shoulder": 200, - "elbow": 200, - "wrist": 200, - "hand": 200, - "torso": 200, - "hip": 200, - "ankle": 200, - "knee": 200, + "shoulder": 100, + "elbow": 100, + "wrist": 100, + "hand": 100, + "torso": 100, + "hip": 100, + "ankle": 100, + "knee": 100, } damping = { "shoulder": 10, @@ -126,11 +126,11 @@ class sim(LeggedRobotCfg.sim): class physx(LeggedRobotCfg.sim.physx): num_threads = 12 - solver_type = 1 # 0: pgs, 1: tgs + solver_type = 0 # 0: pgs, 1: tgs num_position_iterations = 4 - num_velocity_iterations = 0 - contact_offset = 0.01 # [m] - rest_offset = -0.02 # [m] + num_velocity_iterations = 1 + contact_offset = 0.0 # [m] + rest_offset = 0 #-0.02 # [m] bounce_threshold_velocity = 0.1 # [m/s] max_depenetration_velocity = 1.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more @@ -139,12 +139,12 @@ class physx(LeggedRobotCfg.sim.physx): contact_collection = 2 class domain_rand: - randomize_friction = True + randomize_friction = False friction_range = [0.1, 2.0] - randomize_base_mass = True + randomize_base_mass = False added_mass_range = [-1.0, 1.0] - push_robots = True + push_robots = False push_interval_s = 4 max_push_vel_xy = 0.2 max_push_ang_vel = 0.4 @@ -178,23 +178,23 @@ class rewards: max_contact_force = 400 # forces above this value are penalized class scales: - # reference motion tracking - joint_pos = 1.6 - feet_clearance = 1.0 - feet_contact_number = 1.2 - # gait - feet_air_time = 1.0 - foot_slip = -0.05 - feet_distance = 0.2 - knee_distance = 0.2 - # contact - feet_contact_forces = -0.01 - # vel tracking - tracking_lin_vel = 1.2 - tracking_ang_vel = 1.1 - vel_mismatch_exp = 0.5 # lin_z; ang x,y - low_speed = 0.2 - track_vel_hard = 0.5 + # # reference motion tracking + # joint_pos = 1.6 + # feet_clearance = 1.0 + # feet_contact_number = 1.2 + # # gait + # feet_air_time = 1.0 + # foot_slip = -0.05 + # feet_distance = 0.2 + # knee_distance = 0.2 + # # contact + # feet_contact_forces = -0.01 + # # vel tracking + # tracking_lin_vel = 1.2 + # tracking_ang_vel = 1.1 + # vel_mismatch_exp = 0.5 # lin_z; ang x,y + # low_speed = 0.2 + # track_vel_hard = 0.5 # above this was removed # base pos diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 92496b24..01164782 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -178,23 +178,23 @@ class rewards: max_contact_force = 400 # forces above this value are penalized class scales: - # reference motion tracking - joint_pos = 1.6 - feet_clearance = 1.0 - feet_contact_number = 1.2 - # gait - feet_air_time = 1.0 - foot_slip = -0.05 - feet_distance = 0.2 - knee_distance = 0.2 - # contact - feet_contact_forces = -0.01 - # vel tracking - tracking_lin_vel = 1.2 - tracking_ang_vel = 1.1 - vel_mismatch_exp = 0.5 # lin_z; ang x,y - low_speed = 0.2 - track_vel_hard = 0.5 + # # reference motion tracking + # joint_pos = 1.6 + # feet_clearance = 1.0 + # feet_contact_number = 1.2 + # # gait + # feet_air_time = 1.0 + # foot_slip = -0.05 + # feet_distance = 0.2 + # knee_distance = 0.2 + # # contact + # feet_contact_forces = -0.01 + # # vel tracking + # tracking_lin_vel = 1.2 + # tracking_ang_vel = 1.1 + # vel_mismatch_exp = 0.5 # lin_z; ang x,y + # low_speed = 0.2 + # track_vel_hard = 0.5 # above this was removed # base pos diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py index 861fbdf0..36bbcf10 100755 --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -3,16 +3,16 @@ import xml.etree.ElementTree as ET -from sim.stompy.joints import StompyFixed +from sim.stompy_legs.joints import Stompy -STOMPY_URDF = "sim/stompy/robot.urdf" +STOMPY_URDF = "sim/stompy_legs/robot.urdf" def update_urdf() -> None: tree = ET.parse(STOMPY_URDF) root = tree.getroot() - stompy = StompyFixed() - + stompy = Stompy() + print(stompy.default_standing()) revolute_joints = set(stompy.default_standing().keys()) joint_limits = stompy.default_limits() @@ -30,7 +30,7 @@ def update_urdf() -> None: limit.set("upper", upper) # Save the modified URDF to a new file - tree.write("sim/stompy/robot_fixed.urdf") + tree.write("sim/stompy_legs/robot_fixed.urdf") if __name__ == "__main__": diff --git a/sim/stompy_legs/joints.py b/sim/stompy_legs/joints.py index 3f4ffdd8..f669598e 100755 --- a/sim/stompy_legs/joints.py +++ b/sim/stompy_legs/joints.py @@ -86,70 +86,70 @@ def default_positions(cls) -> Dict[str, float]: def default_standing(cls) -> Dict[str, float]: return { # legs - Stompy.legs.left.hip_pitch: -0.12, - Stompy.legs.left.hip_roll: 1.44, - Stompy.legs.left.hip_yaw: -1.19, - Stompy.legs.left.knee_pitch: -2.32, - Stompy.legs.left.ankle_pitch: 0.56, - Stompy.legs.left.ankle_roll: -2.64, - Stompy.legs.right.hip_pitch: -4.33, - Stompy.legs.right.hip_roll: 3.14, - Stompy.legs.right.hip_yaw: -1.13, - Stompy.legs.right.knee_pitch: -1.95, - Stompy.legs.right.ankle_pitch: 0.62, - Stompy.legs.right.ankle_roll: -2.64, + Stompy.legs.left.hip_pitch: 1.61, + Stompy.legs.left.hip_roll: 0, + Stompy.legs.left.hip_yaw: 1, + Stompy.legs.left.knee_pitch: 2.05, + Stompy.legs.left.ankle_pitch: 0.33, + Stompy.legs.left.ankle_roll: 1.73, + Stompy.legs.right.hip_pitch: 0, + Stompy.legs.right.hip_roll: -1.6, + Stompy.legs.right.hip_yaw: -2.15, + Stompy.legs.right.knee_pitch: 2.16, + Stompy.legs.right.ankle_pitch: 0.5, + Stompy.legs.right.ankle_roll: 1.72, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { Stompy.legs.left.hip_pitch: { - "lower": -1.32, - "upper": 0.69, + "lower": 0.5, + "upper": 2.69, }, Stompy.legs.left.hip_roll: { - "lower": 1.13, - "upper": 2.14, + "lower": -0.5, + "upper": 0.5, }, Stompy.legs.left.hip_yaw: { - "lower": -2.2, - "upper": -1.19, + "lower": 0.5, + "upper": 1.19, }, Stompy.legs.left.knee_pitch: { - "lower": -3.14, - "upper": -2.2, + "lower": 0.5, + "upper": 2.1, }, Stompy.legs.left.ankle_pitch: { - "lower": 0.44, + "lower": 0.0, "upper": 1.13, }, Stompy.legs.left.ankle_roll: { - "lower": -3.08, - "upper": -2.26, + "lower": 1.3, + "upper": 2, }, Stompy.legs.right.hip_pitch: { - "lower": -5.00, - "upper": -3.83, + "lower": -1, + "upper": 1, }, Stompy.legs.right.hip_roll: { - "lower": 2.39, - "upper": 3.33, + "lower": -2.39, + "upper": -1, }, Stompy.legs.right.hip_yaw: { - "lower": -1.13, - "upper": -0.69, + "lower": -2.2, + "upper": -1, }, Stompy.legs.right.knee_pitch: { - "lower": -1.95, - "upper": -1.00, + "lower": 1.84, + "upper": 3, }, Stompy.legs.right.ankle_pitch: { - "lower": 0.00, + "lower": -0.5, "upper": 0.94, }, Stompy.legs.right.ankle_roll: { - "lower": -3.00, - "upper": -2.14, + "lower": 1, + "upper": 2, }, } diff --git a/sim/stompy_legs/robot_fixed.urdf b/sim/stompy_legs/robot_fixed.urdf new file mode 100644 index 00000000..be40daa8 --- /dev/null +++ b/sim/stompy_legs/robot_fixed.urdf @@ -0,0 +1,1263 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 440ebdcaf85a2ba15cb161a48d65c35caf06d5d8 Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Thu, 11 Jul 2024 11:48:56 -0700 Subject: [PATCH 19/21] saving model with inertia, training works --- third_party/humanoid-gym | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/third_party/humanoid-gym b/third_party/humanoid-gym index dfb38549..4b54818b 160000 --- a/third_party/humanoid-gym +++ b/third_party/humanoid-gym @@ -1 +1 @@ -Subproject commit dfb38549f3f06d723fee2827c1877f1ca7179fab +Subproject commit 4b54818b1302e86f0c289863126b4854b1749578 From b1335a9aa665aa0eb97fdf255f3de7d71ff272ed Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Sat, 13 Jul 2024 00:24:25 -0700 Subject: [PATCH 20/21] latest settings for making legs_only ppo realistic --- sim/humanoid_gym/envs/only_legs_config.py | 45 ++++++++---- sim/stompy_legs/__init__.py | 0 sim/stompy_legs/joints.py | 86 +++++++++++++++++++++-- sim/stompy_legs/robot_fixed.urdf | 8 +-- 4 files changed, 114 insertions(+), 25 deletions(-) create mode 100755 sim/stompy_legs/__init__.py diff --git a/sim/humanoid_gym/envs/only_legs_config.py b/sim/humanoid_gym/envs/only_legs_config.py index 8af5849d..1445c52f 100755 --- a/sim/humanoid_gym/envs/only_legs_config.py +++ b/sim/humanoid_gym/envs/only_legs_config.py @@ -44,12 +44,18 @@ class asset(LeggedRobotCfg.asset): foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - termination_height = 0.23 # TODO: find a suitable value + # 0.23 + termination_height = 0.23 default_feet_height = 0.0 - terminate_after_contacts_on = [] # TODO: find a part for this + terminate_after_contacts_on = ["link_leg_assembly_left_1_leg_part_1_2", "link_leg_assembly_right_1_leg_part_1_2"] penalize_contacts_on = [] self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter + # self_collisions = 1 + + # is2ac + collapse_fixed_joints = True + flip_visual_attachments = False replace_cylinder_with_capsule = False fix_base_link = False @@ -95,14 +101,14 @@ class init_state(LeggedRobotCfg.init_state): class control(LeggedRobotCfg.control): # PD Drive parameters: stiffness = { - "shoulder": 100, - "elbow": 100, - "wrist": 100, - "hand": 100, - "torso": 100, - "hip": 100, - "ankle": 100, - "knee": 100, + "shoulder": 200, + "elbow": 200, + "wrist": 200, + "hand": 200, + "torso": 200, + "hip": 250, + "ankle": 15, + "knee": 350, } damping = { "shoulder": 10, @@ -114,8 +120,15 @@ class control(LeggedRobotCfg.control): "ankle": 10, "knee": 10, } + # set all to a certain default value for now + for k in stiffness: + stiffness[k]*= .01 + damping[k] *= .01 + # print(stiffness) + # print(damping) # action scale: target angle = actionScale * action + defaultAngle - action_scale = 0.25 + # action_scale = 0.25 + action_scale = 0.5 # decimation: Number of control action updates @ sim DT per policy DT decimation = 10 # 100hz @@ -129,8 +142,8 @@ class physx(LeggedRobotCfg.sim.physx): solver_type = 0 # 0: pgs, 1: tgs num_position_iterations = 4 num_velocity_iterations = 1 - contact_offset = 0.0 # [m] - rest_offset = 0 #-0.02 # [m] + contact_offset = 0.01 # [m] + rest_offset = 0.0 #-0.02 # [m] bounce_threshold_velocity = 0.1 # [m/s] max_depenetration_velocity = 1.0 max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more @@ -164,7 +177,7 @@ class ranges: class rewards: # quite important to keep it right - base_height_target = 0.97 + base_height_target = 0.72 min_dist = 0.2 max_dist = 0.5 # put some settings here for LLM parameter tuning @@ -201,16 +214,18 @@ class scales: default_joint_pos = 0.5 orientation = 1 base_height = 0.2 - base_acc = 0.2 + # energy action_smoothness = -0.002 torques = -1e-5 dof_vel = -5e-4 dof_acc = -1e-7 + base_acc = 0.2 collision = -1.0 class normalization: class obs_scales: + #is2ac lin_vel = 2.0 ang_vel = 1.0 dof_pos = 1.0 diff --git a/sim/stompy_legs/__init__.py b/sim/stompy_legs/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/sim/stompy_legs/joints.py b/sim/stompy_legs/joints.py index f669598e..0aecdeaa 100755 --- a/sim/stompy_legs/joints.py +++ b/sim/stompy_legs/joints.py @@ -85,7 +85,7 @@ def default_positions(cls) -> Dict[str, float]: @classmethod def default_standing(cls) -> Dict[str, float]: return { - # legs + # # # legs Stompy.legs.left.hip_pitch: 1.61, Stompy.legs.left.hip_roll: 0, Stompy.legs.left.hip_yaw: 1, @@ -98,11 +98,85 @@ def default_standing(cls) -> Dict[str, float]: Stompy.legs.right.knee_pitch: 2.16, Stompy.legs.right.ankle_pitch: 0.5, Stompy.legs.right.ankle_roll: 1.72, + # legs - squat from urdf + # Stompy.legs.left.hip_pitch: 1.3479, + # Stompy.legs.left.hip_roll: 0.0821, + # Stompy.legs.left.hip_yaw: 0.9936, + # Stompy.legs.left.knee_pitch: 1.1113, + # Stompy.legs.left.ankle_pitch: 0.33, + # Stompy.legs.left.ankle_roll: 1.7218, + # Stompy.legs.right.hip_pitch: 0.2051, + # Stompy.legs.right.hip_roll: -1.5596, + # Stompy.legs.right.hip_yaw: -2.08, + # Stompy.legs.right.knee_pitch: 2.8126, + # Stompy.legs.right.ankle_pitch: 0.8182, + # Stompy.legs.right.ankle_roll: 1.7821, + # legs + # Stompy.legs.left.hip_pitch: -0.12, + # Stompy.legs.left.hip_roll: 1.44, + # Stompy.legs.left.hip_yaw: -1.19, + # Stompy.legs.left.knee_pitch: -2.32, + # Stompy.legs.left.ankle_pitch: 0.56, + # Stompy.legs.left.ankle_roll: -2.64, + # Stompy.legs.right.hip_pitch: -4.33, + # Stompy.legs.right.hip_roll: 3.14, + # Stompy.legs.right.hip_yaw: -1.10, + # Stompy.legs.right.knee_pitch: -1.90, + # Stompy.legs.right.ankle_pitch: 0.62, + # Stompy.legs.right.ankle_roll: -2.64, } @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { + # Stompy.legs.left.hip_pitch: { + # "lower": -1.32, + # "upper": 0.69, + # }, + # Stompy.legs.left.hip_roll: { + # "lower": 1.13, + # "upper": 2.14, + # }, + # Stompy.legs.left.hip_yaw: { + # "lower": -2.2, + # "upper": -1.01, + # }, + # Stompy.legs.left.knee_pitch: { + # "lower": -3.14, + # "upper": -2.2, + # }, + # Stompy.legs.left.ankle_pitch: { + # "lower": -0.14, + # "upper": 1.13, + # }, + # Stompy.legs.left.ankle_roll: { + # "lower": -3.08, + # "upper": -2.26, + # }, + # Stompy.legs.right.hip_pitch: { + # "lower": -5.00, + # "upper": -3.83, + # }, + # Stompy.legs.right.hip_roll: { + # "lower": 2.39, + # "upper": 3.33, + # }, + # Stompy.legs.right.hip_yaw: { + # "lower": -1.13, + # "upper": -0.69, + # }, + # Stompy.legs.right.knee_pitch: { + # "lower": -1.95, + # "upper": -1.00, + # }, + # Stompy.legs.right.ankle_pitch: { + # "lower": 0.50, + # "upper": 1.54, + # }, + # Stompy.legs.right.ankle_roll: { + # "lower": -3.00, + # "upper": -2.14, + # }, Stompy.legs.left.hip_pitch: { "lower": 0.5, "upper": 2.69, @@ -116,11 +190,11 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]: "upper": 1.19, }, Stompy.legs.left.knee_pitch: { - "lower": 0.5, - "upper": 2.1, + "lower": 1, + "upper": 3.1, }, Stompy.legs.left.ankle_pitch: { - "lower": 0.0, + "lower": -0.3, "upper": 1.13, }, Stompy.legs.left.ankle_roll: { @@ -140,7 +214,7 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]: "upper": -1, }, Stompy.legs.right.knee_pitch: { - "lower": 1.84, + "lower": 1.54, "upper": 3, }, Stompy.legs.right.ankle_pitch: { @@ -149,7 +223,7 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]: }, Stompy.legs.right.ankle_roll: { "lower": 1, - "upper": 2, + "upper": 2.3, }, } diff --git a/sim/stompy_legs/robot_fixed.urdf b/sim/stompy_legs/robot_fixed.urdf index be40daa8..aa14d37c 100644 --- a/sim/stompy_legs/robot_fixed.urdf +++ b/sim/stompy_legs/robot_fixed.urdf @@ -680,7 +680,7 @@ - + @@ -709,7 +709,7 @@ - + @@ -900,7 +900,7 @@ - + @@ -1149,7 +1149,7 @@ - + From b89bf6267c776726445205214779b25bdf2c978d Mon Sep 17 00:00:00 2001 From: is2ac2 Date: Sat, 13 Jul 2024 00:28:02 -0700 Subject: [PATCH 21/21] cleanup / removing comments --- sim/humanoid_gym/envs/only_legs_config.py | 10 +---- sim/stompy_legs/joints.py | 48 ----------------------- 2 files changed, 1 insertion(+), 57 deletions(-) diff --git a/sim/humanoid_gym/envs/only_legs_config.py b/sim/humanoid_gym/envs/only_legs_config.py index 1445c52f..1595c626 100755 --- a/sim/humanoid_gym/envs/only_legs_config.py +++ b/sim/humanoid_gym/envs/only_legs_config.py @@ -44,16 +44,13 @@ class asset(LeggedRobotCfg.asset): foot_name = "_foot_1_rmd_x4_24_mock_1_inner_rmd_x4_24_1" knee_name = "_rmd_x8_90_mock_3_inner_rmd_x8_90_1" - # 0.23 termination_height = 0.23 default_feet_height = 0.0 terminate_after_contacts_on = ["link_leg_assembly_left_1_leg_part_1_2", "link_leg_assembly_right_1_leg_part_1_2"] penalize_contacts_on = [] self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter - # self_collisions = 1 - # is2ac collapse_fixed_joints = True flip_visual_attachments = False @@ -120,14 +117,9 @@ class control(LeggedRobotCfg.control): "ankle": 10, "knee": 10, } - # set all to a certain default value for now for k in stiffness: stiffness[k]*= .01 damping[k] *= .01 - # print(stiffness) - # print(damping) - # action scale: target angle = actionScale * action + defaultAngle - # action_scale = 0.25 action_scale = 0.5 # decimation: Number of control action updates @ sim DT per policy DT decimation = 10 # 100hz @@ -209,7 +201,7 @@ class scales: # low_speed = 0.2 # track_vel_hard = 0.5 - # above this was removed + # above this was removed for standing policy # base pos default_joint_pos = 0.5 orientation = 1 diff --git a/sim/stompy_legs/joints.py b/sim/stompy_legs/joints.py index 0aecdeaa..aee2556b 100755 --- a/sim/stompy_legs/joints.py +++ b/sim/stompy_legs/joints.py @@ -129,54 +129,6 @@ def default_standing(cls) -> Dict[str, float]: @classmethod def default_limits(cls) -> Dict[str, Dict[str, float]]: return { - # Stompy.legs.left.hip_pitch: { - # "lower": -1.32, - # "upper": 0.69, - # }, - # Stompy.legs.left.hip_roll: { - # "lower": 1.13, - # "upper": 2.14, - # }, - # Stompy.legs.left.hip_yaw: { - # "lower": -2.2, - # "upper": -1.01, - # }, - # Stompy.legs.left.knee_pitch: { - # "lower": -3.14, - # "upper": -2.2, - # }, - # Stompy.legs.left.ankle_pitch: { - # "lower": -0.14, - # "upper": 1.13, - # }, - # Stompy.legs.left.ankle_roll: { - # "lower": -3.08, - # "upper": -2.26, - # }, - # Stompy.legs.right.hip_pitch: { - # "lower": -5.00, - # "upper": -3.83, - # }, - # Stompy.legs.right.hip_roll: { - # "lower": 2.39, - # "upper": 3.33, - # }, - # Stompy.legs.right.hip_yaw: { - # "lower": -1.13, - # "upper": -0.69, - # }, - # Stompy.legs.right.knee_pitch: { - # "lower": -1.95, - # "upper": -1.00, - # }, - # Stompy.legs.right.ankle_pitch: { - # "lower": 0.50, - # "upper": 1.54, - # }, - # Stompy.legs.right.ankle_roll: { - # "lower": -3.00, - # "upper": -2.14, - # }, Stompy.legs.left.hip_pitch: { "lower": 0.5, "upper": 2.69,