From 857f721dc79b4d564c7fbbfb34d69d86d2460b32 Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Mon, 14 Oct 2024 12:46:04 -0700 Subject: [PATCH 1/6] Progress: g1 updates and small fixes (need to fix across funcs) --- mani_skill/agents/robots/unitree_g1/g1.py | 88 +++++++++++++++++++++- mani_skill/utils/building/actors/common.py | 5 ++ mani_skill/utils/building/ground.py | 9 ++- 3 files changed, 98 insertions(+), 4 deletions(-) diff --git a/mani_skill/agents/robots/unitree_g1/g1.py b/mani_skill/agents/robots/unitree_g1/g1.py index 2cd66bd70..add97f757 100644 --- a/mani_skill/agents/robots/unitree_g1/g1.py +++ b/mani_skill/agents/robots/unitree_g1/g1.py @@ -19,7 +19,47 @@ class UnitreeG1(BaseAgent): keyframes = dict( standing=Keyframe( pose=sapien.Pose(p=[0, 0, 0.755]), - qpos=np.array([0.0] * 37) * 1, + qpos=np.array( + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -0.77, + -0.77, + 0.0, + 0.77, + 0.77, + 0.1, + -0.92, + -0.92, + -0.1, + 0.92, + 0.92, + 0.92, + -0.92, + ] + ), ) ) @@ -66,6 +106,52 @@ class UnitreeG1(BaseAgent): body_damping = 1 body_force_limit = 100 + lower_body_joints = [ + "left_hip_pitch_joint", + "right_hip_pitch_joint", + "left_hip_roll_joint", + "right_hip_roll_joint", + "left_hip_yaw_joint", + "right_hip_yaw_joint", + "left_knee_joint", + "right_knee_joint", + "left_ankle_pitch_joint", + "right_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_ankle_roll_joint", + ] + upper_body_joints = [ + "torso_joint", + "left_shoulder_pitch_joint", + "right_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "right_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "right_elbow_pitch_joint", + "left_elbow_roll_joint", + "right_elbow_roll_joint", + ] + left_hand_joints = [ + "left_zero_joint", + "left_three_joint", + "left_five_joint", + "left_one_joint", + "left_four_joint", + "left_six_joint", + "left_two_joint", + ] + right_hand_joints = [ + "right_zero_joint", + "right_three_joint", + "right_five_joint", + "right_one_joint", + "right_four_joint", + "right_six_joint", + "right_two_joint", + ] + @property def _controller_configs(self): body_pd_joint_pos = PDJointPosControllerConfig( diff --git a/mani_skill/utils/building/actors/common.py b/mani_skill/utils/building/actors/common.py index 49614c59f..e8431be24 100644 --- a/mani_skill/utils/building/actors/common.py +++ b/mani_skill/utils/building/actors/common.py @@ -2,12 +2,15 @@ Common utilities for adding primitive prebuilt shapes to a scene """ +from typing import Optional, Union + import numpy as np import sapien import sapien.render from mani_skill.envs.scene import ManiSkillScene from mani_skill.utils.building.actor_builder import ActorBuilder +from mani_skill.utils.structs.pose import Pose def _build_by_type(builder: ActorBuilder, name, body_type): @@ -52,6 +55,7 @@ def build_box( name: str, body_type: str = "dynamic", add_collision: bool = True, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() if add_collision: @@ -64,6 +68,7 @@ def build_box( base_color=color, ), ) + builder.set_initial_pose(initial_pose) return _build_by_type(builder, name, body_type) diff --git a/mani_skill/utils/building/ground.py b/mani_skill/utils/building/ground.py index 2a040cfd0..0f12a479e 100644 --- a/mani_skill/utils/building/ground.py +++ b/mani_skill/utils/building/ground.py @@ -1,6 +1,7 @@ """ Useful utilities for creating the ground of a scene """ + from __future__ import annotations import os.path as osp @@ -24,6 +25,7 @@ def build_ground( texture_file=osp.join(osp.dirname(__file__), "assets/grid_texture.png"), texture_square_len=4, mipmap_levels=4, + add_collision=True, ): """Procedurally creates a checkered floor given a floor width in meters. @@ -31,9 +33,10 @@ def build_ground( and usually is never run more than once as it is for building a scene, not loading. """ ground = scene.create_actor_builder() - ground.add_plane_collision( - sapien.Pose(p=[0, 0, altitude], q=[0.7071068, 0, -0.7071068, 0]), - ) + if add_collision: + ground.add_plane_collision( + sapien.Pose(p=[0, 0, altitude], q=[0.7071068, 0, -0.7071068, 0]), + ) if scene.parallel_in_single_scene: # when building a ground and using a parallel render in the GUI, we want to only build one ground visual+collision plane ground.set_scene_idxs([0]) From f974de33e383ebea727d336747fe7d40c3e13e46 Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Wed, 16 Oct 2024 01:48:52 -0700 Subject: [PATCH 2/6] Trivial: G1 left and right knee up keyframes --- mani_skill/agents/robots/unitree_g1/g1.py | 90 ++++++++++++++++++++++- 1 file changed, 89 insertions(+), 1 deletion(-) diff --git a/mani_skill/agents/robots/unitree_g1/g1.py b/mani_skill/agents/robots/unitree_g1/g1.py index add97f757..5d4ccd203 100644 --- a/mani_skill/agents/robots/unitree_g1/g1.py +++ b/mani_skill/agents/robots/unitree_g1/g1.py @@ -60,7 +60,95 @@ class UnitreeG1(BaseAgent): -0.92, ] ), - ) + ), + right_knee_up=Keyframe( + pose=sapien.Pose(p=[0, 0, 0.755]), + qpos=np.array( + [ + 0.0, + -1.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -0.77, + -0.77, + 0.0, + 0.77, + 0.77, + 0.1, + -0.92, + -0.92, + -0.1, + 0.92, + 0.92, + 0.92, + -0.92, + ] + ), + ), + left_knee_up=Keyframe( + pose=sapien.Pose(p=[0, 0, 0.755]), + qpos=np.array( + [ + -1.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.5, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -0.77, + -0.77, + 0.0, + 0.77, + 0.77, + 0.1, + -0.92, + -0.92, + -0.1, + 0.92, + 0.92, + 0.92, + -0.92, + ] + ), + ), ) body_joints = [ From 6345427fafc2919ef30b26c2373948fb37c5cbf8 Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Fri, 18 Oct 2024 13:38:04 -0700 Subject: [PATCH 3/6] Fix: g1 rest qpos -> arms down at side --- mani_skill/agents/robots/unitree_g1/g1.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/mani_skill/agents/robots/unitree_g1/g1.py b/mani_skill/agents/robots/unitree_g1/g1.py index 5d4ccd203..2273d9d62 100644 --- a/mani_skill/agents/robots/unitree_g1/g1.py +++ b/mani_skill/agents/robots/unitree_g1/g1.py @@ -30,16 +30,16 @@ class UnitreeG1(BaseAgent): 0.0, 0.0, 0.0, + 0.2, + -0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, - 0.0, - 0.0, - 0.0, + 0.9, + 0.9, 0.0, 0.0, 0.0, From 89c562ee051c0cbf599c6ffe0f688144b4447002 Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Thu, 24 Oct 2024 14:03:24 -0700 Subject: [PATCH 4/6] Fix: g1 arms down --- mani_skill/agents/robots/unitree_g1/g1.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/mani_skill/agents/robots/unitree_g1/g1.py b/mani_skill/agents/robots/unitree_g1/g1.py index 2273d9d62..a5fca2d54 100644 --- a/mani_skill/agents/robots/unitree_g1/g1.py +++ b/mani_skill/agents/robots/unitree_g1/g1.py @@ -74,16 +74,16 @@ class UnitreeG1(BaseAgent): 0.0, 0.0, 0.0, - 0.0, - 0.0, + 0.2, + -0.2, 0.0, 1.5, 0.0, 0.0, 0.0, 0.0, - 0.0, - 0.0, + 0.9, + 0.9, 0.0, 0.0, 0.0, @@ -118,16 +118,16 @@ class UnitreeG1(BaseAgent): 0.0, 0.0, 0.0, - 0.0, - 0.0, + 0.2, + -0.2, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, - 0.0, + 0.9, + 0.9, 0.0, 0.0, 0.0, From b2f25ff3cdac91e85fb915c6f7b7ea969c559984 Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Thu, 24 Oct 2024 15:45:43 -0700 Subject: [PATCH 5/6] Trivial: allow initial pose and scene idxs for building.actors.common --- mani_skill/utils/building/actors/common.py | 45 +++++++++++++++++----- 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/mani_skill/utils/building/actors/common.py b/mani_skill/utils/building/actors/common.py index e8431be24..07f5b695e 100644 --- a/mani_skill/utils/building/actors/common.py +++ b/mani_skill/utils/building/actors/common.py @@ -11,9 +11,20 @@ from mani_skill.envs.scene import ManiSkillScene from mani_skill.utils.building.actor_builder import ActorBuilder from mani_skill.utils.structs.pose import Pose +from mani_skill.utils.structs.types import Array -def _build_by_type(builder: ActorBuilder, name, body_type): +def _build_by_type( + builder: ActorBuilder, + name, + body_type, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, +): + if scene_idxs is not None: + builder.set_scene_idxs(scene_idxs) + if initial_pose is not None: + builder.set_initial_pose(initial_pose) if body_type == "dynamic": actor = builder.build(name=name) elif body_type == "static": @@ -33,6 +44,8 @@ def build_cube( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() if add_collision: @@ -45,7 +58,7 @@ def build_cube( base_color=color, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_box( @@ -55,6 +68,7 @@ def build_box( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() @@ -68,8 +82,7 @@ def build_box( base_color=color, ), ) - builder.set_initial_pose(initial_pose) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_cylinder( @@ -80,6 +93,8 @@ def build_cylinder( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() if add_collision: @@ -94,7 +109,7 @@ def build_cylinder( base_color=color, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_sphere( @@ -104,6 +119,8 @@ def build_sphere( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() if add_collision: @@ -116,7 +133,7 @@ def build_sphere( base_color=color, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_red_white_target( @@ -126,6 +143,8 @@ def build_red_white_target( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): TARGET_RED = np.array([194, 19, 22, 255]) / 255 builder = scene.create_actor_builder() @@ -175,7 +194,7 @@ def build_red_white_target( radius=radius * 1 / 5, half_length=thickness / 2 + 4e-5, ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_twocolor_peg( @@ -187,6 +206,8 @@ def build_twocolor_peg( name: str, body_type="dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() if add_collision: @@ -207,7 +228,7 @@ def build_twocolor_peg( base_color=color_2, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) RED_COLOR = [220 / 255, 12 / 255, 12 / 255, 1] @@ -226,6 +247,8 @@ def build_fourcolor_peg( color_4=[1, 1, 1, 1], body_type="dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): """ A peg with four sections and four different colors. Useful for visualizing every possible rotation without any symmetries @@ -263,7 +286,7 @@ def build_fourcolor_peg( base_color=color_4, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) def build_colorful_cube( @@ -273,6 +296,8 @@ def build_colorful_cube( name: str, body_type: str = "dynamic", add_collision: bool = True, + scene_idxs: Optional[Array] = None, + initial_pose: Optional[Union[Pose, sapien.Pose]] = None, ): builder = scene.create_actor_builder() @@ -291,4 +316,4 @@ def build_colorful_cube( base_color=color, ), ) - return _build_by_type(builder, name, body_type) + return _build_by_type(builder, name, body_type, scene_idxs, initial_pose) From e09eab05f11368b11d928a10c7170f3bd58fddbd Mon Sep 17 00:00:00 2001 From: arth-shukla <44527268+arth-shukla@users.noreply.github.com> Date: Thu, 24 Oct 2024 16:44:17 -0700 Subject: [PATCH 6/6] Chore: fmt off unitree --- mani_skill/agents/robots/unitree_g1/g1.py | 123 ++-------------------- 1 file changed, 6 insertions(+), 117 deletions(-) diff --git a/mani_skill/agents/robots/unitree_g1/g1.py b/mani_skill/agents/robots/unitree_g1/g1.py index a5fca2d54..948189854 100644 --- a/mani_skill/agents/robots/unitree_g1/g1.py +++ b/mani_skill/agents/robots/unitree_g1/g1.py @@ -19,134 +19,23 @@ class UnitreeG1(BaseAgent): keyframes = dict( standing=Keyframe( pose=sapien.Pose(p=[0, 0, 0.755]), + # fmt: off qpos=np.array( - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.2, - -0.2, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.9, - 0.9, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.77, - -0.77, - 0.0, - 0.77, - 0.77, - 0.1, - -0.92, - -0.92, - -0.1, - 0.92, - 0.92, - 0.92, - -0.92, - ] + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9, 0.9, 0.0, 0.0, 0.0, 0.0, 0.0, -0.77, -0.77, 0.0, 0.77, 0.77, 0.1, -0.92, -0.92, -0.1, 0.92, 0.92, 0.92, -0.92] ), ), right_knee_up=Keyframe( pose=sapien.Pose(p=[0, 0, 0.755]), + # fmt: off qpos=np.array( - [ - 0.0, - -1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.2, - -0.2, - 0.0, - 1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.9, - 0.9, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.77, - -0.77, - 0.0, - 0.77, - 0.77, - 0.1, - -0.92, - -0.92, - -0.1, - 0.92, - 0.92, - 0.92, - -0.92, - ] + [0.0, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, -0.2, 0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.9, 0.9, 0.0, 0.0, 0.0, 0.0, 0.0, -0.77, -0.77, 0.0, 0.77, 0.77, 0.1, -0.92, -0.92, -0.1, 0.92, 0.92, 0.92, -0.92] ), ), left_knee_up=Keyframe( pose=sapien.Pose(p=[0, 0, 0.755]), + # fmt: off qpos=np.array( - [ - -1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.2, - -0.2, - 1.5, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.9, - 0.9, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.77, - -0.77, - 0.0, - 0.77, - 0.77, - 0.1, - -0.92, - -0.92, - -0.1, - 0.92, - 0.92, - 0.92, - -0.92, - ] + [-1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, -0.2, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.9, 0.9, 0.0, 0.0, 0.0, 0.0, 0.0, -0.77, -0.77, 0.0, 0.77, 0.77, 0.1, -0.92, -0.92, -0.1, 0.92, 0.92, 0.92, -0.92] ), ), )