void XMLDocument::addPhysicalOffsetFrame(SimTK::Xml::Element& element,
    const std::string& frameName,
    const std::string& parentFrameName, 
    const SimTK::Vec3& location, const SimTK::Vec3& orientation)
{
    SimTK::Xml::element_iterator  frames_node = element.element_begin("frames");
    //SimTK::String debug; //Only used for debugging
    
    if (frames_node == element.element_end()) {
        SimTK::Xml::Element framesElement("frames");
        element.insertNodeBefore(element.element_begin(), framesElement);
        frames_node = element.element_begin("frames");
    }
    // Here we're guaranteed frames node exists, add individual frame
    SimTK::Xml::Element newFrameElement("PhysicalOffsetFrame");
    newFrameElement.setAttributeValue("name", frameName);
    //newFrameElement.writeToString(debug);

    XMLDocument::addConnector(newFrameElement, "Connector_PhysicalFrame_", "parent", parentFrameName);

    std::ostringstream transValue;
    transValue << location[0] << " " << location[1] << " " << location[2];
    SimTK::Xml::Element translationElement("translation", transValue.str());
    newFrameElement.insertNodeAfter(newFrameElement.element_end(), translationElement);

    std::ostringstream orientValue; 
    orientValue << orientation[0] << " " << orientation[1] << " " << orientation[2];
    SimTK::Xml::Element orientationElement("orientation", orientValue.str());
    newFrameElement.insertNodeAfter(newFrameElement.element_end(), orientationElement);

    frames_node->insertNodeAfter(frames_node->element_end(), newFrameElement);
    //frames_node->writeToString(debug);
}
void ModelComponent::updateFromXMLNode(SimTK::Xml::Element& aNode,
        int versionNumber) {

    if (versionNumber < XMLDocument::getLatestVersion()) {
        if (versionNumber < 30506) {
            // geometry list property removed. Everything that was in this list
            // should be moved to the components list property.
            SimTK::Xml::element_iterator geometry = aNode.element_begin("geometry");
            if (geometry != aNode.element_end()) {
                // We found a list property of geometry.
                SimTK::Xml::Element componentsNode;
                SimTK::Xml::element_iterator componentsIt = aNode.element_begin("components");
                if (componentsIt == aNode.element_end()) {
                    // This component does not yet have a list property of
                    // components, so we'll create one.
                    componentsNode = SimTK::Xml::Element("components");
                    aNode.insertNodeBefore(aNode.element_begin(), componentsNode);
                } else {
                    componentsNode = *componentsIt;
                }
                // Copy each node under <geometry> into <components>.
                for (auto geomIt = geometry->element_begin();
                        geomIt != geometry->element_end(); ++geomIt) {
                    componentsNode.appendNode(geomIt->clone());
                }
                // Now that we moved over the geometry, we can delete the
                // <geometry> element.
                aNode.eraseNode(geometry);
            }
        }
    }
    Super::updateFromXMLNode(aNode, versionNumber);
}
/*
 * Helper function to add connector to the xmlElement passed in
 */
void XMLDocument::addConnector(SimTK::Xml::Element& element, const std::string& connectorTag, const std::string& connectorName, const std::string& connectorValue)
{
    SimTK::Xml::element_iterator  connectors_node =  element.element_begin("connectors");
    SimTK::String debug;
    if (connectors_node == element.element_end()){
        SimTK::Xml::Element connectorsElement("connectors");
        element.insertNodeBefore(element.element_begin(), connectorsElement);
        connectors_node =  element.element_begin("connectors");
    }
    // Here we're guaranteed connectors node exist, add individual connector
    SimTK::Xml::Element newConnectorElement(connectorTag);
    newConnectorElement.setAttributeValue("name", connectorName);
    newConnectorElement.writeToString(debug);
    SimTK::Xml::Element connecteeElement("connectee_name");
    connecteeElement.insertNodeAfter(connecteeElement.element_end(), SimTK::Xml::Text(connectorValue));
    // Insert text under newConnectorElement
    newConnectorElement.insertNodeAfter(newConnectorElement.element_end(), connecteeElement);
    connectors_node->insertNodeAfter(connectors_node->element_end(), newConnectorElement);
    connectors_node->writeToString(debug);
}