Use relative paths in updateFromXMLNode() for socket connectees

OpenSim/Common/Component.h

@@ -2935,8 +2935,13 @@ void Socket<C>::findAndConnect(const Component& root) {
             comp =  &getOwner().template getComponent<C>(path.toString());
         }
     }
-    catch (const ComponentNotFoundOnSpecifiedPath&) {
-        // TODO leave out for hackathon std::cout << ex.getMessage() << std::endl;
+    catch (const ComponentNotFoundOnSpecifiedPath& ex) {
+        if (Object::getDebugLevel() > 0) {
+            // TODO once we fix how connections are established when building
+            // models programmatically, we should show this warning even for
+            // debug level 0.
+            std::cout << ex.getMessage() << std::endl;
+        }
         comp =  root.template findComponent<C>(path.toString());
     }
     if (comp)

OpenSim/Common/XMLDocument.cpp

@@ -31,6 +31,7 @@
 //-----------------------------------------------------------------------------
 #include "XMLDocument.h"
 #include "Object.h"
+#include <functional>
 
 
 using namespace OpenSim;
@@ -469,7 +470,7 @@ void XMLDocument::updateConnectors30508(SimTK::Xml::Element& componentElt)
     componentElt.eraseNode(connectors_node);
 }
 
-void XMLDocument::addPhysicalOffsetFrame(SimTK::Xml::Element& element,
+void XMLDocument::addPhysicalOffsetFrame30505(SimTK::Xml::Element& element,
     const std::string& frameName,
     const std::string& parentFrameName, 
     const SimTK::Vec3& location, const SimTK::Vec3& orientation)
@@ -487,7 +488,12 @@ void XMLDocument::addPhysicalOffsetFrame(SimTK::Xml::Element& element,
     newFrameElement.setAttributeValue("name", frameName);
     //newFrameElement.writeToString(debug);
 
-    XMLDocument::addConnector(newFrameElement, "Connector_PhysicalFrame_", "parent", parentFrameName);
+    // This function always adds the frame as a subcomponent of a component
+    // that is 1 level deep, making this frame two levels deep. The connectee
+    // is always only 1 level deep, so prepending "../../" yields the correct
+    // relative path.
+    XMLDocument::addConnector(newFrameElement, "Connector_PhysicalFrame_",
+            "parent", "../../" + parentFrameName);
 
     std::ostringstream transValue;
     transValue << location[0] << " " << location[1] << " " << location[2];
@@ -502,3 +508,38 @@ void XMLDocument::addPhysicalOffsetFrame(SimTK::Xml::Element& element,
     frames_node->insertNodeAfter(frames_node->element_end(), newFrameElement);
     //frames_node->writeToString(debug);
 }
+
+SimTK::Xml::Element XMLDocument::findElementWithName(
+        SimTK::Xml::Element& element, const std::string& name) {
+    using namespace SimTK;
+
+    if (name.empty()) return Xml::Element();
+
+    // First, get to the root of the XML document.
+    Xml::Element current = element;
+    while (current.hasParentElement())
+        current = current.getParentElement();
+    Xml::Element root = current;
+
+    // This will be a recursive lambda function.
+    std::function<Xml::Element(Xml::Element&, const std::string&)>
+        searchForElement;
+    // For recursion, must capture the function itself.
+    // Returns an invalid Element if no element with `name` could be found.
+    searchForElement = [&searchForElement](
+            Xml::Element& elem, const std::string& name) -> Xml::Element {
+        // This is a depth-first search.
+        for (auto it = elem.element_begin(); it != elem.element_end();
+                ++it) {
+            std::string elemName = it->getOptionalAttributeValue("name");
+            if (elemName == name)
+                return elem;
+            Xml::Element foundElem = searchForElement(*it, name);
+            if (foundElem.isValid())
+                return foundElem;
+            // Keep searching other branches.
+        }
+        return Xml::Element(); // Did not find.
+    };
+    return searchForElement(root, name);
+}

OpenSim/Common/XMLDocument.h

@@ -128,10 +128,17 @@ class OSIMCOMMON_API XMLDocument  : public SimTK::Xml::Document {
     If there is no `<connectors>` element, then this function does not edit
     componentElt. */
     static void updateConnectors30508(SimTK::Xml::Element& componentElt);
-    static void addPhysicalOffsetFrame(SimTK::Xml::Element& element,
+    static void addPhysicalOffsetFrame30505(SimTK::Xml::Element& element,
         const std::string& frameName,
         const std::string& parentFrameName,
         const SimTK::Vec3& location, const SimTK::Vec3& orientation);
+    /** Find the first XML Element (depth-first search) with the provided
+    `name`, anywhere in the XML document that contains `element`. If the XML
+    document does not contain an element with name `name`, or if `name` is
+    empty, then the returned Xml::Element is empty (Xml::Element::isValid()
+    returns false). */
+    static SimTK::Xml::Element findElementWithName(
+            SimTK::Xml::Element& element, const std::string& name);
 
 private:
     static bool isElementEqual(SimTK::Xml::Element& elt1, SimTK::Xml::Element& elt2, double toleranceForDoubles);

OpenSim/Simulation/Model/AbstractPathPoint.cpp

@@ -74,6 +74,11 @@ void AbstractPathPoint::updateFromXMLNode(SimTK::Xml::Element& aNode,
             std::string bodyName("");
             if (bodyElement != aNode.element_end()) {
                 bodyElement->getValueAs<std::string>(bodyName);
+                // PathPoints in pre-4.0 models are necessarily 3 levels deep
+                // (model, muscle, geometry path), and Bodies are necessarily
+                // 1 level deep: prepend "../../../" to get the correct
+                // relative path.
+                if (!bodyName.empty()) bodyName = "../../../" + bodyName;
                 XMLDocument::addConnector(aNode, "Connector_PhysicalFrame_",
                     "parent_frame", bodyName);
             }

OpenSim/Simulation/Model/ConditionalPathPoint.cpp

@@ -67,10 +67,32 @@ void ConditionalPathPoint::updateFromXMLNode(SimTK::Xml::Element& node,
     if (versionNumber < 30505) {
         // replace old properties with latest use of Sockets
         SimTK::Xml::element_iterator coord = node.element_begin("coordinate");
-        std::string coord_name("");
+        std::string coordName("");
         if (coord != node.element_end())
-            coord->getValueAs<std::string>(coord_name);
-        XMLDocument::addConnector(node, "Connector_Coordinate_", "coordinate", coord_name);
+            coord->getValueAs<std::string>(coordName);
+
+        // As a backup, we will specify just the coordinate name as the
+        // connectee name...
+        std::string connectee_name = coordName;
+
+        // ...but if possible, we try to create a relative path from this
+        // PathPoint to the coordinate.
+        SimTK::Xml::Element coordElem = 
+            XMLDocument::findElementWithName(node, coordName);
+        if (coordElem.isValid() && coordElem.hasParentElement()) {
+            // We found an Xml Element with the coordinate's name.
+            std::string jointName =
+                coordElem.getParentElement().getOptionalAttributeValue("name");
+            // PathPoints in pre-4.0 models are necessarily
+            // 3 levels deep (model, muscle, geometry path), and Coordinates 
+            // are necessarily 2 levels deep: prepend "../../../<joint-name>/"
+            // to get the correct relative path.
+            if (!jointName.empty())
+                connectee_name = "../../../" + jointName + "/" + coordName;
+        }
+
+        XMLDocument::addConnector(node, "Connector_Coordinate_", "coordinate",
+                connectee_name);
     }
 
     // Call base class now assuming _node has been corrected for current version

OpenSim/Simulation/Model/ContactGeometry.cpp

@@ -126,6 +126,10 @@ void ContactGeometry::updateFromXMLNode(SimTK::Xml::Element& node,
             if (bodyElement != node.element_end()) {
                 bodyElement->getValueAs<std::string>(body_name);
             }
+            // ContactGeometry in pre-4.0 models are necessarily 1 level deep
+            // (model, contact_geom), and Bodies are necessarily 1 level deep:
+            // prepend "../" to get the correct relative path.
+            if (!body_name.empty()) body_name = "../" + body_name;
             XMLDocument::addConnector(node, "Connector_PhysicalFrame_",
                     "frame", body_name);
         }

OpenSim/Simulation/Model/ExternalLoads.cpp

@@ -439,7 +439,13 @@ void ExternalLoads::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNum
                 PrescribedForce& oldPrescribedForce = oldForces.get(i);
                 ExternalForce* newExternalForce = new ExternalForce();
                 newExternalForce->setName(oldPrescribedForce.getName());
-                newExternalForce->setAppliedToBodyName(oldPrescribedForce.getBodyName());
+                // In 4.0, PrescribedForce's body_name became a relative path
+                // to the body; we need to pull off just the body name.
+                std::string bodyName = oldPrescribedForce.getBodyName();
+                const auto slashLoc = bodyName.rfind('/');
+                if (slashLoc != std::string::npos)
+                    bodyName = bodyName.substr(slashLoc + 1);
+                newExternalForce->setAppliedToBodyName(bodyName);
                 newExternalForce->setPointExpressedInBodyName("ground");
                 newExternalForce->setForceExpressedInBodyName("ground");
                 // Reconstruct function names and use these to extract the identifier(s)

OpenSim/Simulation/Model/Marker.cpp

@@ -179,6 +179,10 @@ void Marker::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
             connectorsElement.insertNodeAfter(connectorsElement.node_end(), frameElement);
             frameElement.setAttributeValue("name", "parent_frame");
             SimTK::Xml::Element connecteeElement("connectee_name");
+            // Markers in pre-4.0 models are necessarily 1 level deep
+            // (model, markers), and Bodies were necessarily 1 level deep:
+            // prepend "../" to get the correct relative path.
+            if (!bName.empty()) bName = "../" + bName;
             connecteeElement.setValue(bName);
             frameElement.insertNodeAfter(frameElement.node_end(), connecteeElement);
             aNode.insertNodeAfter(bIter, connectorsElement);

OpenSim/Simulation/Model/Model.cpp

@@ -237,10 +237,22 @@ void Model::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
                     SimTK::String parent_name = "ground";
                     parentBodyElement->getValueAs<SimTK::String>(parent_name);
 
+                    // In version 30501, this Joint is 1 level deep (in the
+                    // model's JointSet), and the Bodies are necessarily 1
+                    // level deep (in the model's BodySet). Prepend "../" to
+                    // get the correct relative path.
+                    std::string parent_frame = parent_name;
+                    if (!parent_frame.empty())
+                        parent_frame = "../" + parent_frame;
                     XMLDocument::addConnector(*concreteJointNode, 
-                        "Connector_PhysicalFrame_", "parent_frame", parent_name);
+                        "Connector_PhysicalFrame_", "parent_frame",
+                        parent_frame);
+                    std::string child_frame = body_name;
+                    if (!child_frame.empty())
+                        child_frame = "../" + child_frame;
                     XMLDocument::addConnector(*concreteJointNode, 
-                        "Connector_PhysicalFrame_", "child_frame", body_name);
+                        "Connector_PhysicalFrame_", "child_frame",
+                        child_frame);
                     concreteJointNode->eraseNode(parentBodyElement);
                     jointObjects.insertNodeAfter(jointObjects.node_end(),
                         *concreteJointNode);

OpenSim/Simulation/Model/MovingPathPoint.cpp

@@ -162,20 +162,48 @@ void MovingPathPoint::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionN
         SimTK::Xml::element_iterator zCoord = aNode.element_begin("z_coordinate");
 
         std::string xCoord_name(""), yCoord_name(""), zCoord_name("");
-        // If default constructed then elements not serialized since they are default
-        // values. Check that we have associated elements, then extract their values.
+        // If default constructed then elements not serialized since they are
+        // default values. Check that we have associated elements, then extract
+        // their values.
         if (xCoord != aNode.element_end())
             xCoord->getValueAs<std::string>(xCoord_name);
         if (yCoord != aNode.element_end())
             yCoord->getValueAs<std::string>(yCoord_name);
         if (zCoord != aNode.element_end())
             zCoord->getValueAs<std::string>(zCoord_name);
+
+        // Helper function to try creating relative paths to the coordinates.
+        auto createConnecteeName = [](SimTK::Xml::Element& elem, 
+                const std::string& coordName) -> std::string {
+            if (coordName.empty()) return coordName;
+            // As a backup, we will specify just the coordinate name as the
+            // connectee name...
+            std::string connectee_name = coordName;
+            // ...but if possible, we try to create a relative path from this
+            // PathPoint to the coordinate.
+            SimTK::Xml::Element coordElem = 
+                XMLDocument::findElementWithName(elem, coordName);
+            if (coordElem.isValid() && coordElem.hasParentElement()) {
+                // We found an Xml Element with the coordinate's name.
+                const auto jointElem = coordElem.getParentElement();
+                std::string jointName =
+                    jointElem.getOptionalAttributeValue("name");
+                // PathPoints in pre-4.0 models are necessarily 3 levels deep
+                // (model, muscle, geometry path), and Coordinates were
+                // necessarily 2 level deep: prepend "../../../<joint-name>/"
+                // to get the correct relative path.
+                if (!jointName.empty())
+                    connectee_name = "../../../" + jointName + "/" + coordName;
+            }
+            return connectee_name;
+        };
+
         XMLDocument::addConnector(aNode, "Connector_Coordinate_", 
-            "x_coordinate", xCoord_name);
+            "x_coordinate", createConnecteeName(aNode, xCoord_name));
         XMLDocument::addConnector(aNode, "Connector_Coordinate_", 
-            "y_coordinate", yCoord_name);
+            "y_coordinate", createConnecteeName(aNode, yCoord_name));
         XMLDocument::addConnector(aNode, "Connector_Coordinate_", 
-            "z_coordinate", zCoord_name);
+            "z_coordinate", createConnecteeName(aNode, zCoord_name));
     }
 
     // Call base class now assuming _node has been corrected for current version

