Skip to content

Commit

Permalink
defeating mypy
Browse files Browse the repository at this point in the history
  • Loading branch information
@is2ac2
is2ac2 committed Jul 9, 2024
1 parent c627bda commit 9a25cbf
Show file tree
Hide file tree
Showing 5 changed files with 155 additions and 83 deletions.
2 changes: 1 addition & 1 deletion sim/humanoid_gym/envs/stompy_config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
2 changes: 1 addition & 1 deletion sim/humanoid_gym/envs/stompy_env.py
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down
18 changes: 11 additions & 7 deletions sim/scripts/simulate_urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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.")

Expand Down Expand Up @@ -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")

Expand Down Expand Up @@ -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))
Expand Down
Empty file added sim/stompy2/__init__.py
View file
Empty file.
216 changes: 142 additions & 74 deletions sim/stompy2/joints.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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},
}


Expand Down

0 comments on commit 9a25cbf

Please sign in to comment.