<NME> Body.cpp
<BEF> /* -------------------------------------------------------------------------- *
 *                             OpenSim:  Body.cpp                             *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Frank C. Anderson, Ajay Seth                                    *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//=============================================================================
// INCLUDES
//=============================================================================
#include "Body.h"
#include <OpenSim/Common/ScaleSet.h>

//=============================================================================
// STATICS
//=============================================================================
using namespace std;
//using namespace SimTK;
using namespace OpenSim;
using SimTK::Mat33;
using SimTK::Vec3;
using SimTK::DecorativeGeometry;

//=============================================================================
// CONSTRUCTOR(S)
//=============================================================================
//_____________________________________________________________________________
/**
 * Default constructor.
 */
Body::Body() : PhysicalFrame()
{
    constructProperties();
}

//_____________________________________________________________________________
/**
 * Constructor.
 */
Body::Body(const std::string &aName,double aMass,const SimTK::Vec3& aMassCenter,const SimTK::Inertia& aInertia) :
    PhysicalFrame()
{
    constructProperties();
    setName(aName);
    set_mass(aMass);
    set_mass_center(aMassCenter);
    setInertia(aInertia);
}

    set_mass_center(aMassCenter);
    setInertia(aInertia);
    // Better use name search or more robust method
    upd_GeometrySet(0).setFrameName(aName);
}

//_____________________________________________________________________________
    constructProperty_mass_center(SimTK::Vec3(0));
    constructProperty_inertia(SimTK::Vec6(0));
}


void Body::extendFinalizeFromProperties()
{
    Super::extendFinalizeFromProperties();
    const SimTK::MassProperties& massProps = getMassProperties();
    _internalRigidBody = SimTK::Body::Rigid(massProps);
    _slaves.clear();
}

//_____________________________________________________________________________
/* Connect this Body to the Model it belongs to
 *
 * @param aModel OpenSim model containing this Body.
 */
void Body::extendConnectToModel(Model& aModel)
{
    Super::extendConnectToModel(aModel);
    
    int nslaves = (int)_slaves.size();

    if (nslaves){
        int nbods = nslaves + 1; // include the master
        const SimTK::MassProperties& massProps = getMassProperties();
        SimTK::MassProperties slaveMassProps(massProps.getMass() / nbods,
            massProps.getMassCenter(), massProps.getUnitInertia());

        // update the portion taken on by the master
        _internalRigidBody = SimTK::Body::Rigid(slaveMassProps);

        // and the slaves
        for (int i = 0; i < nslaves; ++i){
            _slaves[i]->_internalRigidBody = SimTK::Body::Rigid(slaveMassProps);
            _slaves[i]->setInertia(slaveMassProps.getUnitInertia());
            _slaves[i]->setMass(slaveMassProps.getMass());
            _slaves[i]->setMassCenter(slaveMassProps.getMassCenter());
        }
    }
}

//=============================================================================
// GET AND SET
//=============================================================================

//_____________________________________________________________________________
/**
 * Get the inertia matrix of the body.
 *
 * @return 3x3 inertia matrix.
 */
const SimTK::Inertia& Body::getInertia() const
{
    // Has not been set programmatically
    if (_inertia.isNaN()){
        // initialize from properties
        const double& m = getMass();
        const SimTK::Vec6& Ivec = get_inertia();
        // if mass is zero, non-zero inertia makes no sense
        if (std::abs(m) <= SimTK::SignificantReal &&
                Ivec.norm() > SimTK::SignificantReal) {
            // force zero inertia
            log_warn("Body '{}' is massless but nonzero inertia provided.",getName());
            log_warn(" Inertia reset to zero. Otherwise provide nonzero mass.");
            _inertia = SimTK::Inertia(0);
        }
        else{
            try {
                _inertia = SimTK::Inertia(Ivec.getSubVec<3>(0), Ivec.getSubVec<3>(3));
            } 
            catch (const std::exception& ex){
                // Should throw an Exception but we have models we have released with
                // bad inertias. E.g. early gait23 models had an error in the inertia
                // of the toes Body. We cannot allow failures with our models so 
                // raise a warning and do something sensible with the values at hand.
                log_warn("Body {} has invalid inertia. ", getName());
                log_error(ex.what());

                // get some aggregate value for the inertia based on existing values
                double diag = Ivec.getSubVec<3>(0).norm()/sqrt(3.0);

                // and then assume a spherical shape.
                _inertia = SimTK::Inertia(Vec3(diag), Vec3(0));
                
                log_warn("{} Body's inertia being reset to: {}",
                    getName(), _inertia);
            }
        }
    }
    return _inertia;
}
//_____________________________________________________________________________
/**
 * Set the inertia matrix of the body.
 *
 * @param aInertia 9-element inertia matrix.
 */
void Body::setInertia(const SimTK::Inertia& inertia)
{
    _inertia = inertia;
    const SimTK::SymMat33& I = _inertia.asSymMat33();
    upd_inertia()[0] = I[0][0];
    upd_inertia()[1] = I[1][1];
    upd_inertia()[2] = I[2][2];
    upd_inertia()[3] = I[0][1];
    upd_inertia()[4] = I[0][2];
    upd_inertia()[5] = I[1][2];
}

//==============================================================================
// SCALING
//==============================================================================
void Body::scale(const SimTK::Vec3& scaleFactors, bool scaleMass)
{
    Super::scaleAttachedGeometry(scaleFactors);
    upd_mass_center() = get_mass_center().elementwiseMultiply(scaleFactors);
    scaleInertialProperties(scaleFactors, scaleMass);
}

void Body::extendScale(const SimTK::State& s, const ScaleSet& scaleSet)
{
    Super::extendScale(s, scaleSet);

    // Get scale factors (if an entry for this Body exists).
    const Vec3& scaleFactors = getScaleFactors(scaleSet, *this);
    if (scaleFactors == ModelComponent::InvalidScaleFactors)
        return;

    upd_mass_center() = get_mass_center().elementwiseMultiply(scaleFactors);
}