OpenSim/Simulation/Model/PhysicalFrame.cpp

@@ -194,7 +194,11 @@ void PhysicalFrame::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNum
                         SimTK::Xml::Element scaleFactorsNode("scale_factors", localScaleStr.str());
                         meshNode.insertNodeAfter(meshNode.element_end(), scaleFactorsNode);
                         meshNode.insertNodeAfter(meshNode.element_end(), meshFileNode);
-                        XMLDocument::addConnector(meshNode, "Connector_Frame_", "frame", bodyName);
+                        // The transform is relative to the owning component
+                        // (this PhysicalFrame), so the connectee name just
+                        // points up to this component ("..").
+                        XMLDocument::addConnector(meshNode, "Connector_Frame_",
+                                "frame", "..");
                         geometrySetNode.insertNodeAfter(geometrySetNode.element_end(), meshNode);
                     }
                 }
@@ -274,10 +278,17 @@ void PhysicalFrame::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bod
                     componentsNode = bodyNode.element_begin("components");
                 }
 
-                // Create Frame from composed transform and insert in components list
-                createFrameForXform(componentsNode, frameName, composedXformVec6, bodyName);
-
-                XMLDocument::addConnector(meshNode, "Connector_Frame_", "frame", frameName);
+                // Create Frame from composed transform and insert in
+                // components list.
+                // The transform is relative to the owning component (this
+                // PhysicalFrame), so the connectee name just points up to this
+                // component ("..").
+                createFrameForXform(componentsNode, frameName,
+                        composedXformVec6, "..");
+
+                // For Geometry under 'attached_geometry', the 'frame' is
+                // always the owning component.
+                XMLDocument::addConnector(meshNode, "Connector_Frame_", "frame", "..");
                 SimTK::Xml::element_iterator parentFrame = componentsNode->element_begin("PhysicalOffsetFrame");
                 while (parentFrame->getRequiredAttributeValue("name") != frameName)
                     parentFrame++;
