Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
5 changes: 3 additions & 2 deletions docs/source/device/soma.rst
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,9 @@ select their body and hand sources independently.

The body schema contains SOMA's complete body control set, including its hand
controls. The standalone hand schemas provide independent left and right hand
streams. Isaac Teleop does not automatically fuse standalone hands with the
hand controls in a body payload.
streams, identified by ``hand_side`` (``SomaHandSide.LEFT`` or
``SomaHandSide.RIGHT``). Isaac Teleop does not automatically fuse standalone
hands with the hand controls in a body payload.

Choose a representation
-----------------------
Expand Down
2 changes: 1 addition & 1 deletion docs/source/device/trackers.rst
Original file line number Diff line number Diff line change
Expand Up @@ -348,7 +348,7 @@ reference-frame convention as ``SomaBodyJointRotations``.
When a producer has already evaluated forward kinematics, it can instead publish
``SomaHandJointPoses``. That profile carries a global position, orientation, and validity for each
of the same 25 joints and requires no downstream FK. Create one tracker per hand collection; each
payload identifies its side through ``handedness``. As with the body contract, a vendor adapter
payload identifies its side through ``hand_side``. As with the body contract, a vendor adapter
converts its device-native hand data before calling ``SchemaPusher``.

For a retargeting graph, ``SomaHandSource`` selects the rotation tracker by default and emits an
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
SomaHandSource,
)
from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex
from isaaccapture.schema import SomaHandedness, SomaHandJoint
from isaaccapture.schema import SomaHandSide, SomaHandJoint

from .hand_pipeline import HandViewLayout, HandViewPipeline

Expand Down Expand Up @@ -98,14 +98,14 @@ def create_soma_hand_pipeline(
left = SomaHandSource(
"hand_left",
left_collection_id,
SomaHandedness.LEFT,
SomaHandSide.LEFT,
layers[0],
representation=representation,
)
right = SomaHandSource(
"hand_right",
right_collection_id,
SomaHandedness.RIGHT,
SomaHandSide.RIGHT,
layers[1],
representation=representation,
)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
Point,
Pose,
Quaternion,
SomaHandedness,
SomaHandSide,
SomaHandJoint,
SomaHandJointPose,
SomaHandJointPoses,
Expand Down Expand Up @@ -150,31 +150,31 @@ def demo_hand_frames(path: Path, body, hands):
return frames


def soma_joint_rotations(rotations, translation, handedness):
def soma_joint_rotations(rotations, translation, hand_side):
joints = [
SomaHandJointRotation(joint, Quaternion(*rotation))
for joint, rotation in zip(HAND_JOINTS, rotations, strict=True)
]
return SomaHandJointRotations(joints, Point(*translation), True, handedness)
return SomaHandJointRotations(joints, Point(*translation), True, hand_side)


def soma_joint_poses(positions, orientations, handedness):
def soma_joint_poses(positions, orientations, hand_side):
joints = [
SomaHandJointPose(joint, Pose(Point(*position), Quaternion(*orientation)))
for joint, position, orientation in zip(
HAND_JOINTS, positions, orientations, strict=True
)
]
return SomaHandJointPoses(joints, handedness)
return SomaHandJointPoses(joints, hand_side)


def _payload(frame, index: int, representation: str, handedness):
def _payload(frame, index: int, representation: str, hand_side):
if representation == "joint-poses":
return soma_joint_poses(
frame["positions"][index], frame["orientations"][index], handedness
frame["positions"][index], frame["orientations"][index], hand_side
).to_bytes()
return soma_joint_rotations(
frame["rotations"][index], frame["translation"][index], handedness
frame["rotations"][index], frame["translation"][index], hand_side
).to_bytes()


Expand Down Expand Up @@ -237,13 +237,13 @@ def main(argv: list[str]) -> int:
frames["left"],
index,
args.hand_representation,
SomaHandedness.LEFT,
SomaHandSide.LEFT,
)
right = _payload(
frames["right"],
index,
args.hand_representation,
SomaHandedness.RIGHT,
SomaHandSide.RIGHT,
)
if len(left) > max_payload_size or len(right) > max_payload_size:
raise ValueError("SOMA hand payload exceeds transport capacity")
Expand Down
4 changes: 2 additions & 2 deletions src/core/schema/fbs/soma_common.fbs
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,8 @@