void Body::scaleInertialProperties(const SimTK::Vec3& scaleFactors, bool scaleMass)
{
    // Save the unscaled mass for possible use later.
    double unscaledMass = get_mass();

    // Calculate and store the product of the scale factors.
    double massScaleFactor = abs(scaleFactors[0] * scaleFactors[1] * scaleFactors[2]);

    // Scale the mass.
    if (scaleMass)
        upd_mass() *= massScaleFactor;
    // Fix issue #2871 by fmatari
    SimTK::Mat33 inertia = _inertia.toMat33();
    //SimTK::SymMat33 inertia = _inertia.asSymMat33();

    // If the mass is zero, then make the inertia tensor zero as well.
    // If the X, Y, Z scale factors are equal, then you can scale the
    // inertia tensor exactly by the square of the scale factor (and
    // possibly by massScaleFactor), since each element in the tensor
    // is proportional to the square of one or more dimensional
    // measurements. For determining if the scale factors are equal,
    // ignore reflections-- look only at the absolute value of the factors.
    if (get_mass() <= SimTK::Eps) {
        inertia *= 0.0;
    }
    else if (SimTK::isNumericallyEqual(scaleFactors[0], scaleFactors[1])
             && SimTK::isNumericallyEqual(scaleFactors[1], scaleFactors[2])) {
        // If the mass is also being scaled, scale the inertia terms by massScaleFactor.
        if (scaleMass) {
            inertia *= massScaleFactor;
        }

        // Now scale by the length-squared component.
        inertia *= (scaleFactors[0] * scaleFactors[0]);

    } else {
        // If the scale factors are not equal, then assume that the segment
        // is a cylinder and the inertia is calculated about one end of it.
        int axis;

        // 1. Find the smallest diagonal component. This dimension is the axis
        //    of the cylinder.
        if (inertia[0][0] <= inertia[1][1]){
            if (inertia[0][0] <= inertia[2][2])
                axis = 0;
            else
                axis = 2;

        } else if (inertia[1][1] <= inertia[2][2]) {
            axis = 1;

        } else {
            axis = 2;
        }

        // 2. The smallest inertia component is equal to 0.5 * mass * radius * radius,
        //    so you can rearrange and solve for the radius.
        int oa;
        double radius, rad_sqr, length;
        double term = 2.0 * inertia[axis][axis] / unscaledMass;
        if (term < 0.0)
            radius = 0.0;
        else
            radius = sqrt(term);

        // 3. Choose either of the other diagonal components and use it to solve for the
        //    length of the cylinder. This component is equal to:
        //    0.083 * mass * length * length  +  0.25 * mass * radius * radius
        if (axis == 0)
            oa = 1;
        else
            oa = 0;
        term = 12.0 * (inertia[oa][oa] - 0.25 * unscaledMass * radius * radius) / unscaledMass;
        if (term < 0.0)
            length = 0.0;
        else
            length = sqrt(term);

        // 4. Scale the radius and length, and recalculate the diagonal inertia terms.
        length *= scaleFactors[axis];

        if (axis == 0) {
            rad_sqr = radius * (scaleFactors[1]) * radius * (scaleFactors[2]);
            inertia[0][0] = 0.5 * get_mass() * rad_sqr;
            inertia[1][1] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);
            inertia[2][2] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);

        } else if (axis == 1) {
            rad_sqr = radius * (scaleFactors[0]) * radius * (scaleFactors[2]);
            inertia[0][0] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);
            inertia[1][1] = 0.5 * get_mass() * rad_sqr;
            inertia[2][2] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);

        } else {
            rad_sqr = radius * (scaleFactors[0]) * radius * (scaleFactors[1]);
            inertia[0][0] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);
            inertia[1][1] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);
            inertia[2][2] = 0.5 * get_mass() * rad_sqr;
        }

        // 5. Scale the inertia products, in case some are non-zero. These are scaled by
        //    two scale factors for the length term (which two depend on the inertia term
        //    being scaled), and, if the mass is also scaled, by massScaleFactor.
        inertia[0][1] *= ((scaleFactors[0] * scaleFactors[1]));
        inertia[0][2] *= ((scaleFactors[0] * scaleFactors[2]));
        inertia[1][0] *= ((scaleFactors[1] * scaleFactors[0]));
        inertia[1][2] *= ((scaleFactors[1] * scaleFactors[2]));
        inertia[2][0] *= ((scaleFactors[2] * scaleFactors[0]));
        inertia[2][1] *= ((scaleFactors[2] * scaleFactors[1]));

        if (scaleMass) {
            inertia[0][1] *= massScaleFactor;
            inertia[0][2] *= massScaleFactor;
            inertia[1][0] *= massScaleFactor;
            inertia[1][2] *= massScaleFactor;
            inertia[2][0] *= massScaleFactor;
            inertia[2][1] *= massScaleFactor;
        }
    }

    setInertia(SimTK::Inertia(inertia));
}

void Body::scaleInertialProperties(const ScaleSet& scaleSet, bool scaleMass)
{
    // Get scale factors (if an entry for this Body exists).
    const Vec3& scaleFactors = getScaleFactors(scaleSet, *this);
    if (scaleFactors == ModelComponent::InvalidScaleFactors)
        return;

    scaleInertialProperties(scaleFactors, scaleMass);
}

//_____________________________________________________________________________
/**
 * Scale the body's mass and inertia tensor (represents a scaling of the
 * body's density).
 *
 * @param aScaleFactors XYZ scale factors.
 */
void Body::scaleMass(double aScaleFactor)
{
    upd_mass() *= aScaleFactor;
    _inertia *= aScaleFactor;
    upd_inertia() *= aScaleFactor;
}

