Skip to content

plantarchitecture: fix USD export joint anchors and duplicate fruit/flower prim names #106

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

Closed
NilsCode wants to merge 1 commit into from
Closed

plantarchitecture: fix USD export joint anchors and duplicate fruit/flower prim names #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
44 changes: 38 additions & 6 deletions plugins/plantarchitecture/src/InputOutput.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3508,6 +3508,38 @@ vec3 worldToLocal(const vec3 &world_point, const vec3 &xform_position, float qw,
return result;
}

// Inverse of worldToLocal: transform a point in a link's local frame into world space.
vec3 localToWorld(const vec3 &local_point, const USDLink &link) {
const vec3 &v = local_point;
vec3 u(link.qx, link.qy, link.qz);
vec3 uv;
uv.x = u.y * v.z - u.z * v.y;
uv.y = u.z * v.x - u.x * v.z;
uv.z = u.x * v.y - u.y * v.x;
vec3 uuv;
uuv.x = u.y * uv.z - u.z * uv.y;
uuv.y = u.z * uv.x - u.x * uv.z;
uuv.z = u.x * uv.y - u.y * uv.x;
vec3 result;
result.x = v.x + 2.f * (link.qw * uv.x + uuv.x);
result.y = v.y + 2.f * (link.qw * uv.y + uuv.y);
result.z = v.z + 2.f * (link.qw * uv.z + uuv.z);
return result + link.position;
}

// Joint anchor on the parent link at the point where the child actually starts. A child does not always begin at
// the parent's tip: a lateral shoot branches off at a node, and segments shorter than min_segment_length are dropped.
vec3 parentAnchorAt(const vec3 &world_point, const USDLink &parent) {
return worldToLocal(world_point, parent.position, parent.qw, parent.qx, parent.qy, parent.qz);
}

// Joint anchor on a leaf/fruit/flower link at the parent's tip. Organ links are positioned at their mesh centroid, so
// the anchor has to be expressed relative to it rather than placed at the organ's origin.
vec3 organAnchorAtParentTip(const USDLink &parent, const USDLink &organ) {
vec3 tip = localToWorld(make_vec3(0, 0, parent.half_length), parent);
return worldToLocal(tip, organ.position, organ.qw, organ.qx, organ.qy, organ.qz);
}

// Build a visual mesh for a tube segment by extracting the primitives belonging to
// that segment from the tube compound object. For a tube with N nodes and R radial
// subdivisions, segment i (between node i and node i+1) has 2*R triangles.
Expand Down Expand Up @@ -3798,7 +3830,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
} else {
joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name;
joint.parent_link_index = parent_idx;
joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length);
joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]);
joint.local_pos_child = vec3(0, 0, -half_len);

float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f;
Expand Down Expand Up @@ -3904,7 +3936,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name;
joint.parent_link_index = parent_idx;
joint.child_link_index = this_link_idx;
joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length);
joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]);
joint.local_pos_child = vec3(0, 0, -half_len);
float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f;
joint.stiffness = computeJointStiffness(params.elastic_modulus, avg_radius, length);
Expand Down Expand Up @@ -3970,7 +4002,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
joint.parent_link_index = parent_idx;
joint.child_link_index = this_link_idx;
joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length);
joint.local_pos_child = vec3(0, 0, 0);
joint.local_pos_child = organAnchorAtParentTip(data.links[parent_idx], data.links[this_link_idx]);
joint.stiffness = params.organ_spring_stiffness;
joint.damping = params.organ_spring_damping;
joint.is_fixed = false;
Expand Down Expand Up @@ -4059,7 +4091,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
joint.name = "J_" + data.links[parent_idx].name + "_to_" + link.name;
joint.parent_link_index = parent_idx;
joint.child_link_index = this_link_idx;
joint.local_pos_parent = vec3(0, 0, data.links[parent_idx].half_length);
joint.local_pos_parent = parentAnchorAt(start, data.links[parent_idx]);
joint.local_pos_child = vec3(0, 0, -half_len);
float avg_radius = (data.links[parent_idx].radius + seg_radius) / 2.f;
joint.stiffness = computeJointStiffness(params.elastic_modulus, avg_radius, length);
Expand Down Expand Up @@ -4101,7 +4133,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
uint organ_objID = fbud.inflorescence_objIDs[inf_idx];

