Skip to content

Commit 1393f3b

Browse files
jtigue-bdaiMayankm96kellyguo11
authored
Adds body_incoming_joint_wrench_b to ArticulationData field (#2128)
# Description <!-- Thank you for your interest in sending a pull request. Please make sure to check the contribution guidelines. Link: https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html --> This PR exposes the physics tensor view of the joint reaction wrenches. Fixes #[2127](#2127) [1566](#1566) <!-- As a practice, it is recommended to open an issue to have discussions on the proposed pull request. This makes it easier for the community to keep track of what is being developed or added, and if a given feature is demanded by more than one party. --> ## Type of change <!-- As you go through the list, delete the ones that are not applicable. --> - New feature (non-breaking change which adds functionality) <!-- Example: | Before | After | | ------ | ----- | | _gif/png before_ | _gif/png after_ | To upload images to a PR -- simply drag and drop an image while in edit mode and it should upload the image directly. You can then paste that source into the above before/after sections. --> ## Checklist - [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with `./isaaclab.sh --format` - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works - [ ] I have updated the changelog and the corresponding version in the extension's `config/extension.toml` file - [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there <!-- As you go through the checklist above, you can mark something as done by putting an x character in it For example, - [x] I have done this task - [ ] I have not done this task --> --------- Signed-off-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com> Co-authored-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com> Co-authored-by: Kelly Guo <kellyg@nvidia.com>
1 parent 8a913d0 commit 1393f3b

File tree

3 files changed

+116
-4
lines changed

3 files changed

+116
-4
lines changed

‎source/isaaclab/isaaclab/assets/articulation/articulation_data.py

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,7 @@ def __init__(self, root_physx_view: physx.ArticulationView, device: str):
7474
self._joint_pos = TimestampedBuffer()
7575
self._joint_acc = TimestampedBuffer()
7676
self._joint_vel = TimestampedBuffer()
77+
self._body_incoming_joint_wrench_b = TimestampedBuffer()
7778

7879
def update(self, dt: float):
7980
# update the simulation timestamp
@@ -509,6 +510,22 @@ def body_acc_w(self):
509510
self._body_acc_w.timestamp = self._sim_timestamp
510511
return self._body_acc_w.data
511512

513+
@property
514+
def body_incoming_joint_wrench_b(self) -> torch.Tensor:
515+
"""Joint reaction wrench applied from body parent to child body in parent body frame.
516+
517+
Shape is (num_instances, num_bodies, 6). All body reaction wrenches are provided including the root body to the
518+
world of an articulation.
519+
520+
For more information on joint wrenches, please check the`PhysX documentation <https://nvidia-omniverse.github.io/PhysX/physx/5.5.1/docs/Articulations.html#link-incoming-joint-force>`__
521+
and the underlying `PhysX Tensor API <https://docs.omniverse.nvidia.com/kit/docs/omni_physics/latest/extensions/runtime/source/omni.physics.tensors/docs/api/python.html#omni.physics.tensors.impl.api.ArticulationView.get_link_incoming_joint_force>`__ .
522+
"""
523+
524+
if self._body_incoming_joint_wrench_b.timestamp < self._sim_timestamp:
525+
self._body_incoming_joint_wrench_b.data = self._root_physx_view.get_link_incoming_joint_force()
526+
self._body_incoming_joint_wrench_b.time_stamp = self._sim_timestamp
527+
return self._body_incoming_joint_wrench_b.data
528+
512529
@property
513530
def projected_gravity_b(self):
514531
"""Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""

‎source/isaaclab/isaaclab/envs/mdp/observations.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -233,8 +233,8 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor
233233
# extract the used quantities (to enable type-hinting)
234234
asset: Articulation = env.scene[asset_cfg.name]
235235
# obtain the link incoming forces in world frame
236-
link_incoming_forces = asset.root_physx_view.get_link_incoming_joint_force()[:, asset_cfg.body_ids]
237-
return link_incoming_forces.view(env.num_envs, -1)
236+
body_incoming_joint_wrench_b = asset.data.body_incoming_joint_wrench_b[:, asset_cfg.body_ids]
237+
return body_incoming_joint_wrench_b.view(env.num_envs, -1)
238238

239239

240240
def imu_orientation(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor:

‎source/isaaclab/test/assets/test_articulation.py

Lines changed: 97 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -96,10 +96,15 @@ def generate_articulation_cfg(
9696
velocity_limit_sim=velocity_limit_sim,
9797
effort_limit=effort_limit,
9898
velocity_limit=velocity_limit,
99-
stiffness=0.0,
100-
damping=10.0,
99+
stiffness=2000.0,
100+
damping=100.0,
101101
),
102102
},
103+
init_state=ArticulationCfg.InitialStateCfg(
104+
pos=(0.0, 0.0, 0.0),
105+
joint_pos=({"RevoluteJoint": 1.5708}),
106+
rot=(0.7071055, 0.7071081, 0, 0),
107+
),
103108
)
104109
elif articulation_type == "single_joint_explicit":
105110
# we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying.
@@ -1509,6 +1514,96 @@ def test_write_root_state(self):
15091514
elif state_location == "link":
15101515
torch.testing.assert_close(rand_state, articulation.data.root_link_state_w)
15111516

1517+
def test_body_incoming_joint_wrench_b_single_joint(self):
1518+
"""Test the data.body_incoming_joint_wrench_b buffer is populated correctly and statically correct for single joint."""
1519+
for num_articulations in (2, 1):
1520+
for device in ("cpu", "cuda:0"):
1521+
print(num_articulations, device)
1522+
with self.subTest(num_articulations=num_articulations, device=device):
1523+
with build_simulation_context(
1524+
gravity_enabled=True, device=device, add_ground_plane=False, auto_add_lighting=True
1525+
) as sim:
1526+
sim._app_control_on_stop_handle = None
1527+
articulation_cfg = generate_articulation_cfg(articulation_type="single_joint_implicit")
1528+
articulation, _ = generate_articulation(
1529+
articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device
1530+
)
1531+
1532+
# Play the simulator
1533+
sim.reset()
1534+
# apply external force
1535+
external_force_vector_b = torch.zeros(
1536+
(num_articulations, articulation.num_bodies, 3), device=device
1537+
)
1538+
external_force_vector_b[:, 1, 1] = 10.0 # 10 N in Y direction
1539+
external_torque_vector_b = torch.zeros(
1540+
(num_articulations, articulation.num_bodies, 3), device=device
1541+
)
1542+
external_torque_vector_b[:, 1, 2] = 10.0 # 10 Nm in z direction
1543+
1544+
# apply action to the articulation
1545+
joint_pos = torch.ones_like(articulation.data.joint_pos) * 1.5708 / 2.0
1546+
articulation.write_joint_state_to_sim(
1547+
torch.ones_like(articulation.data.joint_pos), torch.zeros_like(articulation.data.joint_vel)
1548+
)
1549+
articulation.set_joint_position_target(joint_pos)
1550+
articulation.write_data_to_sim()
1551+
for _ in range(50):
1552+
articulation.set_external_force_and_torque(
1553+
forces=external_force_vector_b, torques=external_torque_vector_b
1554+
)
1555+
articulation.write_data_to_sim()
1556+
# perform step
1557+
sim.step()
1558+
# update buffers
1559+
articulation.update(sim.cfg.dt)
1560+
1561+
# check shape
1562+
self.assertEqual(
1563+
articulation.data.body_incoming_joint_wrench_b.shape,
1564+
(num_articulations, articulation.num_bodies, 6),
1565+
)
1566+
1567+
# calculate expected static
1568+
mass = articulation.data.default_mass
1569+
pos_w = articulation.data.body_pos_w
1570+
quat_w = articulation.data.body_quat_w
1571+
1572+
mass_link2 = mass[:, 1].view(num_articulations, -1)
1573+
gravity = (
1574+
torch.tensor(sim.cfg.gravity, device="cpu")
1575+
.repeat(num_articulations, 1)
1576+
.view((num_articulations, 3))
1577+
)
1578+
1579+
# NOTE: the com and link pose for single joint are colocated
1580+
weight_vector_w = mass_link2 * gravity
1581+
# expected wrench from link mass and external wrench
1582+
expected_wrench = torch.zeros((num_articulations, 6), device=device)
1583+
expected_wrench[:, :3] = math_utils.quat_apply(
1584+
math_utils.quat_conjugate(quat_w[:, 0, :]),
1585+
weight_vector_w.to(device)
1586+
+ math_utils.quat_apply(quat_w[:, 1, :], external_force_vector_b[:, 1, :]),
1587+
)
1588+
expected_wrench[:, 3:] = math_utils.quat_apply(
1589+
math_utils.quat_conjugate(quat_w[:, 0, :]),
1590+
torch.cross(
1591+
pos_w[:, 1, :].to(device) - pos_w[:, 0, :].to(device),
1592+
weight_vector_w.to(device)
1593+
+ math_utils.quat_apply(quat_w[:, 1, :], external_force_vector_b[:, 1, :]),
1594+
dim=-1,
1595+
)
1596+
+ math_utils.quat_apply(quat_w[:, 1, :], external_torque_vector_b[:, 1, :]),
1597+
)
1598+
1599+
# check value of last joint wrench
1600+
torch.testing.assert_close(
1601+
expected_wrench,
1602+
articulation.data.body_incoming_joint_wrench_b[:, 1, :].squeeze(1),
1603+
atol=1e-2,
1604+
rtol=1e-3,
1605+
)
1606+
15121607

15131608
if __name__ == "__main__":
15141609
run_tests()

0 commit comments

Comments
 (0)