Skip to content

Enhance physics attributes APIs and related fucntors #193

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
yuecideng merged 7 commits into from
Mar 23, 2026
Merged

Enhance physics attributes APIs and related fucntors #193

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c8ed4f2
wip
yuecideng Mar 22, 2026
b66f0bf
Merge remote-tracking branch 'origin/main' into yueci/more-physics-api
yuecideng Mar 22, 2026
8c641eb
wip
yuecideng Mar 22, 2026
69c9b7c
wip
yuecideng Mar 22, 2026
1882e86
wip
yuecideng Mar 22, 2026
64e2c4e
wip
yuecideng Mar 22, 2026
b848608
Merge branch 'main' into yueci/more-physics-api
yuecideng Mar 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 79 additions & 6 deletions docs/source/overview/gym/observation_functors.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,14 @@ This page lists all available observation functors that can be used with the Obs

* - Functor Name
- Description
* - ``get_object_pose``
- Get the arena poses of objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) as [x, y, z, qw, qx, qy, qz] when ``to_matrix=False``. Returns zero tensor if object doesn't exist.
* - ``get_rigid_object_pose``
- Get world poses of rigid objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4). If the object doesn't exist, returns a zero tensor.
- Get the arena poses of rigid objects. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 7) when ``to_matrix=False``. If the object doesn't exist, returns a zero tensor. (Deprecated: use ``get_object_pose`` instead.)
* - ``get_sensor_pose_in_robot_frame``
- Transform sensor poses to robot coordinate frame. Returns pose as [x, y, z, qw, qx, qy, qz] of shape (num_envs, 7).
- Transform sensor poses to robot coordinate frame. Returns 4x4 transformation matrices of shape (num_envs, 4, 4). For stereo cameras, supports selecting left or right camera pose via ``is_right`` parameter.
* - ``get_robot_eef_pose``
- Get robot end-effector pose using forward kinematics. Returns 4x4 transformation matrices of shape (num_envs, 4, 4) by default, or (num_envs, 3) for position only when ``position_only=True``. Supports specifying ``part_name`` for different control parts.
```

## Sensor Information
Expand All @@ -30,7 +34,7 @@ This page lists all available observation functors that can be used with the Obs
* - ``get_sensor_intrinsics``
- Get the intrinsic matrix of a camera sensor. Returns 3x3 intrinsic matrices of shape (num_envs, 3, 3). For stereo cameras, supports selecting left or right camera intrinsics.
* - ``compute_semantic_mask``
- Compute semantic masks from camera segmentation masks. Returns masks of shape (num_envs, height, width, 3) with channels for robot, background, and foreground objects.
- Compute semantic masks from camera segmentation masks. Returns masks of shape (num_envs, height, width, 4) with channels for background, foreground, robot left-side, and robot right-side.
```

## Keypoint Projections
Expand All @@ -54,14 +58,44 @@ This page lists all available observation functors that can be used with the Obs
* - Functor Name
- Description
* - ``normalize_robot_joint_data``
- Normalize joint positions or velocities to [0, 1] range based on joint limits. Supports both ``qpos_limits`` and ``qvel_limits``. Operates in ``modify`` mode.
- Normalize joint positions or velocities to a specified range based on joint limits. Supports both ``qpos_limits`` and ``qvel_limits``. Operates in ``modify`` mode. Default range is [0, 1].
```

## Object Properties

```{list-table} Object Properties Functors
:header-rows: 1
:widths: 30 70

* - Functor Name
- Description
* - ``get_object_body_scale``
- Get the body scale of objects. Returns tensor of shape (num_envs, 3). Only supports ``RigidObject``. Returns zero tensor if object doesn't exist.
* - ``get_rigid_object_velocity``
- Get the world velocities (linear and angular) of rigid objects. Returns tensor of shape (num_envs, 6). Returns zero tensor if object doesn't exist.
* - ``get_rigid_object_physics_attributes``
- Get physics attributes (mass, friction, damping, inertia) of rigid objects with caching. Returns a ``TensorDict`` containing: ``mass`` (num_envs, 1), ``friction`` (num_envs, 1), ``damping`` (num_envs, 1), ``inertia`` (num_envs, 3). Cache is cleared on environment reset. Implemented as a Functor class.
Copy link

Copilot AI Mar 23, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Docs list get_rigid_object_physics_attributes returning damping with shape (num_envs, 1), but the underlying API (RigidObject.get_damping) returns (num_envs, 2) (linear, angular). Please update the documented shapes/fields to match the actual return values.