namespace core;

// Hand side shared by the SOMA pose transport schemas.
enum SomaHandedness : uint8 {
// Anatomical hand side, independent of the right-handed reference frame.
enum SomaHandSide : uint8 {
UNSPECIFIED = 0,
LEFT = 1,
RIGHT = 2,
Expand Down
2 changes: 1 addition & 1 deletion src/core/schema/fbs/soma_hand_joint_poses.fbs
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ table SomaHandJointPoses {
// Use CreateVectorOfSortedStructs when building the vector directly.
// A joint absent from this vector is not provided by the producer.
joint_poses: [SomaHandJointPose] (id: 0);
handedness: SomaHandedness = UNSPECIFIED (id: 1);
hand_side: SomaHandSide = UNSPECIFIED (id: 1);
}

table SomaHandJointPosesRecord {
Expand Down
2 changes: 1 addition & 1 deletion src/core/schema/fbs/soma_hand_joint_rotations.fbs
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ table SomaHandJointRotations {
// when global_translation_is_valid is false.
global_translation: Point (id: 1);
global_translation_is_valid: bool (id: 2);
handedness: SomaHandedness = UNSPECIFIED (id: 3);
hand_side: SomaHandSide = UNSPECIFIED (id: 3);
}

table SomaHandJointRotationsRecord {
Expand Down
Binary file modified src/core/schema/golden/soma_common.bfbs
Binary file not shown.
Binary file modified src/core/schema/golden/soma_hand_joint_poses.bfbs
Binary file not shown.
Binary file modified src/core/schema/golden/soma_hand_joint_rotations.bfbs
Binary file not shown.
8 changes: 4 additions & 4 deletions src/core/schema/python/soma_common_bindings.h
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,10 @@ namespace core

inline void bind_soma_common(py::module& m)
{
py::enum_<SomaHandedness>(m, "SomaHandedness")
.value("UNSPECIFIED", SomaHandedness_UNSPECIFIED)
.value("LEFT", SomaHandedness_LEFT)
.value("RIGHT", SomaHandedness_RIGHT);
py::enum_<SomaHandSide>(m, "SomaHandSide")
.value("UNSPECIFIED", SomaHandSide_UNSPECIFIED)
.value("LEFT", SomaHandSide_LEFT)
.value("RIGHT", SomaHandSide_RIGHT);
}

} // namespace core
9 changes: 4 additions & 5 deletions src/core/schema/python/soma_hand_joint_poses_bindings.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ inline void bind_soma_hand_joint_poses(py::module& m)

serialized_class<SomaHandJointPoses>(m, "SomaHandJointPoses", "Encoded evaluated SOMA hand poses.")
.def(py::init(
[](std::vector<SomaHandJointPose> joint_poses, SomaHandedness handedness)
[](std::vector<SomaHandJointPose> joint_poses, SomaHandSide hand_side)
{
std::sort(joint_poses.begin(), joint_poses.end(),
[](const auto& a, const auto& b) { return a.joint() < b.joint(); });
Expand All @@ -50,11 +50,10 @@ inline void bind_soma_hand_joint_poses(py::module& m)
}
SomaHandJointPosesT native;
native.joint_poses = std::move(joint_poses);
native.handedness = handedness;
native.hand_side = hand_side;
return pack<SomaHandJointPoses>(native);
}),
py::arg("joint_poses") = std::vector<SomaHandJointPose>{},
py::arg("handedness") = SomaHandedness_UNSPECIFIED)
py::arg("joint_poses") = std::vector<SomaHandJointPose>{}, py::arg("hand_side") = SomaHandSide_UNSPECIFIED)
.def_property_readonly("joint_poses",
[](const Serialized<SomaHandJointPoses>& self)
{
Expand All @@ -79,7 +78,7 @@ inline void bind_soma_hand_joint_poses(py::module& m)
return found != nullptr ? py::cast(*found) : py::none();
},
py::arg("joint"), "Return one provided joint pose, or None when absent.")
.def_property_readonly("handedness", field(&SomaHandJointPoses::handedness));
.def_property_readonly("hand_side", field(&SomaHandJointPoses::hand_side));

bind_record<SomaHandJointPosesRecord, SomaHandJointPoses>(m, "SomaHandJointPosesRecord", "SomaHandJointPoses");
}
Expand Down
8 changes: 4 additions & 4 deletions src/core/schema/python/soma_hand_joint_rotations_bindings.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ inline void bind_soma_hand_joint_rotations(py::module& m)
serialized_class<SomaHandJointRotations>(m, "SomaHandJointRotations", "Encoded SOMA hand joint rotations.")
.def(py::init(
[](std::vector<SomaHandJointRotation> joint_rotations, const Point& global_translation,
bool global_translation_is_valid, SomaHandedness handedness)
bool global_translation_is_valid, SomaHandSide hand_side)
{
std::sort(joint_rotations.begin(), joint_rotations.end(),
[](const auto& a, const auto& b) { return a.joint() < b.joint(); });
Expand All @@ -81,11 +81,11 @@ inline void bind_soma_hand_joint_rotations(py::module& m)
native.joint_rotations = std::move(joint_rotations);
native.global_translation = std::make_shared<Point>(global_translation);
native.global_translation_is_valid = global_translation_is_valid;
native.handedness = handedness;
native.hand_side = hand_side;
return pack<SomaHandJointRotations>(native);
}),
py::arg("joint_rotations") = std::vector<SomaHandJointRotation>{}, py::arg("global_translation") = Point(),
py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandedness_UNSPECIFIED)
py::arg("global_translation_is_valid") = false, py::arg("hand_side") = SomaHandSide_UNSPECIFIED)
.def_property_readonly("joint_rotations",
[](const Serialized<SomaHandJointRotations>& self)
{
Expand Down Expand Up @@ -113,7 +113,7 @@ inline void bind_soma_hand_joint_rotations(py::module& m)
.def_property_readonly("global_translation", field(&SomaHandJointRotations::global_translation),
py::return_value_policy::reference_internal)
.def_property_readonly("global_translation_is_valid", field(&SomaHandJointRotations::global_translation_is_valid))
.def_property_readonly("handedness", field(&SomaHandJointRotations::handedness));
.def_property_readonly("hand_side", field(&SomaHandJointRotations::hand_side));

bind_record<SomaHandJointRotationsRecord, SomaHandJointRotations>(
m, "SomaHandJointRotationsRecord", "SomaHandJointRotations");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
SomaHandJointPosesTracker,
SomaHandJointRotationsTracker,
)
from isaaccapture.schema import SomaHandedness
from isaaccapture.schema import SomaHandSide

from .deviceio_tensor_types import (
DeviceIOSomaHandJointPosesTracked,
Expand Down Expand Up @@ -43,17 +43,17 @@ def __init__(
self,
name: str,
collection_id: str,
handedness: SomaHandedness,
hand_side: SomaHandSide,
layer: Any | None = None,
*,
representation: SomaHandRepresentation
| str = SomaHandRepresentation.JOINT_ROTATIONS,
) -> None:
if not collection_id:
raise ValueError("SOMA hand collection_id must not be empty")
if handedness not in (SomaHandedness.LEFT, SomaHandedness.RIGHT):
raise ValueError("SOMA hand source requires LEFT or RIGHT handedness")
self.handedness = handedness
if hand_side not in (SomaHandSide.LEFT, SomaHandSide.RIGHT):
raise ValueError("SOMA hand source requires LEFT or RIGHT hand side")
self.hand_side = hand_side
self.representation = SomaHandRepresentation(representation)
self._tracker: SomaHandJointRotationsTracker | SomaHandJointPosesTracker
if self.representation is SomaHandRepresentation.JOINT_ROTATIONS:
Expand All @@ -65,7 +65,7 @@ def __init__(
self._tracker = SomaHandJointPosesTracker(collection_id)
self._input_type = DeviceIOSomaHandJointPosesTracked()
# Pybind enum values read from buffers are not singleton objects.
expected_hand_type = "left" if handedness == SomaHandedness.LEFT else "right"
expected_hand_type = "left" if hand_side == SomaHandSide.LEFT else "right"
self._evaluator = (
_SomaHandEvaluator(layer, expected_hand_type) if layer is not None else None
)
Expand All @@ -91,10 +91,10 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N
if data is None:
group.set_none()
return
if data.handedness != self.handedness:
if data.hand_side != self.hand_side:
raise ValueError(
f"SOMA hand payload handedness {data.handedness} does not match "
f"source handedness {self.handedness}"
f"SOMA hand payload hand side {data.hand_side} does not match "
f"source hand side {self.hand_side}"
)

if self.representation is SomaHandRepresentation.JOINT_ROTATIONS:
Expand Down
4 changes: 2 additions & 2 deletions src/python/isaaccapture/schema/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@
FullBodyPose,
FullBodyPoseRecord,
# Shared SOMA types.
SomaHandedness,
SomaHandSide,
# SOMA body joint-rotation types.
SomaBodyJoint,
SomaBodyJointRotation,
Expand Down Expand Up @@ -210,7 +210,7 @@ def __getattr__(name: str):
"FullBodyPose",
"FullBodyPoseRecord",
# Shared SOMA types.
"SomaHandedness",
"SomaHandSide",
# SOMA body joint-rotation types.
"SomaBodyJoint",
"SomaBodyJointRotation",
Expand Down
6 changes: 3 additions & 3 deletions tests/cpp/core/schema/test_soma_hand_joint_poses.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ constexpr auto vt(uint16_t field_id)
} // namespace

static_assert(core::SomaHandJointPoses::VT_JOINT_POSES == vt(0));
static_assert(core::SomaHandJointPoses::VT_HANDEDNESS == vt(1));
static_assert(core::SomaHandJointPoses::VT_HAND_SIDE == vt(1));
static_assert(core::SomaHandJointPosesRecord::VT_DATA == vt(0));
static_assert(core::SomaHandJointPosesRecord::VT_TIMESTAMP == vt(1));
static_assert(std::is_trivially_copyable_v<core::SomaHandJointPose>);
Expand All @@ -30,7 +30,7 @@ TEST_CASE("SOMA hand joint poses round trip as a sparse keyed vector", "[soma_ha
core::SomaHandJointPosesT pose;
pose.joint_poses.emplace_back(core::SomaHandJoint_INDEX_END,
core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)));
pose.handedness = core::SomaHandedness_LEFT;
pose.hand_side = core::SomaHandSide_LEFT;

flatbuffers::FlatBufferBuilder builder;
builder.Finish(core::SomaHandJointPoses::Pack(builder, &pose));
Expand All @@ -46,5 +46,5 @@ TEST_CASE("SOMA hand joint poses round trip as a sparse keyed vector", "[soma_ha
CHECK(index_end->pose().position().x() == Catch::Approx(1.0f));
CHECK(index_end->pose().orientation().w() == Catch::Approx(0.8f));
CHECK(decoded->joint_poses()->LookupByKey(core::SomaHandJoint_WRIST) == nullptr);
CHECK(decoded->handedness() == core::SomaHandedness_LEFT);
CHECK(decoded->hand_side() == core::SomaHandSide_LEFT);
}
12 changes: 6 additions & 6 deletions tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,15 @@
static_assert(core::SomaHandJointRotations::VT_JOINT_ROTATIONS == VT(0));
static_assert(core::SomaHandJointRotations::VT_GLOBAL_TRANSLATION == VT(1));
static_assert(core::SomaHandJointRotations::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2));
static_assert(core::SomaHandJointRotations::VT_HANDEDNESS == VT(3));
static_assert(core::SomaHandJointRotations::VT_HAND_SIDE == VT(3));
static_assert(core::SomaHandJointRotationsRecord::VT_DATA == VT(0));
static_assert(core::SomaHandJointRotationsRecord::VT_TIMESTAMP == VT(1));