//=============================================================================
// UTILITY
//=============================================================================
SimTK::MassProperties Body::getMassProperties() const
{
    const double& m = get_mass();
    const Vec3& com = get_mass_center();
    try {
        OPENSIM_THROW_IF_FRMOBJ(!(m >= 0.0), Exception,
                "Body " + getName() + " has unspecified or negative mass.");

        const SimTK::Inertia& Icom = getInertia();

        SimTK::Inertia Ib = Icom;
        // If com and body, b, frame are coincident then don't bother shifting
        if (com.norm() > SimTK::Eps) {
            // shift if com has nonzero distance from b
            Ib = Icom.shiftFromMassCenter(com, m);
        }
    
        return SimTK::MassProperties(m, com, Ib);
    }
    catch (const std::exception& ex) {
        string msg = "Body " + getName() + " has invalid mass properties. ";
        msg += ex.what();
        throw Exception(msg, __FILE__, __LINE__);
    }
}

//=============================================================================
// I/O
//=============================================================================

void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
{
    if (versionNumber < XMLDocument::getLatestVersion()) {
        if (versionNumber < 30500) {
            SimTK::Vec6 newInertia(1.0, 1.0, 1.0, 0., 0., 0.);
            std::string inertiaComponents[] = { "inertia_xx", "inertia_yy", "inertia_zz", "inertia_xy", "inertia_xz", "inertia_yz" };
            for (int i = 0; i < 6; ++i) {
                SimTK::Xml::element_iterator iIter = aNode.element_begin(inertiaComponents[i]);
                if (iIter != aNode.element_end()) {
                    newInertia[i] = iIter->getValueAs<double>();
                    aNode.removeNode(iIter);
                }
            }
            std::ostringstream strs;
            for (int i = 0; i < 6; ++i) {
                strs << newInertia[i];
                if (i < 5) strs << " ";
            }
            std::string strInertia = strs.str();
            SimTK::Xml::Element inertiaNode("inertia", strInertia);
            aNode.insertNodeAfter(aNode.element_end(), inertiaNode);
        }
    }
    Super::updateFromXMLNode(aNode, versionNumber);
}

Body* Body::addSlave()
{
    Body* slave = new Body();
    int count = (int)_slaves.size();

    stringstream name;
    name << getName() << "_slave_" << count;
    slave->setName(name.str());

    //add to internal list of references 
    _slaves.push_back(SimTK::ReferencePtr<Body>(slave));

    //add to list of subcomponents to automatically add to system and initialize
    adoptSubcomponent(slave);

    return slave;
}
                        geometryIter->getValueAs(oldGeometryFiles);
                    }
                    std::string bodyName = aNode.getRequiredAttribute("name").getValue();
                    SimTK::Xml::Element geometrySetNode("GeometrySet");
                    aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);
                    // Create Mesh node for each item in oldGeometryFiles
                    for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {
                    SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");
                    if (geomSetIter != visObjElement.element_end()){
                        convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);
                    }
                }
                // Regardless add a node for FrameGeometry to control the display of BodyFrame
                frameRepresentation.setValue("0");
                appearanceNode.insertNodeAfter(appearanceNode.element_end(), frameRepresentation);
                bodyFrameNode.insertNodeAfter(bodyFrameNode.element_end(), appearanceNode);
                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("GeometrySet");
                if (geomSetIter != aNode.element_end()){
                    geomSetIter->insertNodeAfter(geomSetIter->node_end(), bodyFrameNode);
                }
            }
        }
    }
    SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");

    if (objectsIter != geomSetElement.element_end()){
        SimTK::Xml::Element geometrySetNode("GeometrySet");
        bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);

        SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");

<MSG> Rename container of Geometry property to "geometry" instead of "GeometrySet"

<DFF> @@ -66,7 +66,7 @@ Body::Body(const std::string &aName,double aMass,const SimTK::Vec3& aMassCenter,
     set_mass_center(aMassCenter);
     setInertia(aInertia);
     // Better use name search or more robust method
-    upd_GeometrySet(0).setFrameName(aName);
+    upd_geometry(0).setFrameName(aName);
 }
 
 //_____________________________________________________________________________
@@ -446,7 +446,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
                         geometryIter->getValueAs(oldGeometryFiles);
                     }
                     std::string bodyName = aNode.getRequiredAttribute("name").getValue();
-                    SimTK::Xml::Element geometrySetNode("GeometrySet");
+                    SimTK::Xml::Element geometrySetNode("geometry");
                     aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);
                     // Create Mesh node for each item in oldGeometryFiles
                     for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {
@@ -468,6 +468,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
                     SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");
                     if (geomSetIter != visObjElement.element_end()){
                         convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);
+                        geomSetIter->setElementTag("geometry");
                     }
                 }
                 // Regardless add a node for FrameGeometry to control the display of BodyFrame
@@ -480,10 +481,13 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
                 frameRepresentation.setValue("0");
                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), frameRepresentation);
                 bodyFrameNode.insertNodeAfter(bodyFrameNode.element_end(), appearanceNode);
-                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("GeometrySet");
+                //SimTK::String log;
+                //aNode.writeToString(log);
+                //std::cout << log << std::endl;
+                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("geometry");
                 if (geomSetIter != aNode.element_end()){
                     geomSetIter->insertNodeAfter(geomSetIter->node_end(), bodyFrameNode);
-                }
+                 }
             }
         }
     }
@@ -499,7 +503,7 @@ void Body::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bodyNode,
     SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");
 
     if (objectsIter != geomSetElement.element_end()){
-        SimTK::Xml::Element geometrySetNode("GeometrySet");
+        SimTK::Xml::Element geometrySetNode("geometry");
         bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);
 
         SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");