USDLink link;
link.name = "S" + std::to_string(shoot->ID) + "_P" + std::to_string(phytomer_idx) + "_" + organ_label + std::to_string(inf_idx);
link.name = "S" + std::to_string(shoot->ID) + "_P" + std::to_string(phytomer_idx) + "_Ped" + std::to_string(petiole_idx) + "_B" + std::to_string(bud_idx) + "_" + organ_label + std::to_string(inf_idx);
link.link_type = USD_LINK_MESH;
link.qw = 1; link.qx = 0; link.qy = 0; link.qz = 0;
link.radius = 0;
Expand All @@ -4120,7 +4152,7 @@ ArticulationData buildArticulationData(const PlantInstance &plant, const USDExpo
joint.parent_link_index = organ_attach;
joint.child_link_index = this_link_idx;
joint.local_pos_parent = vec3(0, 0, data.links[organ_attach].half_length);
joint.local_pos_child = vec3(0, 0, 0);
joint.local_pos_child = organAnchorAtParentTip(data.links[organ_attach], data.links[this_link_idx]);
joint.stiffness = params.organ_spring_stiffness;
joint.damping = params.organ_spring_damping;
joint.is_fixed = false;
Expand Down
155 changes: 155 additions & 0 deletions plugins/plantarchitecture/tests/selfTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5111,6 +5111,161 @@ DOCTEST_TEST_CASE("USD export organs") {
std::remove(filename.c_str());
}

DOCTEST_TEST_CASE("USD export joint anchors coincide") {
// Each joint's anchor on body0 (localPos0) and on body1 (localPos1) must map to the same world point.
// Otherwise PhysX pulls the bodies together when the simulation starts and the plant jumps out of its rest pose.
Context context;
PlantArchitecture plantarchitecture(&context);
plantarchitecture.disableMessages();
plantarchitecture.loadPlantModelFromLibrary("bean");

uint plantID = plantarchitecture.buildPlantInstanceFromLibrary(make_vec3(0, 0, 0), 500);

std::string filename = "test_usd_joint_anchors.usda";
plantarchitecture.writePlantStructureUSD(plantID, filename);

std::ifstream file(filename);
DOCTEST_REQUIRE(file.is_open());

auto parse_tuple = [](const std::string &line) {
std::vector<float> values;
std::string inside = line.substr(line.find('(') + 1, line.find(')') - line.find('(') - 1);
std::stringstream ss(inside);
std::string item;
while (std::getline(ss, item, ',')) {
values.push_back(std::stof(item));
}
return values;
};
auto quoted_name = [](const std::string &line) {
size_t first = line.find('"');
return line.substr(first + 1, line.find('"', first + 1) - first - 1);
};
auto link_name = [](const std::string &line) {
size_t start = line.find("/Links/") + 7;
return line.substr(start, line.find('>') - start);
};
// Rotate v by unit quaternion (w, x, y, z)
auto rotate = [](const std::vector<float> &q, const vec3 &v) {
vec3 u(q[1], q[2], q[3]);
vec3 uv = cross(u, v);
vec3 uuv = cross(u, uv);
return v + 2.f * (q[0] * uv + uuv);
};

struct Pose {
vec3 position;
std::vector<float> orient;
};
struct JointAnchors {
std::string name, body0, body1;
vec3 pos0, pos1;
};
std::map<std::string, Pose> poses;
std::vector<JointAnchors> joints;

std::string line, current_xform;
bool in_joint = false;
JointAnchors joint;
while (std::getline(file, line)) {
if (line.find("def Xform \"") != std::string::npos) {
current_xform = quoted_name(line);
} else if (line.find("float3 xformOp:translate = ") != std::string::npos) {
std::vector<float> t = parse_tuple(line);
poses[current_xform].position = make_vec3(t[0], t[1], t[2]);
} else if (line.find("quatf xformOp:orient = ") != std::string::npos) {
poses[current_xform].orient = parse_tuple(line);
} else if (line.find("def PhysicsSphericalJoint \"") != std::string::npos) {
in_joint = true;
joint = JointAnchors();
joint.name = quoted_name(line);
} else if (in_joint && line.find("rel physics:body0") != std::string::npos) {
joint.body0 = link_name(line);
} else if (in_joint && line.find("rel physics:body1") != std::string::npos) {
joint.body1 = link_name(line);
} else if (in_joint && line.find("physics:localPos0") != std::string::npos) {
std::vector<float> p = parse_tuple(line);
joint.pos0 = make_vec3(p[0], p[1], p[2]);
} else if (in_joint && line.find("physics:localPos1") != std::string::npos) {
std::vector<float> p = parse_tuple(line);
joint.pos1 = make_vec3(p[0], p[1], p[2]);
joints.push_back(joint);
in_joint = false;
}
}
file.close();
std::remove(filename.c_str());

DOCTEST_REQUIRE(!joints.empty());

int leaf_joints = 0;
int fruit_joints = 0;
int misaligned = 0;
for (const auto &j : joints) {
DOCTEST_REQUIRE(poses.count(j.body0) == 1);
DOCTEST_REQUIRE(poses.count(j.body1) == 1);
const Pose &p0 = poses[j.body0];
const Pose &p1 = poses[j.body1];
vec3 anchor0 = p0.position + rotate(p0.orient, j.pos0);
vec3 anchor1 = p1.position + rotate(p1.orient, j.pos1);
if ((anchor0 - anchor1).magnitude() > 1e-3f) {
misaligned++;
}
if (j.body1.find("_Leaf") != std::string::npos) {
leaf_joints++;
} else if (j.body1.find("_Fruit") != std::string::npos) {
fruit_joints++;
}
}

DOCTEST_CHECK(misaligned == 0);
// Make sure the organ joints are actually covered
DOCTEST_CHECK(leaf_joints > 0);
DOCTEST_CHECK(fruit_joints > 0);
}

DOCTEST_TEST_CASE("USD export link names are unique") {
// A phytomer can carry more than one fruiting bud. Their fruit links must still get distinct names, or the
// .usda file fails to load with a "Duplicate prim" error. This seed grows such a phytomer on the apple model.
Context context;
context.seedRandomGenerator(9);
PlantArchitecture plantarchitecture(&context);
plantarchitecture.disableMessages();
plantarchitecture.loadPlantModelFromLibrary("apple");

uint plantID = plantarchitecture.buildPlantInstanceFromLibrary(make_vec3(0, 0, 0), 560);

std::string filename = "test_usd_unique_names.usda";
plantarchitecture.writePlantStructureUSD(plantID, filename);

std::ifstream file(filename);
DOCTEST_REQUIRE(file.is_open());

std::set<std::string> names;
std::vector<std::string> duplicates;
int fruit_links = 0;
std::string line;
while (std::getline(file, line)) {
size_t pos = line.find("def Xform \"");
if (pos == std::string::npos) {
continue;
}
size_t start = pos + 11;
std::string name = line.substr(start, line.find('"', start) - start);
if (!names.insert(name).second) {
duplicates.push_back(name);
}
if (name.find("Fruit") != std::string::npos) {
fruit_links++;
}
}
file.close();
std::remove(filename.c_str());

DOCTEST_CHECK(fruit_links > 0);
DOCTEST_CHECK(duplicates.empty());
}

DOCTEST_TEST_CASE("USD export minimum segment filtering") {
Context context;
PlantArchitecture plantarchitecture(&context);
Expand Down