#define TYPE(field) decltype(std::declval<core::SomaHandJointRotations>().field())
static_assert(std::is_same_v<TYPE(joint_rotations), const flatbuffers::Vector<const core::SomaHandJointRotation*>*>);
static_assert(std::is_same_v<TYPE(global_translation), const core::Point*>);
static_assert(std::is_same_v<TYPE(global_translation_is_valid), bool>);
static_assert(std::is_same_v<TYPE(handedness), core::SomaHandedness>);
static_assert(std::is_same_v<TYPE(hand_side), core::SomaHandSide>);
static_assert(std::is_trivially_copyable_v<core::SomaHandJointRotation>);
static_assert(sizeof(core::SomaHandJointRotation) == 20);

Expand All @@ -49,7 +49,7 @@ TEST_CASE("SOMA hand joint rotations round trip as a sparse keyed vector", "[som
pose.joint_rotations.emplace_back(core::SomaHandJoint_INDEX_END, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f));
pose.global_translation = std::make_shared<core::Point>(1.0f, 2.0f, 3.0f);
pose.global_translation_is_valid = true;
pose.handedness = core::SomaHandedness_RIGHT;
pose.hand_side = core::SomaHandSide_RIGHT;

flatbuffers::FlatBufferBuilder builder;
builder.Finish(core::SomaHandJointRotations::Pack(builder, &pose));
Expand All @@ -66,7 +66,7 @@ TEST_CASE("SOMA hand joint rotations round trip as a sparse keyed vector", "[som
CHECK(decoded->joint_rotations()->LookupByKey(core::SomaHandJoint_WRIST) == nullptr);
CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f));
CHECK(decoded->global_translation_is_valid());
CHECK(decoded->handedness() == core::SomaHandedness_RIGHT);
CHECK(decoded->hand_side() == core::SomaHandSide_RIGHT);
}