{'commit': 'diff --git a/OpenSim/Examples/CustomActuatorExample/toyLeg_example.cpp b/OpenSim/Examples/CustomActuatorExample/toyLeg_example.cpp\nindex ee19617d5..389ab5dff 100644\n--- a/OpenSim/Examples/CustomActuatorExample/toyLeg_example.cpp\n+++ b/OpenSim/Examples/CustomActuatorExample/toyLeg_example.cpp\n@@ -84,7 +84,7 @@ int main()\n         Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));\n         cyl2Frame->setName("Cyl2_frame");\n         osimModel.addFrame(cyl2Frame);\n-        (linkage2->upd_GeometrySet(0)).setFrameName("Cyl2_frame");\n+        (linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");\n         // Creat a block to be the pelvis\n         double blockMass = 20.0, blockSideLength = 0.2;\n         Vec3 blockMassCenter(0);\ndiff --git a/OpenSim/Simulation/Model/Geometry.cpp b/OpenSim/Simulation/Model/Geometry.cpp\nindex 552e6a3b7..b54f7594c 100644\n--- a/OpenSim/Simulation/Model/Geometry.cpp\n+++ b/OpenSim/Simulation/Model/Geometry.cpp\n@@ -94,6 +94,7 @@ void Geometry::setDecorativeGeometryTransform(SimTK::Array_<SimTK::DecorativeGeo\n     for (unsigned i = 0; i < decorations.size(); i++){\n         decorations[i].setBodyId(idx);\n         decorations[i].setTransform(transformInBaseFrame);\n+        decorations[i].setIndexOnBody(i);\n     }\n }\n void Sphere::createDecorativeGeometry(SimTK::Array_<SimTK::DecorativeGeometry>& decoGeoms) const\ndiff --git a/OpenSim/Simulation/Model/Ground.cpp b/OpenSim/Simulation/Model/Ground.cpp\nindex 9ed66a84f..5416beefe 100644\n--- a/OpenSim/Simulation/Model/Ground.cpp\n+++ b/OpenSim/Simulation/Model/Ground.cpp\n@@ -43,7 +43,7 @@ Ground::Ground() : PhysicalFrame()\n {\n     setName("ground");\n     setAuthors("Ajay Seth");\n-    upd_GeometrySet(0).setFrameName("ground");\n+    upd_geometry(0).setFrameName("ground");\n }\n \n /*\ndiff --git a/OpenSim/Simulation/Model/Model.cpp b/OpenSim/Simulation/Model/Model.cpp\nindex a710d6b58..1ea477a5c 100644\n--- a/OpenSim/Simulation/Model/Model.cpp\n+++ b/OpenSim/Simulation/Model/Model.cpp\n@@ -223,6 +223,7 @@ void Model::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                         }\n                         SimTK::Xml::element_iterator geomIter = bodyIter->element_begin("GeometrySet");\n                         if (geomIter != bodyIter->element_end()){ // Actual Geometry was found\n+                            geomIter->setElementTag("geometry");\n                             gndIter->insertNodeAfter(gndIter->node_end(), bodyIter->removeNode(geomIter));\n                         }\n                         SimTK::Xml::element_iterator wrapIter = bodyIter->element_begin("WrapObjectSet");\ndiff --git a/OpenSim/Simulation/Model/ModelComponent.cpp b/OpenSim/Simulation/Model/ModelComponent.cpp\nindex ce755da72..395f3d2c0 100644\n--- a/OpenSim/Simulation/Model/ModelComponent.cpp\n+++ b/OpenSim/Simulation/Model/ModelComponent.cpp\n@@ -34,19 +34,19 @@ namespace OpenSim {\n //==============================================================================\n ModelComponent::ModelComponent() : _model(NULL) \n {\n-    constructProperty_GeometrySet();\n+    constructProperty_geometry();\n }\n \n ModelComponent::ModelComponent(const std::string& fileName, bool updFromXMLNode)\n :   Component(fileName, updFromXMLNode), _model(NULL)\n {\n-    constructProperty_GeometrySet();\n+    constructProperty_geometry();\n }\n \n ModelComponent::ModelComponent(SimTK::Xml::Element& element) \n :   Component(element), _model(NULL)\n {\n-    constructProperty_GeometrySet();\n+    constructProperty_geometry();\n }\n \n const Model& ModelComponent::getModel() const\n@@ -82,10 +82,10 @@ void ModelComponent::extendConnect(Component &root)\n void ModelComponent::extendFinalizeFromProperties()\n {\n     Super::extendFinalizeFromProperties();\n-    int geomSize = getProperty_GeometrySet().size();\n+    int geomSize = getProperty_geometry().size();\n     if (geomSize > 0){\n         for (int i = 0; i < geomSize; ++i){\n-            addComponent(&upd_GeometrySet(i));\n+            addComponent(&upd_geometry(i));\n         }\n     }\n }\n@@ -107,8 +107,8 @@ void ModelComponent::addGeometry(OpenSim::Geometry& geom) {\n             ss << getName() << "_geom_" << index;\n             std::string candidate = ss.str();\n             bool exists = false;\n-            for (int idx = 0; idx < getProperty_GeometrySet().size() && !exists; idx++){\n-                if (get_GeometrySet(idx).getName() == candidate){\n+            for (int idx = 0; idx < getProperty_geometry().size() && !exists; idx++){\n+                if (get_geometry(idx).getName() == candidate){\n                     exists = true;\n                     break;\n                 }\n@@ -122,7 +122,7 @@ void ModelComponent::addGeometry(OpenSim::Geometry& geom) {\n         }\n         \n     }\n-    append_GeometrySet(geom);\n+    append_geometry(geom);\n     return;\n }\n \ndiff --git a/OpenSim/Simulation/Model/ModelComponent.h b/OpenSim/Simulation/Model/ModelComponent.h\nindex fd72101c8..b5112995c 100644\n--- a/OpenSim/Simulation/Model/ModelComponent.h\n+++ b/OpenSim/Simulation/Model/ModelComponent.h\n@@ -87,7 +87,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(ModelComponent, Component);\n These are the serializable properties associated with this class. **/\n /**@{**/\n public:\n-    OpenSim_DECLARE_LIST_PROPERTY(GeometrySet, Geometry,\n+    OpenSim_DECLARE_LIST_PROPERTY(geometry, Geometry,\n         "Geometry that can be attached to the Model Component");\n /**@}**/\n //==============================================================================\n@@ -123,7 +123,7 @@ public:\n      * maintained by the ModelComponent.\n      */\n     int getGeometrySize() const {\n-        return getProperty_GeometrySet().size();\n+        return getProperty_geometry().size();\n     }\n     /**\n      * Copy the passed in Geometry and add the copy to the visualization of \ndiff --git a/OpenSim/Simulation/Model/PhysicalFrame.cpp b/OpenSim/Simulation/Model/PhysicalFrame.cpp\nindex 1813a0244..deb761346 100644\n--- a/OpenSim/Simulation/Model/PhysicalFrame.cpp\n+++ b/OpenSim/Simulation/Model/PhysicalFrame.cpp\n@@ -49,7 +49,7 @@ PhysicalFrame::PhysicalFrame() : Frame()\n     frm.setName("frame_geometry");\n     frm.set_display_radius(.004);\n     frm.setRepresentation(Geometry::Hide);\n-    append_GeometrySet(frm);\n+    append_geometry(frm);\n \n }\n \n@@ -89,8 +89,8 @@ void PhysicalFrame::extendConnectToModel(Model& aModel)\n     Super::extendConnectToModel(aModel);\n \n     // Better use name search or more robust method\n-    if (upd_GeometrySet(0).getFrameName() == "")\n-        upd_GeometrySet(0).setFrameName(getName());\n+    if (upd_geometry(0).getFrameName() == "")\n+        upd_geometry(0).setFrameName(getName());\n \n     for (int i = 0; i < get_WrapObjectSet().getSize(); i++)\n         get_WrapObjectSet().get(i).connectToModelAndBody(aModel, *this);\ndiff --git a/OpenSim/Simulation/SimbodyEngine/Body.cpp b/OpenSim/Simulation/SimbodyEngine/Body.cpp\nindex 833499725..8691b2a53 100644\n--- a/OpenSim/Simulation/SimbodyEngine/Body.cpp\n+++ b/OpenSim/Simulation/SimbodyEngine/Body.cpp\n@@ -66,7 +66,7 @@ Body::Body(const std::string &aName,double aMass,const SimTK::Vec3& aMassCenter,\n     set_mass_center(aMassCenter);\n     setInertia(aInertia);\n     // Better use name search or more robust method\n-    upd_GeometrySet(0).setFrameName(aName);\n+    upd_geometry(0).setFrameName(aName);\n }\n \n //_____________________________________________________________________________\n@@ -446,7 +446,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                         geometryIter->getValueAs(oldGeometryFiles);\n                     }\n                     std::string bodyName = aNode.getRequiredAttribute("name").getValue();\n-                    SimTK::Xml::Element geometrySetNode("GeometrySet");\n+                    SimTK::Xml::Element geometrySetNode("geometry");\n                     aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);\n                     // Create Mesh node for each item in oldGeometryFiles\n                     for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {\n@@ -468,6 +468,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                     SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");\n                     if (geomSetIter != visObjElement.element_end()){\n                         convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);\n+                        geomSetIter->setElementTag("geometry");\n                     }\n                 }\n                 // Regardless add a node for FrameGeometry to control the display of BodyFrame\n@@ -480,10 +481,13 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                 frameRepresentation.setValue("0");\n                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), frameRepresentation);\n                 bodyFrameNode.insertNodeAfter(bodyFrameNode.element_end(), appearanceNode);\n-                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("GeometrySet");\n+                //SimTK::String log;\n+                //aNode.writeToString(log);\n+                //std::cout << log << std::endl;\n+                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("geometry");\n                 if (geomSetIter != aNode.element_end()){\n                     geomSetIter->insertNodeAfter(geomSetIter->node_end(), bodyFrameNode);\n-                }\n+                 }\n             }\n         }\n     }\n@@ -499,7 +503,7 @@ void Body::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bodyNode,\n     SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");\n \n     if (objectsIter != geomSetElement.element_end()){\n-        SimTK::Xml::Element geometrySetNode("GeometrySet");\n+        SimTK::Xml::Element geometrySetNode("geometry");\n         bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);\n \n         SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");\n', 'commit_subject': 'Rename container of Geometry property to "geometry" instead of "GeometrySet"', 'repo_name': 'opensim-org/opensim-core', 'committer': {'name': 'Ayman Habib', 'email': '8ed3d36c15b09fc26f15c5d6263fa800d475ac16@stanford.edu', 'time_sec': '1430433352', 'tz_offset': '-420', 'date': {'seconds': '1430433352'}}, 'commit_tree': '7048e54479d95f372cb44a1a48a757c2c3fdc3c2', 'commit_message': 'Rename container of Geometry property to "geometry" instead of "GeometrySet"\n', 'language_name': 'Java', 'license': 'apache-2.0', 'hunks': '@@ -66,7 +66,7 @@ Body::Body(const std::string &aName,double aMass,const SimTK::Vec3& aMassCenter,\n     set_mass_center(aMassCenter);\n     setInertia(aInertia);\n     // Better use name search or more robust method\n-    upd_GeometrySet(0).setFrameName(aName);\n+    upd_geometry(0).setFrameName(aName);\n }\n \n //_____________________________________________________________________________\n@@ -446,7 +446,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                         geometryIter->getValueAs(oldGeometryFiles);\n                     }\n                     std::string bodyName = aNode.getRequiredAttribute("name").getValue();\n-                    SimTK::Xml::Element geometrySetNode("GeometrySet");\n+                    SimTK::Xml::Element geometrySetNode("geometry");\n                     aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);\n                     // Create Mesh node for each item in oldGeometryFiles\n                     for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {\n@@ -468,6 +468,7 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                     SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");\n                     if (geomSetIter != visObjElement.element_end()){\n                         convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);\n+                        geomSetIter->setElementTag("geometry");\n                     }\n                 }\n                 // Regardless add a node for FrameGeometry to control the display of BodyFrame\n@@ -480,10 +481,13 @@ void Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n                 frameRepresentation.setValue("0");\n                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), frameRepresentation);\n                 bodyFrameNode.insertNodeAfter(bodyFrameNode.element_end(), appearanceNode);\n-                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("GeometrySet");\n+                //SimTK::String log;\n+                //aNode.writeToString(log);\n+                //std::cout << log << std::endl;\n+                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("geometry");\n                 if (geomSetIter != aNode.element_end()){\n                     geomSetIter->insertNodeAfter(geomSetIter->node_end(), bodyFrameNode);\n-                }\n+                 }\n             }\n         }\n     }\n@@ -499,7 +503,7 @@ void Body::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bodyNode,\n     SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");\n \n     if (objectsIter != geomSetElement.element_end()){\n-        SimTK::Xml::Element geometrySetNode("GeometrySet");\n+        SimTK::Xml::Element geometrySetNode("geometry");\n         bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);\n \n         SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");\n', 'addition_count': 9, 'deletion_count': 5, 'src_file': 'a/OpenSim/Simulation/SimbodyEngine/Body.cpp', 'tgt_file': 'b/OpenSim/Simulation/SimbodyEngine/Body.cpp', 'file_extension': '.cpp', 'before_file': '/* -------------------------------------------------------------------------- *\n *                             OpenSim:  Body.cpp                             *\n * -------------------------------------------------------------------------- *\n * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *\n * See http://opensim.stanford.edu and the NOTICE file for more information.  *\n * OpenSim is developed at Stanford University and supported by the US        *\n * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *\n * through the Warrior Web program.                                           *\n *                                                                            *\n * Copyright (c) 2005-2017 Stanford University and the Authors                *\n * Author(s): Frank C. Anderson, Ajay Seth                                    *\n *                                                                            *\n * Licensed under the Apache License, Version 2.0 (the "License"); you may    *\n * not use this file except in compliance with the License. You may obtain a  *\n * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *\n *                                                                            *\n * Unless required by applicable law or agreed to in writing, software        *\n * distributed under the License is distributed on an "AS IS" BASIS,          *\n * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *\n * See the License for the specific language governing permissions and        *\n * limitations under the License.                                             *\n * -------------------------------------------------------------------------- */\n\n//=============================================================================\n// INCLUDES\n//=============================================================================\n#include "Body.h"\n#include <OpenSim/Common/ScaleSet.h>\n\n//=============================================================================\n// STATICS\n//=============================================================================\nusing namespace std;\n//using namespace SimTK;\nusing namespace OpenSim;\nusing SimTK::Mat33;\nusing SimTK::Vec3;\nusing SimTK::DecorativeGeometry;\n\n//=============================================================================\n// CONSTRUCTOR(S)\n//=============================================================================\n//_____________________________________________________________________________\n/**\n * Default constructor.\n */\nBody::Body() : PhysicalFrame()\n{\n    constructProperties();\n}\n\n//_____________________________________________________________________________\n/**\n * Constructor.\n */\nBody::Body(const std::string &aName,double aMass,const SimTK::Vec3& aMassCenter,const SimTK::Inertia& aInertia) :\n    PhysicalFrame()\n{\n    constructProperties();\n    setName(aName);\n    set_mass(aMass);\n    set_mass_center(aMassCenter);\n    setInertia(aInertia);\n}\n\n    set_mass_center(aMassCenter);\n    setInertia(aInertia);\n    // Better use name search or more robust method\n    upd_GeometrySet(0).setFrameName(aName);\n}\n\n//_____________________________________________________________________________\n    constructProperty_mass_center(SimTK::Vec3(0));\n    constructProperty_inertia(SimTK::Vec6(0));\n}\n\n\nvoid Body::extendFinalizeFromProperties()\n{\n    Super::extendFinalizeFromProperties();\n    const SimTK::MassProperties& massProps = getMassProperties();\n    _internalRigidBody = SimTK::Body::Rigid(massProps);\n    _slaves.clear();\n}\n\n//_____________________________________________________________________________\n/* Connect this Body to the Model it belongs to\n *\n * @param aModel OpenSim model containing this Body.\n */\nvoid Body::extendConnectToModel(Model& aModel)\n{\n    Super::extendConnectToModel(aModel);\n    \n    int nslaves = (int)_slaves.size();\n\n    if (nslaves){\n        int nbods = nslaves + 1; // include the master\n        const SimTK::MassProperties& massProps = getMassProperties();\n        SimTK::MassProperties slaveMassProps(massProps.getMass() / nbods,\n            massProps.getMassCenter(), massProps.getUnitInertia());\n\n        // update the portion taken on by the master\n        _internalRigidBody = SimTK::Body::Rigid(slaveMassProps);\n\n        // and the slaves\n        for (int i = 0; i < nslaves; ++i){\n            _slaves[i]->_internalRigidBody = SimTK::Body::Rigid(slaveMassProps);\n            _slaves[i]->setInertia(slaveMassProps.getUnitInertia());\n            _slaves[i]->setMass(slaveMassProps.getMass());\n            _slaves[i]->setMassCenter(slaveMassProps.getMassCenter());\n        }\n    }\n}\n\n//=============================================================================\n// GET AND SET\n//=============================================================================\n\n//_____________________________________________________________________________\n/**\n * Get the inertia matrix of the body.\n *\n * @return 3x3 inertia matrix.\n */\nconst SimTK::Inertia& Body::getInertia() const\n{\n    // Has not been set programmatically\n    if (_inertia.isNaN()){\n        // initialize from properties\n        const double& m = getMass();\n        const SimTK::Vec6& Ivec = get_inertia();\n        // if mass is zero, non-zero inertia makes no sense\n        if (std::abs(m) <= SimTK::SignificantReal &&\n                Ivec.norm() > SimTK::SignificantReal) {\n            // force zero inertia\n            log_warn("Body \'{}\' is massless but nonzero inertia provided.",getName());\n            log_warn(" Inertia reset to zero. Otherwise provide nonzero mass.");\n            _inertia = SimTK::Inertia(0);\n        }\n        else{\n            try {\n                _inertia = SimTK::Inertia(Ivec.getSubVec<3>(0), Ivec.getSubVec<3>(3));\n            } \n            catch (const std::exception& ex){\n                // Should throw an Exception but we have models we have released with\n                // bad inertias. E.g. early gait23 models had an error in the inertia\n                // of the toes Body. We cannot allow failures with our models so \n                // raise a warning and do something sensible with the values at hand.\n                log_warn("Body {} has invalid inertia. ", getName());\n                log_error(ex.what());\n\n                // get some aggregate value for the inertia based on existing values\n                double diag = Ivec.getSubVec<3>(0).norm()/sqrt(3.0);\n\n                // and then assume a spherical shape.\n                _inertia = SimTK::Inertia(Vec3(diag), Vec3(0));\n                \n                log_warn("{} Body\'s inertia being reset to: {}",\n                    getName(), _inertia);\n            }\n        }\n    }\n    return _inertia;\n}\n//_____________________________________________________________________________\n/**\n * Set the inertia matrix of the body.\n *\n * @param aInertia 9-element inertia matrix.\n */\nvoid Body::setInertia(const SimTK::Inertia& inertia)\n{\n    _inertia = inertia;\n    const SimTK::SymMat33& I = _inertia.asSymMat33();\n    upd_inertia()[0] = I[0][0];\n    upd_inertia()[1] = I[1][1];\n    upd_inertia()[2] = I[2][2];\n    upd_inertia()[3] = I[0][1];\n    upd_inertia()[4] = I[0][2];\n    upd_inertia()[5] = I[1][2];\n}\n\n//==============================================================================\n// SCALING\n//==============================================================================\nvoid Body::scale(const SimTK::Vec3& scaleFactors, bool scaleMass)\n{\n    Super::scaleAttachedGeometry(scaleFactors);\n    upd_mass_center() = get_mass_center().elementwiseMultiply(scaleFactors);\n    scaleInertialProperties(scaleFactors, scaleMass);\n}\n\nvoid Body::extendScale(const SimTK::State& s, const ScaleSet& scaleSet)\n{\n    Super::extendScale(s, scaleSet);\n\n    // Get scale factors (if an entry for this Body exists).\n    const Vec3& scaleFactors = getScaleFactors(scaleSet, *this);\n    if (scaleFactors == ModelComponent::InvalidScaleFactors)\n        return;\n\n    upd_mass_center() = get_mass_center().elementwiseMultiply(scaleFactors);\n}\n\nvoid Body::scaleInertialProperties(const SimTK::Vec3& scaleFactors, bool scaleMass)\n{\n    // Save the unscaled mass for possible use later.\n    double unscaledMass = get_mass();\n\n    // Calculate and store the product of the scale factors.\n    double massScaleFactor = abs(scaleFactors[0] * scaleFactors[1] * scaleFactors[2]);\n\n    // Scale the mass.\n    if (scaleMass)\n        upd_mass() *= massScaleFactor;\n    // Fix issue #2871 by fmatari\n    SimTK::Mat33 inertia = _inertia.toMat33();\n    //SimTK::SymMat33 inertia = _inertia.asSymMat33();\n\n    // If the mass is zero, then make the inertia tensor zero as well.\n    // If the X, Y, Z scale factors are equal, then you can scale the\n    // inertia tensor exactly by the square of the scale factor (and\n    // possibly by massScaleFactor), since each element in the tensor\n    // is proportional to the square of one or more dimensional\n    // measurements. For determining if the scale factors are equal,\n    // ignore reflections-- look only at the absolute value of the factors.\n    if (get_mass() <= SimTK::Eps) {\n        inertia *= 0.0;\n    }\n    else if (SimTK::isNumericallyEqual(scaleFactors[0], scaleFactors[1])\n             && SimTK::isNumericallyEqual(scaleFactors[1], scaleFactors[2])) {\n        // If the mass is also being scaled, scale the inertia terms by massScaleFactor.\n        if (scaleMass) {\n            inertia *= massScaleFactor;\n        }\n\n        // Now scale by the length-squared component.\n        inertia *= (scaleFactors[0] * scaleFactors[0]);\n\n    } else {\n        // If the scale factors are not equal, then assume that the segment\n        // is a cylinder and the inertia is calculated about one end of it.\n        int axis;\n\n        // 1. Find the smallest diagonal component. This dimension is the axis\n        //    of the cylinder.\n        if (inertia[0][0] <= inertia[1][1]){\n            if (inertia[0][0] <= inertia[2][2])\n                axis = 0;\n            else\n                axis = 2;\n\n        } else if (inertia[1][1] <= inertia[2][2]) {\n            axis = 1;\n\n        } else {\n            axis = 2;\n        }\n\n        // 2. The smallest inertia component is equal to 0.5 * mass * radius * radius,\n        //    so you can rearrange and solve for the radius.\n        int oa;\n        double radius, rad_sqr, length;\n        double term = 2.0 * inertia[axis][axis] / unscaledMass;\n        if (term < 0.0)\n            radius = 0.0;\n        else\n            radius = sqrt(term);\n\n        // 3. Choose either of the other diagonal components and use it to solve for the\n        //    length of the cylinder. This component is equal to:\n        //    0.083 * mass * length * length  +  0.25 * mass * radius * radius\n        if (axis == 0)\n            oa = 1;\n        else\n            oa = 0;\n        term = 12.0 * (inertia[oa][oa] - 0.25 * unscaledMass * radius * radius) / unscaledMass;\n        if (term < 0.0)\n            length = 0.0;\n        else\n            length = sqrt(term);\n\n        // 4. Scale the radius and length, and recalculate the diagonal inertia terms.\n        length *= scaleFactors[axis];\n\n        if (axis == 0) {\n            rad_sqr = radius * (scaleFactors[1]) * radius * (scaleFactors[2]);\n            inertia[0][0] = 0.5 * get_mass() * rad_sqr;\n            inertia[1][1] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n            inertia[2][2] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n\n        } else if (axis == 1) {\n            rad_sqr = radius * (scaleFactors[0]) * radius * (scaleFactors[2]);\n            inertia[0][0] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n            inertia[1][1] = 0.5 * get_mass() * rad_sqr;\n            inertia[2][2] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n\n        } else {\n            rad_sqr = radius * (scaleFactors[0]) * radius * (scaleFactors[1]);\n            inertia[0][0] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n            inertia[1][1] = get_mass() * ((length * length / 12.0) + 0.25 * rad_sqr);\n            inertia[2][2] = 0.5 * get_mass() * rad_sqr;\n        }\n\n        // 5. Scale the inertia products, in case some are non-zero. These are scaled by\n        //    two scale factors for the length term (which two depend on the inertia term\n        //    being scaled), and, if the mass is also scaled, by massScaleFactor.\n        inertia[0][1] *= ((scaleFactors[0] * scaleFactors[1]));\n        inertia[0][2] *= ((scaleFactors[0] * scaleFactors[2]));\n        inertia[1][0] *= ((scaleFactors[1] * scaleFactors[0]));\n        inertia[1][2] *= ((scaleFactors[1] * scaleFactors[2]));\n        inertia[2][0] *= ((scaleFactors[2] * scaleFactors[0]));\n        inertia[2][1] *= ((scaleFactors[2] * scaleFactors[1]));\n\n        if (scaleMass) {\n            inertia[0][1] *= massScaleFactor;\n            inertia[0][2] *= massScaleFactor;\n            inertia[1][0] *= massScaleFactor;\n            inertia[1][2] *= massScaleFactor;\n            inertia[2][0] *= massScaleFactor;\n            inertia[2][1] *= massScaleFactor;\n        }\n    }\n\n    setInertia(SimTK::Inertia(inertia));\n}\n\nvoid Body::scaleInertialProperties(const ScaleSet& scaleSet, bool scaleMass)\n{\n    // Get scale factors (if an entry for this Body exists).\n    const Vec3& scaleFactors = getScaleFactors(scaleSet, *this);\n    if (scaleFactors == ModelComponent::InvalidScaleFactors)\n        return;\n\n    scaleInertialProperties(scaleFactors, scaleMass);\n}\n\n//_____________________________________________________________________________\n/**\n * Scale the body\'s mass and inertia tensor (represents a scaling of the\n * body\'s density).\n *\n * @param aScaleFactors XYZ scale factors.\n */\nvoid Body::scaleMass(double aScaleFactor)\n{\n    upd_mass() *= aScaleFactor;\n    _inertia *= aScaleFactor;\n    upd_inertia() *= aScaleFactor;\n}\n\n//=============================================================================\n// UTILITY\n//=============================================================================\nSimTK::MassProperties Body::getMassProperties() const\n{\n    const double& m = get_mass();\n    const Vec3& com = get_mass_center();\n    try {\n        OPENSIM_THROW_IF_FRMOBJ(!(m >= 0.0), Exception,\n                "Body " + getName() + " has unspecified or negative mass.");\n\n        const SimTK::Inertia& Icom = getInertia();\n\n        SimTK::Inertia Ib = Icom;\n        // If com and body, b, frame are coincident then don\'t bother shifting\n        if (com.norm() > SimTK::Eps) {\n            // shift if com has nonzero distance from b\n            Ib = Icom.shiftFromMassCenter(com, m);\n        }\n    \n        return SimTK::MassProperties(m, com, Ib);\n    }\n    catch (const std::exception& ex) {\n        string msg = "Body " + getName() + " has invalid mass properties. ";\n        msg += ex.what();\n        throw Exception(msg, __FILE__, __LINE__);\n    }\n}\n\n//=============================================================================\n// I/O\n//=============================================================================\n\nvoid Body::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)\n{\n    if (versionNumber < XMLDocument::getLatestVersion()) {\n        if (versionNumber < 30500) {\n            SimTK::Vec6 newInertia(1.0, 1.0, 1.0, 0., 0., 0.);\n            std::string inertiaComponents[] = { "inertia_xx", "inertia_yy", "inertia_zz", "inertia_xy", "inertia_xz", "inertia_yz" };\n            for (int i = 0; i < 6; ++i) {\n                SimTK::Xml::element_iterator iIter = aNode.element_begin(inertiaComponents[i]);\n                if (iIter != aNode.element_end()) {\n                    newInertia[i] = iIter->getValueAs<double>();\n                    aNode.removeNode(iIter);\n                }\n            }\n            std::ostringstream strs;\n            for (int i = 0; i < 6; ++i) {\n                strs << newInertia[i];\n                if (i < 5) strs << " ";\n            }\n            std::string strInertia = strs.str();\n            SimTK::Xml::Element inertiaNode("inertia", strInertia);\n            aNode.insertNodeAfter(aNode.element_end(), inertiaNode);\n        }\n    }\n    Super::updateFromXMLNode(aNode, versionNumber);\n}\n\nBody* Body::addSlave()\n{\n    Body* slave = new Body();\n    int count = (int)_slaves.size();\n\n    stringstream name;\n    name << getName() << "_slave_" << count;\n    slave->setName(name.str());\n\n    //add to internal list of references \n    _slaves.push_back(SimTK::ReferencePtr<Body>(slave));\n\n    //add to list of subcomponents to automatically add to system and initialize\n    adoptSubcomponent(slave);\n\n    return slave;\n}\n                        geometryIter->getValueAs(oldGeometryFiles);\n                    }\n                    std::string bodyName = aNode.getRequiredAttribute("name").getValue();\n                    SimTK::Xml::Element geometrySetNode("GeometrySet");\n                    aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);\n                    // Create Mesh node for each item in oldGeometryFiles\n                    for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {\n                    SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");\n                    if (geomSetIter != visObjElement.element_end()){\n                        convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);\n                    }\n                }\n                // Regardless add a node for FrameGeometry to control the display of BodyFrame\n                frameRepresentation.setValue("0");\n                appearanceNode.insertNodeAfter(appearanceNode.element_end(), frameRepresentation);\n                bodyFrameNode.insertNodeAfter(bodyFrameNode.element_end(), appearanceNode);\n                SimTK::Xml::element_iterator geomSetIter = aNode.element_begin("GeometrySet");\n                if (geomSetIter != aNode.element_end()){\n                    geomSetIter->insertNodeAfter(geomSetIter->node_end(), bodyFrameNode);\n                }\n            }\n        }\n    }\n    SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");\n\n    if (objectsIter != geomSetElement.element_end()){\n        SimTK::Xml::Element geometrySetNode("GeometrySet");\n        bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);\n\n        SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");\n', 'message': 'Rename container of Geometry property to "geometry" instead of "GeometrySet"\n', 'author_name': 'Ayman Habib', 'lang': 'cpp'}