From e546a154b6970057be51c3262e061fb865259939 Mon Sep 17 00:00:00 2001 From: Feishi Wang Date: Mon, 21 Apr 2025 21:29:20 -0700 Subject: [PATCH 1/3] infra: add PrimitiveFrameCfg; isaaclab: support it --- metasim/cfg/objects.py | 16 ++++++++++++++++ metasim/sim/isaaclab/isaaclab.py | 18 +++++++++++++++++- metasim/sim/isaaclab/isaaclab_helper.py | 16 ++++++++++++++++ 3 files changed, 49 insertions(+), 1 deletion(-) diff --git a/metasim/cfg/objects.py b/metasim/cfg/objects.py index a65c773e2..aa6cd5305 100644 --- a/metasim/cfg/objects.py +++ b/metasim/cfg/objects.py @@ -131,6 +131,22 @@ def density(self) -> float: return self.mass / (4 / 3 * math.pi * self.radius**3) +@configclass +class PrimitiveFrameCfg(RigidObjCfg): + """Primitive coordinate frame cfg.""" + + # TODO: This is object shouldn't inherit from RigidObjCfg? + name: str = MISSING + scale: float = 1.0 + """Scale of the frame""" + base_link: str | tuple[str, str] | None = None + """Base link to attach the frame. + If ``None``, the frame will be attached to the world origin. + If a ``str``, the frame will be attached to the root link of the object specified by the name. + If a ``tuple[str, str]``, the frame will be attached to the object specified by the first str and the body link specified by the second str. + """ + + @configclass class PrimitiveCylinderCfg(RigidObjCfg): """Primitive cylinder object cfg.""" diff --git a/metasim/sim/isaaclab/isaaclab.py b/metasim/sim/isaaclab/isaaclab.py index 59a471a4f..618362a51 100644 --- a/metasim/sim/isaaclab/isaaclab.py +++ b/metasim/sim/isaaclab/isaaclab.py @@ -7,7 +7,7 @@ import torch from loguru import logger as log -from metasim.cfg.objects import ArticulationObjCfg, BaseObjCfg, RigidObjCfg +from metasim.cfg.objects import ArticulationObjCfg, BaseObjCfg, PrimitiveFrameCfg, RigidObjCfg from metasim.cfg.scenario import ScenarioCfg from metasim.sim import BaseSimHandler, EnvWrapper, IdentityEnvWrapper from metasim.types import Action, EnvState, Extra, Obs, Reward, Success, TimeOut @@ -128,6 +128,22 @@ def step(self, action: list[Action]) -> tuple[Obs, Reward, Success, TimeOut, Ext time_out = time_out.cpu() success = self.checker.check(self) states = self.get_states() + + ## TODO: organize this + for obj in self.objects: + if isinstance(obj, PrimitiveFrameCfg): + if obj.base_link is None: + pos = torch.zeros((self.num_envs, 3), device=self.device) + rot = torch.zeros((self.num_envs, 4), device=self.device) + elif isinstance(obj.base_link, str): + pos, rot = (states.objects | states.robots)[obj.base_link].root_state[:, :7].split([3, 4], dim=-1) + else: + base_obj_name = obj.base_link[0] + base_body_name = obj.base_link[1] + merged_states = states.objects | states.robots + body_idx = merged_states[base_obj_name].body_names.index(base_body_name) + pos, rot = merged_states[base_obj_name].body_state[:, body_idx, :7].split([3, 4], dim=-1) + self._set_object_pose(obj, pos, rot) return states, None, success, time_out, extras def reset(self, env_ids: list[int] | None = None) -> tuple[list[EnvState], Extra]: diff --git a/metasim/sim/isaaclab/isaaclab_helper.py b/metasim/sim/isaaclab/isaaclab_helper.py index 5fca92e76..e8eb882a8 100644 --- a/metasim/sim/isaaclab/isaaclab_helper.py +++ b/metasim/sim/isaaclab/isaaclab_helper.py @@ -9,6 +9,7 @@ BaseObjCfg, PrimitiveCubeCfg, PrimitiveCylinderCfg, + PrimitiveFrameCfg, PrimitiveSphereCfg, RigidObjCfg, ) @@ -91,6 +92,21 @@ def add_object(env: "EmptyEnv", obj: BaseObjCfg) -> None: ) ) return + if isinstance(obj, PrimitiveFrameCfg): + env.scene.rigid_objects[obj.name] = RigidObject( + RigidObjectCfg( + prim_path=prim_path, + spawn=sim_utils.UsdFileCfg( + usd_path="/home/fs/Downloads/frame_prim.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, kinematic_enabled=True + ), # fixed + collision_props=None, # no collision + scale=obj.scale, + ), + ) + ) + return ## File-based object usd_file_cfg = sim_utils.UsdFileCfg( From d4917172b3f0cc477e901ed10196154a03ca598d Mon Sep 17 00:00:00 2001 From: Feishi Wang Date: Mon, 21 Apr 2025 21:37:05 -0700 Subject: [PATCH 2/3] add frame asset to metasim/data; update path --- .../assets/COMMON/frame/usd/frame.usd | Bin 0 -> 6389 bytes metasim/sim/isaaclab/isaaclab_helper.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 metasim/data/quick_start/assets/COMMON/frame/usd/frame.usd diff --git a/metasim/data/quick_start/assets/COMMON/frame/usd/frame.usd b/metasim/data/quick_start/assets/COMMON/frame/usd/frame.usd new file mode 100644 index 0000000000000000000000000000000000000000..1bb42895649b446f9b9bffaeca169bc7eaedf866 GIT binary patch literal 6389 zcmeHLdvH|c6+idhedb{{2{9WU$?^yg-PXV5Wd&FhHo!icbWeB z0Kj`K06%G2Y3fTZ)AMTqa_BMjp3C%r`W%K#eJKj8Hd(T=JQ-$@ynH#PmL0=ThrB$s z`$}2GRb_47jI7A63ezj?re8%i<0`V5c{0o*dHGqGUg@toPexW` zSA*%5cD_7WLG^OOZZ@Wu=5MS8Mn1*)x!7`)Lj`zY4yKl$f&yAKUVd(#jAdyey!^a8 znPpd-C)==LgJCy6Pqut{t_=(FWY*j)M48pDcs#z=LYg)%wyQ4h8*APd<$ZJL;K5rh z{KEX^u(>Y`hPJdvX`d0@g770&Ayy-9MXW*4g0pOR%r_yL5jaSIiy4B5bqLxGMC%c4 zh}#g<<95VGgjHupUY^b$W0;Y5A-WNU%rcIkz#9BJFyDjNjG$@Qf}loFgLniI%>)tl zdp`lQuUTV&`L&iFUe*C*S~FFkW=s|6aF{C4uAeHb$S>TK zU!Vz?GNaL-D$o>66=)uwc%0~Enht{wuDU<|^F1F-l@FO^Q~sJ|XK_n!m9NgTroR(Y zelNE>dkwi|^`H69S|c>o*4m>Q^X1lZu%?i7mY!_?oiX&Vu_69#G4#(ALsuK?yb`7v z{F}wlwCRJINlT2#TKmKewIR*Lcp9fmvOPTA6Mk#@AZF>T82YN&{~J&@4?$#&`(n&n z_Dc*+0YA<7Fc)(#m<^e2=&l`edyH?qaIN?_Wa^jZ)t7VW<(FMWR(}=Q|KsF0kCVow z>Aw^w8*mP-m9aOv1`C4-tGpfaLkMG^A^%?r z&Q{01E*|f|b`!l~elSdD4bdre)si|4-F&TrBp(Lb*hrWZxv0_B_#!_5R)<*ps|(~qq;OGf-a~D zQHL%m(XT^TNkgBKNyS=P;S|8Kj${gLR88C<>Ag2Y2C(v&sP;kDuIfYnnAn#|+%AdR z{b^}PlmjL3j9(Xr^cF={)VXk`a5yC-rD#Bk$1|GP+U8HIN}t$*73cvdnGNi7t*R93 zP^E;FS_jvOObQLa^eXW<_8-_jP0%x{FwVF;;gbZ8q`yx_|7k@^ ztp=tVN}dH9Gm{u__^5dnm?~H+OK}m{N0K;MQd%B<3Ye02fn_$q*6sb0F1`$VWHF@$ zBu!V;&ru2IVVx2cq06zlglz%R7{D1KEIIc zlf;zXKEkmMF6)kFuusW$VOUfHf-ZnAC2{6s;w!f7v~CdxJqAFFP>M^U3O#W}O@cn0 z7B}Gn#%1e(P*1NcrJT@(BL|y{p%hWJ1vw(!t|$W<3^jw^k?v4&Nfrb9A_Qez9%b;D zs_2;M*}SC;&O5LxvIrT;ACt7SEDX1*N+#_A=22)up|S!)b1Kct=a?mfn$xMUeFvs7w z1nesqpQ3)ogrNoJ=t;+!Vu;I%pf_&46JkoH5Bn6+Vj)!=#QoR(OE|;o$^ae>g97f4 zK1r7J;T6C==E&MZg5IxzhSQ!(h-$cBNCm`gtS=_q=z#Bl&Bg3&(f}tUB}TvG*qgyN z14ewUs*uL7KCpG)oe}idBT&|OAMC`s6+9Apty~lsh5KJRg>1(U0En*qDtg z7()HSniSRWER$l*N^JNngF&ons0_-|^TFf4-^nb1OfV(j;Ki!o#{&{AeqD-IL8wnr zWT0mV5d{@GlPMt43xh57LPcl@9np>;YRxf#PRc|W$W{hBD=K(wd=OV&h-DdOF~hQlx=!tH+244m zCF{x7oXXZ7XCcci8b4m^WGA>Iwa3lh_pM0nWAJjz7xKHEE~u{q&IzwWA&+lHyc4+j zUwL60^*G&(yRHUbql@^P++@DG$PEp3u-*j?KG^Jn#yXH(Q2qnNJ{Nd4EyLGqA%DaJ z<$pm@rx(gU!hF~RzHN3lH2UBxFL<_=pvOXf7v3qnBX^?yB@ftMg7Uk(#_$|~@?I2f za{+q+$`@lLUcS$ZGgk@?ULLRkUjAqgG|qvT56b_Brl+y4Nf-Ev9J5JWg~j=ZMg))8 zh}eQiU<-%W0yD(mour)E1q}?Z`(PWbnUOUkj3-fJG=C-9_M$B!VNW6dJ>wo!II<6GCweW%0M6TKk}$Fb!xiCh2&X&J)QL0i2^K-;v`8cptc2`C zL6lr{o^#sHIGv}RuFsu?pE`>_c6ug0_I@~h^1~S?XHA~0J~8|Kxs&hDn|Qx=+;MEd z7mi~KFF20Xop&5t^bg0e8_qe7Ej}|=Gq&XP*wRyD%d%tjpN}B z=S2JDJ0}umH!M#7Y#8(m{pEnIk91aWu+tu?2uDJZridFRYb$_>F}ni{<7Ax9y-dU5 zSw(L*9G+Q>$&BkTnO=fPl?M}V>Dy&xZkN;ff)fftMvktT2JM?fia0v={%Fy!IXGQW z7k(@hX^yxfwV0_`@z!^l=imVw%iQnopL_~>C%MNV;fCc;KpfMbfr#mDh++C9MBVVh zLy(^Ac)pTlb|@~G^X(KS_arg7S9Zb3eFHh2->Uo>%9Pq6*SiV`Lx{n@nceV~XOfv@ zYR4{4G7~PmEpBJ9I8k_>gV6a2hMBO&i4GKlc$$41*jWhTV3ig_7O&8g+|+NuvD-~c zIEN#NV;V-gic(KgBp8TvMj{OE6b4sExB{FGyM3I6Qd53e!`$Ps<9@}1zy%y0*v`pu=NB}WMB$DN!A None: RigidObjectCfg( prim_path=prim_path, spawn=sim_utils.UsdFileCfg( - usd_path="/home/fs/Downloads/frame_prim.usd", + usd_path="metasim/data/quick_start/assets/COMMON/frame/usd/frame.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=True, kinematic_enabled=True ), # fixed From 4f9ffa66fea406b54ab1a4b6d8123fe10c179885 Mon Sep 17 00:00:00 2001 From: Feishi Wang Date: Mon, 21 Apr 2025 22:28:41 -0700 Subject: [PATCH 3/3] quickfix --- metasim/sim/isaaclab/isaaclab.py | 1 + 1 file changed, 1 insertion(+) diff --git a/metasim/sim/isaaclab/isaaclab.py b/metasim/sim/isaaclab/isaaclab.py index 618362a51..942063cc3 100644 --- a/metasim/sim/isaaclab/isaaclab.py +++ b/metasim/sim/isaaclab/isaaclab.py @@ -135,6 +135,7 @@ def step(self, action: list[Action]) -> tuple[Obs, Reward, Success, TimeOut, Ext if obj.base_link is None: pos = torch.zeros((self.num_envs, 3), device=self.device) rot = torch.zeros((self.num_envs, 4), device=self.device) + rot[:, 0] = 1.0 elif isinstance(obj.base_link, str): pos, rot = (states.objects | states.robots)[obj.base_link].root_state[:, :7].split([3, 4], dim=-1) else: