Skip to content
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.

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

--Convenience functions for accessing markers in local and world space #2389

Merged
merged 2 commits into from
May 8, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
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
19 changes: 15 additions & 4 deletions src/esp/bindings/PhysicsObjectBindings.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -207,8 +207,19 @@ void declareBasePhysicsObjectWrapper(py::module& m,
.def_property_readonly(
"marker_sets", &PhysObjWrapper::getMarkerSets,
py::return_value_policy::reference_internal,
("The MarkerSets defined for " + objType + " this object.").c_str())

("The MarkerSets defined for this " + objType + ".").c_str())
.def("marker_points_local", &PhysObjWrapper::getMarkerPointsLocal,
("A nested dict structure holding all the marker"
"points defined for this this " +
jturner65 marked this conversation as resolved.
Show resolved Hide resolved
objType +
" in object-local space. Same result as "
"<obj>.marker_sets.get_all_marker_points.")
.c_str())
.def("marker_points_global", &PhysObjWrapper::getMarkerPointsGlobal,
("A nested dict structure holding all the marker"
"points defined for this this " +
jturner65 marked this conversation as resolved.
Show resolved Hide resolved
objType + " transformed to world space.")
.c_str())
.def_property_readonly(
"csv_info", &PhysObjWrapper::getObjectInfo,
("Comma-separated informational string describing this " + objType +
Expand Down Expand Up @@ -663,8 +674,8 @@ void initPhysicsObjectBindings(py::module& m) {

// create bindings for ArticulatedObjects
// physics object base instance for articulated object
declareBasePhysicsObjectWrapper<ArticulatedObject>(m, "Articulated Object",
"ArticulatedObject");
declareBasePhysicsObjectWrapper<ArticulatedObject>(
m, "Articulated Object", "ManagedArticulatedObject");

// ==== ManagedArticulatedObject ====
declareArticulatedObjectWrapper(m, "Articulated Object",
Expand Down
50 changes: 50 additions & 0 deletions src/esp/physics/ArticulatedObject.h
Original file line number Diff line number Diff line change
Expand Up @@ -506,6 +506,56 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
return linkIter->second->transformWorldPointsToLocal(points, linkId);
}

/**
* @brief Retrieves the hierarchical map-of-map-of-maps containing
* the @ref MarkerSets constituent marker points, in local space
* (which is the space they are given in).
*/
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
getMarkerPointsGlobal() const override {
const auto lclPoints = markerSets_->getAllMarkerPoints();
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
res{};
// for each task
for (const auto& taskEntry : lclPoints) {
const std::string taskName = taskEntry.first;
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>
perTaskMap;
// for each link
for (const auto& linkEntry : taskEntry.second) {
const std::string linkName = linkEntry.first;
int linkId = getLinkIdFromName(linkName);
auto linkIter = links_.find(linkId);
ESP_CHECK(
linkIter != links_.end(),
"ArticulatedObject::getMarkerPointsGlobal - no link found with "
"linkId ="
<< linkId);
std::unordered_map<std::string, std::vector<Mn::Vector3>> perLinkMap;
// for each set in link
for (const auto& markersEntry : linkEntry.second) {
const std::string markersName = markersEntry.first;
perLinkMap[markersName] =
linkIter->second->transformLocalPointsToWorld(markersEntry.second,
linkId);
}
perTaskMap[linkName] = perLinkMap;
}
res[taskName] = perTaskMap;
}
return res;
} // getMarkerPointsGlobal

/**
* @brief Set forces/torques for all joints indexed by degrees of freedom.
*
Expand Down
56 changes: 56 additions & 0 deletions src/esp/physics/PhysicsObjectBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -537,6 +537,62 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
markerSets_->overwriteWithConfig(attr);
}

/**
* @brief Retrieves the hierarchical map-of-map-of-maps containing
* the @ref MarkerSets constituent marker points, in local space
* (which is the space they are given in).
*/
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
getMarkerPointsLocal() const {
return markerSets_->getAllMarkerPoints();
}

/**
* @brief Retrieves the hierarchical map-of-map-of-maps containing
* the @ref MarkerSets constituent marker points, in local space
* (which is the space they are given in).
*/
virtual std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
getMarkerPointsGlobal() const {
const auto lclPoints = markerSets_->getAllMarkerPoints();
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
res{};
// for each task
for (const auto& taskEntry : lclPoints) {
const std::string taskName = taskEntry.first;
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>
perTaskMap;
// for each link - should only have 1 link in rigids
for (const auto& linkEntry : taskEntry.second) {
const std::string linkName = linkEntry.first;
std::unordered_map<std::string, std::vector<Mn::Vector3>> perLinkMap;
// for each set in link
for (const auto& markersEntry : linkEntry.second) {
const std::string markersName = markersEntry.first;
perLinkMap[markersName] =
transformLocalPointsToWorld(markersEntry.second, -1);
}
perTaskMap[linkName] = perLinkMap;
}
res[taskName] = perTaskMap;
}
return res;
} // getMarkerPointsGlobal

/** @brief Get the scale of the object set during initialization.
* @return The scaling for the object relative to its initially loaded meshes.
*/
Expand Down
34 changes: 34 additions & 0 deletions src/esp/physics/objectWrappers/ManagedPhysicsObjectBase.h
Original file line number Diff line number Diff line change
Expand Up @@ -203,6 +203,40 @@ class AbstractManagedPhysicsObject
return {};
}

/**
* @brief Retrieves the hierarchical map-of-map-of-maps containing
* the @ref MarkerSets constituent marker points, in local space
* (which is the space they are given in).
*/
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
getMarkerPointsLocal() const {
if (auto sp = this->getObjectReference()) {
return sp->getMarkerPointsLocal();
}
return {};
}

/**
* @brief Retrieves the hierarchical map-of-map-of-maps containing
* the @ref MarkerSets constituent marker points, in local space
* (which is the space they are given in).
*/
std::unordered_map<
std::string,
std::unordered_map<
std::string,
std::unordered_map<std::string, std::vector<Mn::Vector3>>>>
getMarkerPointsGlobal() const {
if (auto sp = this->getObjectReference()) {
return sp->getMarkerPointsGlobal();
}
return {};
}

Magnum::Quaternion getRotation() const {
if (auto sp = this->getObjectReference()) {
return sp->getRotation();
Expand Down