@@ -60,7 +60,7 @@ def add_body_pos(
6060 articulations : dict [str , Articulation ],
6161 context_manager : ContextManager ,
6262):
63- """Add body position inputs for all articulations.
63+ """Add body position inputs for all articulations, skipping root bodies .
6464
6565 For each articulation, this function adds inputs for the position
6666 of each body belonging to that articulation.
@@ -72,7 +72,10 @@ def add_body_pos(
7272
7373 # Add inputs for all body positions in world frame
7474 for obj_name , articulation in articulations .items ():
75+ root_name = articulation .body_names [0 ]
7576 for body_name in articulation .data .body_names :
77+ if body_name == root_name :
78+ continue
7679 body_ids , _ = articulation .find_bodies (body_name )
7780 assert len (body_ids ) == 1 , (
7881 f"Body name { body_name } is not unique in articulation { obj_name } . "
@@ -90,7 +93,7 @@ def add_body_quat(
9093 articulations : dict [str , Articulation ],
9194 context_manager : ContextManager ,
9295):
93- """Add body orientation inputs for all articulations.
96+ """Add body orientation inputs for all articulations, skipping root bodies .
9497
9598 For each articulation, this function adds inputs for the quaternion
9699 of each body belonging to that articulation.
@@ -101,7 +104,10 @@ def add_body_quat(
101104 """
102105 # Add inputs for all body quaternions in world frame
103106 for obj_name , articulation in articulations .items ():
107+ root_name = articulation .body_names [0 ]
104108 for body_name in articulation .data .body_names :
109+ if body_name == root_name :
110+ continue
105111 body_ids , _ = articulation .find_bodies (body_name )
106112 assert len (body_ids ) == 1 , (
107113 f"Body name { body_name } is not unique in articulation { obj_name } . "
@@ -119,7 +125,7 @@ def add_body_pos_and_quat(
119125 articulations : dict [str , Articulation ],
120126 context_manager : ContextManager ,
121127):
122- """Add body position and orientation inputs for all articulations.
128+ """Add body position and orientation inputs for all articulations, skipping root bodies .
123129
124130 For each articulation, this function adds inputs for the position and quaternion
125131 of each body belonging to that articulation.
@@ -153,19 +159,48 @@ def add_base_vel(
153159 context_manager: The context manager to add base velocity inputs to.
154160 """
155161 for obj_name , articulation in articulations .items ():
156- input_name_prefix = f"{ OBJ_PREFIX } .{ obj_name } "
162+ input_name_prefix = f"{ OBJ_PREFIX } .{ obj_name } . { articulation . body_names [ 0 ] } "
157163
158- # Add base orientation and velocities
159- base_lin_vel_b_rt_w_in_b = Input (
160- name = f"{ input_name_prefix } .base. lin_vel_b_rt_w_in_b" ,
164+ # Add base linear and angular velocities expressed in base frame.
165+ lin_vel_b_rt_w_in_b = Input (
166+ name = f"{ input_name_prefix } .lin_vel_b_rt_w_in_b" ,
161167 get_from_env_cb = lambda art = articulation : art .data .root_lin_vel_b ,
162168 )
163- base_ang_vel_b_rt_w_in_b = Input (
164- name = f"{ input_name_prefix } .base. ang_vel_b_rt_w_in_b" ,
169+ ang_vel_b_rt_w_in_b = Input (
170+ name = f"{ input_name_prefix } .ang_vel_b_rt_w_in_b" ,
165171 get_from_env_cb = lambda art = articulation : art .data .root_ang_vel_b ,
166172 )
167- context_manager .add_component (base_lin_vel_b_rt_w_in_b )
168- context_manager .add_component (base_ang_vel_b_rt_w_in_b )
173+ context_manager .add_component (lin_vel_b_rt_w_in_b )
174+ context_manager .add_component (ang_vel_b_rt_w_in_b )
175+
176+
177+ def add_base_pose (
178+ articulations : dict [str , Articulation ],
179+ context_manager : ContextManager ,
180+ ):
181+ """Add base pose inputs for all articulations.
182+
183+ For each articulation, this function adds inputs for the base position and orientation
184+ in the world frame.
185+
186+ Args:
187+ articulations: Dictionary mapping object names to Articulation instances.
188+ context_manager: The context manager to add base pose inputs to.
189+ """
190+ for obj_name , articulation in articulations .items ():
191+ input_name_prefix = f"{ OBJ_PREFIX } .{ obj_name } .{ articulation .body_names [0 ]} "
192+
193+ # Add base position and orientation
194+ pos_b_rt_w_in_w = Input (
195+ name = f"{ input_name_prefix } .pos_b_rt_w_in_w" ,
196+ get_from_env_cb = lambda art = articulation : art .data .root_pos_w ,
197+ )
198+ w_Q_b = Input (
199+ name = f"{ input_name_prefix } .w_Q_b" ,
200+ get_from_env_cb = lambda art = articulation : art .data .root_quat_w ,
201+ )
202+ context_manager .add_component (pos_b_rt_w_in_w )
203+ context_manager .add_component (w_Q_b )
169204
170205
171206def add_joint_pos_and_vel (
0 commit comments