Suggested change
- Get physics attributes (mass, friction, damping, inertia) of rigid objects with caching. Returns a ``TensorDict`` containing: ``mass`` (num_envs, 1), ``friction`` (num_envs, 1), ``damping`` (num_envs, 1), ``inertia`` (num_envs, 3). Cache is cleared on environment reset. Implemented as a Functor class.
- Get physics attributes (mass, friction, damping, inertia) of rigid objects with caching. Returns a ``TensorDict`` containing: ``mass`` (num_envs, 1), ``friction`` (num_envs, 1), ``damping`` (num_envs, 2) [linear, angular], ``inertia`` (num_envs, 3). Cache is cleared on environment reset. Implemented as a Functor class.

Copilot uses AI. Check for mistakes.
* - ``get_articulation_joint_drive``
- Get joint drive properties (stiffness, damping, max_effort, max_velocity, friction) of articulations (e.g. robots) with caching. Returns a ``TensorDict`` containing properties of shape ``(num_envs, num_joints)``. Cache is cleared on environment reset. Implemented as a Functor class.
```

## Target / Goal

```{list-table} Target / Goal Functors
:header-rows: 1
:widths: 30 70

* - Functor Name
- Description
* - ``target_position``
- Get virtual target position from environment state. Reads target pose from ``env.{target_pose_key}`` (set by randomization events). Returns tensor of shape (num_envs, 3). Returns zeros if not yet initialized. Supports custom ``target_pose_key`` parameter.
```

```{currentmodule} embodichain.lab.sim.objects
```

```{note}
To get robot end-effector poses, you can use the robot's {meth}`~Robot.compute_fk()` method directly in your observation functors or task code.
For custom observation needs, you can also use the robot's {meth}`~Robot.compute_fk()` method directly in your observation functors or task code.
```

## Usage Example
Expand All @@ -72,9 +106,19 @@ from embodichain.lab.gym.envs.managers.cfg import ObservationCfg, SceneEntityCfg
# Example: Add object pose to observations
observations = {
"object_pose": ObservationCfg(
func="get_rigid_object_pose",
func="get_object_pose",
mode="add",
name="object/cube/pose",
params={
"entity_cfg": SceneEntityCfg(uid="cube"),
"to_matrix": True,
},
),
# Example: Get object velocity
"object_velocity": ObservationCfg(
func="get_rigid_object_velocity",
mode="add",
name="object/cube/velocity",
params={
"entity_cfg": SceneEntityCfg(uid="cube"),
},
Expand All @@ -87,6 +131,35 @@ observations = {
params={
"joint_ids": list(range(7)), # First 7 joints
"limit": "qpos_limits",
"range": [0.0, 1.0],
},
),
# Example: Get robot end-effector pose
"eef_pose": ObservationCfg(
func="get_robot_eef_pose",
mode="add",
name="robot/eef/pose",
params={
"part_name": "left_arm",
"position_only": False,
},
),
# Example: Get object physics attributes
"object_physics": ObservationCfg(
func="get_rigid_object_physics_attributes",
mode="add",
name="object/cube/physics",
params={
"entity_cfg": SceneEntityCfg(uid="cube"),
},
),
# Example: Get articulation joint drive properties
"robot_joint_drive": ObservationCfg(
func="get_articulation_joint_drive",
mode="add",
name="robot/joint_drive",
params={
"entity_cfg": SceneEntityCfg(uid="robot"),
},
),
}
Expand Down
69 changes: 57 additions & 12 deletions docs/source/overview/sim/sim_articulation.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,24 +90,33 @@ robot = sim.add_articulation(cfg=usd_art_cfg_override)

## Articulation Class

State data is accessed via getter methods that return batched tensors.
State data is accessed via getter methods that return batched tensors (`N` environments). Certain static properties are available as standard class properties.

| Property | Shape | Description |
| Property | Type | Description |
| :--- | :--- | :--- |
| `get_local_pose` | `(N, 7)` | Root link pose `[x, y, z, qw, qx, qy, qz]`. |
| `get_qpos` | `(N, dof)` | Joint positions. |
| `get_qvel` | `(N, dof)` | Joint velocities. |

| `num_envs` | `int` | Number of simulation environments this articulation is instantiated in. |
| `dof` | `int` | Degrees of freedom (number of actuated joints). |
| `joint_names` | `List[str]` | Names of all movable joints. |
| `link_names` | `List[str]` | Names of all rigid links. |
| `mass` | `Tensor` | Total mass of the articulation per environment `(N, 1)`. |

| Method | Shape / Return Type | Description |
| :--- | :--- | :--- |
| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Root link pose `[x, y, z, qw, qx, qy, qz]` or a 4x4 matrix. |
| `get_link_pose(link_name, to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Specific link pose `[x, y, z, qw, qx, qy, qz]` or a 4x4 matrix. |
| `get_qpos(target=False)` | `(N, dof)` | Current joint positions (or joint targets if `target=True`). |
| `get_qvel(target=False)` | `(N, dof)` | Current joint velocities (or velocity targets if `target=True`). |
| `get_joint_drive()` | `Tuple[Tensor, ...]` | Returns `(stiffness, damping, max_effort, max_velocity, friction)`, each shaped `(N, dof)`. |

```python
# Example: Accessing state
# Note: Use methods (with brackets) instead of properties
print(f"Degrees of freedom: {articulation.dof}")
print(f"Current Joint Positions: {articulation.get_qpos()}")
print(f"Root Pose: {articulation.get_local_pose()}")
print(f"End Effector Pose: {articulation.get_link_pose('ee_link')}")
```

### Control & Dynamics
You can control the articulation by setting joint targets.
You can control the articulation by setting target states or directly applying forces.

### Joint Control
```python
Expand All @@ -121,29 +130,65 @@ target_qpos = torch.zeros_like(current_qpos)
# target=False: Instantly resets/teleports joints to this position (ignoring physics).
articulation.set_qpos(target_qpos, target=True)

# Set target velocities
target_qvel = torch.zeros_like(current_qpos)
articulation.set_qvel(target_qvel, target=True)

# Apply forces directly
# Sets an external force tensor (N, dof) applied at the degree of freedom.
target_qf = torch.ones_like(current_qpos) * 10.0
articulation.set_qf(target_qf)

# Important: Step simulation to apply control
sim.update()
```

### Pose Control
```python
# Teleport the articulation root to a new pose
# shape: (N, 7) formatted as [x, y, z, qw, qx, qy, qz]
new_root_pose = torch.tensor([[0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0]], device=device).repeat(sim.num_envs, 1)
articulation.set_local_pose(new_root_pose)
```

### Drive Configuration
Dynamically adjust drive properties.

```python
# Set stiffness for all joints
articulation.set_drive(
articulation.set_joint_drive(
stiffness=torch.tensor([100.0], device=device),
damping=torch.tensor([10.0], device=device)
)
```

### Kinematics
Supports differentiable Forward Kinematics (FK) and Jacobian computation.

```python
# Compute Forward Kinematics
# Note: Ensure 'build_pk_chain=True' in cfg
# Note: Ensure `build_pk_chain=True` in cfg
if getattr(art_cfg, 'build_pk_chain', False):
# Returns (batch_size, 4, 4) homogeneous transformation matrix
ee_pose = articulation.compute_fk(
qpos=articulation.get_qpos(), # Use method call
qpos=articulation.get_qpos(),
end_link_name="ee_link" # Replace with actual link name
)

# Or return a dictionary of multiple link transforms (pytorch_kinematics Transform3d objects)
link_poses = articulation.compute_fk(
qpos=articulation.get_qpos(),
link_names=["link1", "link2"],
to_dict=True
)
```

### State Reset
Resetting an articulation returns it to its initial state properties.
```python
# Clear the physical dynamics and velocities
articulation.clear_dynamics()

# Reset the articulation entirely (resets pose, velocities, and root states to config defaults)
articulation.reset()
```
65 changes: 62 additions & 3 deletions docs/source/overview/sim/sim_rigid_object.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,19 +103,78 @@ obj2 = sim.add_rigid_object(cfg=usd_cfg_override)

Rigid objects are observed and controlled via single poses and linear/angular velocities. Key APIs include:

### Pose & State

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `get_local_pose(to_matrix=False)` | `(N, 7)` or `(N, 4, 4)` | Get object local pose as (x, y, z, qw, qx, qy, qz) or 4x4 matrix per environment. |
| `set_local_pose(pose, env_ids=None)` | `pose: (N, 7)` or `(N, 4, 4)` | Teleport object to given pose (requires calling `sim.update()` to apply). |
| `body_data.pose` | `(N, 7)` | Access object pose directly (for dynamic/kinematic bodies). |
| `body_data.lin_vel` | `(N, 3)` | Access linear velocity of object root (for dynamic/kinematic bodies). |
| `body_data.ang_vel` | `(N, 3)` | Access angular velocity of object root (for dynamic/kinematic bodies). |
| `body_data.vel` | `(N, 6)` | Concatenated linear and angular velocities. |
| `body_data.com_pose` | `(N, 7)` | Get center of mass pose of rigid bodies. |
| `body_data.default_com_pose` | `(N, 7)` | Default center of mass pose. |
| `body_state` | `(N, 13)` | Get full body state: [x, y, z, qw, qx, qy, qz, lin_x, lin_y, lin_z, ang_x, ang_y, ang_z]. |
| `add_force_torque(force, torque, pos, env_ids)` | `force: (N, 3)`, `torque: (N, 3)` | Apply continuous force and/or torque to the object. |

### Dynamics Control

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `add_force_torque(force, torque, pos, env_ids)` | `force: (N, 3)`, `torque: (N, 3)` | Apply continuous force and/or torque to object. |
| `set_velocity(lin_vel, ang_vel, env_ids)` | `lin_vel: (N, 3)`, `ang_vel: (N, 3)` | Set linear and/or angular velocity directly. |
| `clear_dynamics(env_ids=None)` | - | Reset velocities and clear all forces/torques. |
| `set_visual_material(mat, env_ids=None)` | `mat: VisualMaterial` | Change visual appearance at runtime. |
| `enable_collision(flag, env_ids=None)` | `flag: torch.Tensor` | Enable/disable collision for specific instances. |

### Physical Properties

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `set_attrs(attrs, env_ids=None)` | `attrs: RigidBodyAttributesCfg` | Set physical attributes (mass, friction, damping, etc.). |
| `set_mass(mass, env_ids=None)` | `mass: (N,)` | Set mass for rigid object. |
| `get_mass(env_ids=None)` | `(N,)` | Get mass for rigid object. |
| `set_friction(friction, env_ids=None)` | `friction: (N,)` | Set dynamic and static friction. |
| `get_friction(env_ids=None)` | `(N,)` | Get friction (dynamic friction value). |
| `set_damping(damping, env_ids=None)` | `damping: (N, 2)` | Set linear and angular damping. |
| `get_damping(env_ids=None)` | `(N, 2)` | Get linear and angular damping. |
| `set_inertia(inertia, env_ids=None)` | `inertia: (N, 3)` | Set inertia tensor diagonal values. |
| `get_inertia(env_ids=None)` | `(N, 3)` | Get inertia tensor diagonal values. |
| `set_com_pose(com_pose, env_ids=None)` | `com_pose: (N, 7)` | Set center of mass pose (dynamic/kinematic only). |

### Geometry & Body Type

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `get_vertices(env_ids=None)` | `(N, num_verts, 3)` | Get mesh vertices of the rigid objects. |
| `get_body_scale(env_ids=None)` | `(N, 3)` | Get the body scale. |
| `set_body_scale(scale, env_ids=None)` | `scale: (N, 3)` | Set scale of rigid body (CPU only). |
| `set_body_type(body_type)` | `body_type: str` | Change body type between 'dynamic' and 'kinematic'. |
| `is_static` | `bool` | Check if the rigid object is static. |
| `is_non_dynamic` | `bool` | Check if the rigid object is non-dynamic (static or kinematic). |

### Collision & Filtering

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `enable_collision(enable, env_ids=None)` | `enable: (N,)` | Enable/disable collision for specific instances. |
| `set_collision_filter(filter_data, env_ids=None)` | `filter_data: (N, 4)` | Set collision filter data (arena id, collision flag, ...). |

### Visual & Appearance

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `set_visual_material(mat, env_ids=None, shared=False)` | `mat: VisualMaterial` | Change visual appearance at runtime. |
| `get_visual_material_inst(env_ids=None)` | `List[VisualMaterialInst]` | Get material instances for the rigid object. |
| `share_visual_material_inst(mat_insts)` | `mat_insts: List[VisualMaterialInst]` | Share material instances between objects. |
| `set_visible(visible)` | `visible: bool` | Set visibility of the rigid object. |
| `set_physical_visible(visible, rgba=None)` | `visible: bool`, `rgba: (4,)` | Set collision body render visibility. |

### Utility & Identification

| Method / Property | Return / Args | Description |
| :--- | :--- | :--- |
| `get_user_ids()` | `(N,)` | Get the user IDs of the rigid bodies. |
| `reset(env_ids=None)` | - | Reset objects to initial configuration. |
| `destroy()` | - | Destroy and remove the rigid object from simulation. |

### Observation Shapes

Expand Down
5 changes: 5 additions & 0 deletions embodichain/lab/gym/envs/embodied_env.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -526,6 +526,11 @@ def _initialize_episode(
if "reset" in self.event_manager.available_modes:
self.event_manager.apply(mode="reset", env_ids=env_ids)

# reset observation manager for environments that need a reset
# This clears any cached data in observation functors (e.g., physics attributes)
if self.cfg.observations:
self.observation_manager.reset(env_ids=env_ids)

# reset reward manager for environments that need a reset
if self.cfg.rewards:
self.reward_manager.reset(env_ids=env_ids)
Expand Down
Loading
Loading