@@ -293,7 +304,10 @@ void PhysicalFrame::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bod
 
             }
             else
-                XMLDocument::addConnector(meshNode, "Connector_Frame_", "frame", bodyName);
+                // For Geometry under 'attached_geometry', the 'frame' is
+                // always the owning component.
+                XMLDocument::addConnector(meshNode, "Connector_Frame_",
+                        "frame", "..");
             // scale_factor
             SimTK::Vec3 localScale(1.);
             SimTK::Xml::element_iterator localScaleIter = displayGeomIter->element_begin("scale_factors");
@@ -339,19 +353,22 @@ void PhysicalFrame::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bod
     }
 }
 
-// This private method creates a frame in the owner model document with passed in name and content relative to bodyName
-void PhysicalFrame::createFrameForXform(const SimTK::Xml::element_iterator& frameSetIter, const std::string& frameName, const SimTK::Vec6& localXform, const std::string& bodyName)
+void PhysicalFrame::createFrameForXform(
+        const SimTK::Xml::element_iterator& ownerIter,
+        const std::string& frameName, const SimTK::Vec6& localXform,
+        const std::string& parentConnecteeName)
 {
     SimTK::Xml::Element frameNode("PhysicalOffsetFrame");
     frameNode.setAttributeValue("name", frameName);
-    stringstream ss;
-    ss << localXform[3] << " " << localXform[4] << " " << localXform[5];
-    SimTK::Xml::Element translationNode("translation", ss.str());
-    ss.clear();
-    ss << localXform[0] << " " << localXform[1] << " " << localXform[2];
-    SimTK::Xml::Element orientationNode("rotation", ss.str());
+    stringstream ssT;
+    ssT << localXform[3] << " " << localXform[4] << " " << localXform[5];
+    SimTK::Xml::Element translationNode("translation", ssT.str());
+    stringstream ssO;
+    ssO << localXform[0] << " " << localXform[1] << " " << localXform[2];
+    SimTK::Xml::Element orientationNode("orientation", ssO.str());
     frameNode.insertNodeAfter(frameNode.element_end(), translationNode);
     frameNode.insertNodeAfter(frameNode.element_end(), orientationNode);
-    frameSetIter->insertNodeAfter(frameSetIter->element_end(), frameNode);
-    XMLDocument::addConnector(frameNode, "Connector_PhysicalFrame_", "parent", bodyName);
+    ownerIter->insertNodeAfter(ownerIter->element_end(), frameNode);
+    XMLDocument::addConnector(frameNode, "Connector_PhysicalFrame_", "parent",
+            parentConnecteeName);
 }

OpenSim/Simulation/Model/PhysicalFrame.h

@@ -212,11 +212,13 @@ class OSIMSIMULATION_API PhysicalFrame : public Frame {
         const SimTK::Vec6& outerTransform,
         SimTK::Xml::Element& geomSetElement);
 
-    /* Utility to construct a PhysicalOffsetFrame from properties of an
-       offset transform. */
+    /* Utility for updating XML: add a PhysicalOffsetFrame to the XML element
+    pointed to by ownerIter, and gives the frame the provided name, transform
+    (from the frame's parent frame), and the path (connectee name) to the
+    parent frame.*/
     static void createFrameForXform(const SimTK::Xml::element_iterator&,
         const std::string& frameName,
-        const SimTK::Vec6& localXform, const std::string& bodyName);
+        const SimTK::Vec6& localXform, const std::string& parentConnecteeName);
 
 
     /* ID for the underlying mobilized body in Simbody system.