TEST_CASE("SOMA hand record preserves keyed quaternion components", "[soma_hand_joint_rotations][flatbuffers]")
Expand All @@ -80,7 +80,7 @@ TEST_CASE("SOMA hand record preserves keyed quaternion components", "[soma_hand_
core::SomaHandJoint_PINKY_END, core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale));
record.data->global_translation = std::make_shared<core::Point>(1.0f, -2.0f, 3.0f);
record.data->global_translation_is_valid = false;
record.data->handedness = core::SomaHandedness_LEFT;
record.data->hand_side = core::SomaHandSide_LEFT;
record.timestamp = std::make_shared<core::DeviceDataTimestamp>(100, 200, 300);

flatbuffers::FlatBufferBuilder builder;
Expand All @@ -106,7 +106,7 @@ TEST_CASE("SOMA hand record preserves keyed quaternion components", "[soma_hand_
CHECK(pinky->rotation().z() == Catch::Approx(-3.0f * scale));
CHECK(pinky->rotation().w() == Catch::Approx(4.0f * scale));
CHECK(values->LookupByKey(core::SomaHandJoint_INDEX1) == nullptr);
CHECK(decoded->data()->handedness() == core::SomaHandedness_LEFT);
CHECK(decoded->data()->hand_side() == core::SomaHandSide_LEFT);
CHECK(decoded->timestamp()->available_time_local_common_clock() == 100);
CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200);
CHECK(decoded->timestamp()->sample_time_raw_device_clock() == 300);
Expand Down
Loading
Loading