diff --git a/.gitignore b/.gitignore index 3defebe6..9adf7c6f 100755 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,11 @@ # .gitignore - +# Robot Parts +*.stl +*.dae +*.urdf # Local files .env +*.DS_Store # Python *.py[oc] __pycache__/ @@ -12,7 +16,6 @@ __pycache__/ .pytest_cache/ .ruff_cache/ .dmypy.json - # Databases *.db 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 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/humanoid_gym/envs/getup_config.py b/sim/humanoid_gym/envs/getup_config.py index 9cf0e6f8..706b4fdc 100755 --- a/sim/humanoid_gym/envs/getup_config.py +++ b/sim/humanoid_gym/envs/getup_config.py @@ -39,8 +39,9 @@ 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" + 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_config.py b/sim/humanoid_gym/envs/legs_config.py index 8be39bcd..4574562f 100755 --- a/sim/humanoid_gym/envs/legs_config.py +++ b/sim/humanoid_gym/envs/legs_config.py @@ -33,13 +33,13 @@ 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" 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"] 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..7a6b5e44 100755 --- a/sim/humanoid_gym/envs/legs_env.py +++ b/sim/humanoid_gym/envs/legs_env.py @@ -1,7 +1,7 @@ # 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 @@ -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 diff --git a/sim/humanoid_gym/envs/stompy_config.py b/sim/humanoid_gym/envs/stompy_config.py index 1833af37..ee6216bb 100755 --- a/sim/humanoid_gym/envs/stompy_config.py +++ b/sim/humanoid_gym/envs/stompy_config.py @@ -1,11 +1,14 @@ """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 ( # type: ignore + LeggedRobotCfg, + LeggedRobotCfgPPO, +) from sim.env import stompy_urdf_path from sim.stompy.joints import Stompy -NUM_JOINTS = len(Stompy.all_joints()) # 37 +NUM_JOINTS = len(Stompy.all_joints()) # 33 class StompyCfg(LeggedRobotCfg): @@ -38,13 +41,12 @@ 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" 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"] penalize_contacts_on = [] self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter @@ -82,7 +84,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] default_joint_angles = {k: 0.0 for k in Stompy.all_joints()} @@ -93,26 +95,25 @@ class init_state(LeggedRobotCfg.init_state): class control(LeggedRobotCfg.control): # PD Drive parameters: 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, } - # action scale: target angle = actionScale * action + defaultAngle action_scale = 0.25 # decimation: Number of control action updates @ sim DT per policy DT diff --git a/sim/humanoid_gym/envs/stompy_env.py b/sim/humanoid_gym/envs/stompy_env.py index 461c9a6a..d848f2de 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 @@ -77,7 +77,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)) @@ -123,16 +122,15 @@ 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"]] = 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,12 +205,10 @@ 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 diff = self.dof_pos - self.ref_dof_pos - self.privileged_obs_buf = torch.cat( ( self.command_input, # 2 + 3 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() diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py old mode 100644 new mode 100755 index 0fa0be87..861fbdf0 --- a/sim/scripts/create_fixed_torso.py +++ b/sim/scripts/create_fixed_torso.py @@ -5,7 +5,7 @@ from sim.stompy.joints import StompyFixed -STOMPY_URDF = "stompy/robot.urdf" +STOMPY_URDF = "sim/stompy/robot.urdf" def update_urdf() -> None: @@ -30,7 +30,7 @@ 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__": 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..fb6e5a93 --- a/sim/scripts/simulate_urdf.py +++ b/sim/scripts/simulate_urdf.py @@ -27,7 +27,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 @@ -80,7 +80,7 @@ def load_gym() -> GymParams: sim_params.physx.num_threads = args.num_threads sim_params.physx.use_gpu = args.use_gpu - # sim_params.use_gpu_pipeline = False + sim_params.use_gpu_pipeline = False # if args.use_gpu_pipeline: # warnings.warn("Forcing CPU pipeline.") @@ -116,14 +116,14 @@ 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 + 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") @@ -150,7 +150,7 @@ 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) for joint_name, joint_position in starting_positions.items(): dof_pos[0, dof_ids[joint_name]] = joint_position @@ -188,7 +188,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) - # 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}, }