void
LogComponentEnableAll (enum LogLevel level)
{
ComponentList *components = GetComponentList ();
for (ComponentListI i = components->begin ();
i != components->end ();
i++)
{
i->second->Enable (level);
}
}
void
LogComponentEnable (char const *name, enum LogLevel level)
{
ComponentList *components = GetComponentList ();
ComponentListI i;
for (i = components->begin ();
i != components->end ();
i++)
{
if (i->first.compare (name) == 0)
{
i->second->Enable (level);
return;
}
}
if (i == components->end())
{
// nothing matched
LogComponentPrintList();
NS_FATAL_ERROR ("Logging component \"" << name <<
"\" not found. See above for a list of available log components");
}
}
Component::~Component()
{
reparent(0);
ComponentList childListCopy = childList; // need to work on a copy of the set due to iterator problems when deleting the children which attempt to remove themselves from the set..
// clear these two to make the below delete call (which ultimately calls
// reparent and childAdded()) peppier..
childList.clear();
childNameMap.clear();
for (ComponentList::iterator it = childListCopy.begin(); it != childListCopy.end(); ++it)
delete (*it);
idCompMap.erase(id());
NameMap::iterator it = nameCompMap.find(nam);
if (it != nameCompMap.end() && it->second == this) nameCompMap.erase(it);
}
void
LogComponentDisable (char const *name, enum LogLevel level)
{
ComponentList *components = GetComponentList ();
for (ComponentListI i = components->begin ();
i != components->end ();
i++)
{
if (i->first.compare (name) == 0)
{
i->second->Disable (level);
break;
}
}
}
LogComponent::LogComponent (char const * name)
: m_levels (0), m_name (name)
{
//EnvVarCheck (name);
ComponentList *components = GetComponentList ();
for (ComponentListI i = components->begin ();
i != components->end ();
i++)
{
if (i->first == name)
{
std::cerr << "Log component \""<<name<<"\" has already been registered once." << std::endl;
}
}
components->push_back (std::make_pair (name, this));
}
void PathWrap::extendConnectToModel(Model& model)
{
Super::extendConnectToModel(model);
_path = dynamic_cast<const GeometryPath*>(&getParent());
std::string msg = "PathWrap '" + getName()
+ "' must have a GeometryPath as its parent.";
OPENSIM_THROW_IF(_path == nullptr, Exception, msg);
ComponentList<const PhysicalFrame> bodiesList =
model.getComponentList<PhysicalFrame>();
for (ComponentList<PhysicalFrame>::const_iterator it = bodiesList.begin();
it != bodiesList.end(); ++it) {
const WrapObject* wo = it->getWrapObject(getWrapObjectName());
if (wo) {
_wrapObject = wo;
updWrapPoint1().setBody(wo->getFrame());
updWrapPoint1().setWrapObject(wo);
updWrapPoint2().setBody(wo->getFrame());
updWrapPoint2().setWrapObject(wo);
break;
}
}
if (get_method() == "hybrid" || get_method() == "Hybrid" || get_method() == "HYBRID")
_method = hybrid;
else if (get_method() == "midpoint" || get_method() == "Midpoint" || get_method() == "MIDPOINT")
_method = midpoint;
else if (get_method() == "axial" || get_method() == "Axial" || get_method() == "AXIAL")
_method = axial;
else if (get_method() == "Unassigned") { // method was not specified in wrap object definition; use default
_method = hybrid;
upd_method() = "hybrid";
} else { // method was specified incorrectly in wrap object definition; throw an exception
string errorMessage = "Error: wrapping method for wrap object " + getName() + " was either not specified, or specified incorrectly.";
throw Exception(errorMessage);
}
}
//---------------------------------------------------------------------------
void Avatar::fireWeaponGroup(int /*groupId*/)
{
// for now, fire everything
ComponentList list;
findComponents(EquipmentSlotComponent::getClassDef(), list);
for (ComponentList::iterator it = list.begin(); it != list.end(); ++it)
{
EquipmentSlotComponent* pSlot = static_cast<EquipmentSlotComponent*>(*it);
if (pSlot)
{
size_t numEquipment = pSlot->getNumEquipment();
for (size_t i=0; i<numEquipment; ++i)
{
Mountable* pMountable = pSlot->getEquipment(i);
if (pMountable && pMountable->isOfType(Weapon::getClassDef()))
{
Weapon* pWeapon = static_cast<Weapon*>(pMountable);
pWeapon->fire();
}
}
}
}
}
void PathWrap::connectToModelAndPath(Model& aModel, GeometryPath& aPath)
{
_path = &aPath;
ComponentList<PhysicalFrame> bodiesList = aModel.getComponentList<PhysicalFrame>();
for (ComponentList<PhysicalFrame>::const_iterator it = bodiesList.begin();
it != bodiesList.end(); ++it) {
const WrapObject* wo = it->getWrapObject(getWrapObjectName());
if (wo) {
_wrapObject = wo;
updWrapPoint1().setBody(wo->getBody());
updWrapPoint1().setWrapObject(wo);
updWrapPoint2().setBody(wo->getBody());
updWrapPoint2().setWrapObject(wo);
break;
}
}
// connectToModelAndPath() must be called after setBody() because it requires
// that _bodyName already be assigned.
updWrapPoint1().connectToModelAndPath(aModel, aPath);
updWrapPoint2().connectToModelAndPath(aModel, aPath);
if (get_method() == "hybrid" || get_method() == "Hybrid" || get_method() == "HYBRID")
_method = hybrid;
else if (get_method() == "midpoint" || get_method() == "Midpoint" || get_method() == "MIDPOINT")
_method = midpoint;
else if (get_method() == "axial" || get_method() == "Axial" || get_method() == "AXIAL")
_method = axial;
else if (get_method() == "Unassigned") { // method was not specified in wrap object definition; use default
_method = hybrid;
upd_method() = "hybrid";
} else { // method was specified incorrectly in wrap object definition; throw an exception
string errorMessage = "Error: wrapping method for wrap object " + getName() + " was either not specified, or specified incorrectly.";
throw Exception(errorMessage);
}
}
//---------------------------------------------------------------------------
bool Avatar::update(float deltaT)
{
if (_update(deltaT))
{
// we can't extract mount points from the derived avatar type
// until it's fully initialized, so we need to do this here in
// update() as opposed to in initialize()
// check components for an AnimationComponent
AnimationComponent* pAnim = 0;
if (findComponents(AnimationComponent::getClassDef(), (Component**)&pAnim))
{
// and then, if we have a valid animation blender...
AnimationBlender* pBlender = pAnim->getBlender();
if (pBlender)
{
// and then, if it has mounts but our mount data is empty...
size_t numMP = pBlender->getNumMountPoints();
if (numMP)
{
if (m_mountPoints.size() == 0)
{
// then we can fill in our data structure
for (size_t i=0; i<numMP; ++i)
{
const AnimationBlender::MountPointData* pData =
pBlender->getMountPoint(i);
MountPoint mp;
mp.m_index = i;
mp.m_boneIndex = pData->m_boneIndex;
mp.m_transform = pData->m_transform;
mp.m_pSlot = 0;
m_mountPoints[pData->m_name] = mp;
}
// now get a list of all of the EquipmentSlotComponents
// configured for us; we need to associate each component
// with a mount point
ComponentList list;
findComponents(EquipmentSlotComponent::getClassDef(), list);
for (ComponentList::iterator it=list.begin(); it != list.end(); ++it)
{
EquipmentSlotComponent* pSlot = static_cast<EquipmentSlotComponent*>(*it);
MountPoints::iterator mp = m_mountPoints.find(pSlot->getMountPoint());
if (mp != m_mountPoints.end())
mp->second.m_pSlot = pSlot;
else
{
// log a mismatch
String msg("Could not find mount point '");
msg += pSlot->getMountPoint();
msg += String("' in avatar: ");
msg += getName();
Environment::get().pLogger->logMessage(msg);
}
}
// and finally, clear up our mount backlog, if any
for (size_t i=0; i<m_delayedMounts.size(); ++i)
{
addToSlot(m_delayedMounts[i].pMountable, m_delayedMounts[i].slotId);
}
m_delayedMounts.clear();
}
else
{
// we can update our mount point transforms
for (size_t i=0; i<numMP; ++i)
{
const AnimationBlender::MountPointData* pData =
pBlender->getMountPoint(i);
MountPoints::iterator it = m_mountPoints.find(pData->m_name);
if (it != m_mountPoints.end())
{
it->second.m_transform = pData->m_transform;
it->second.m_pSlot->setTransform(pData->m_transform);
}
}
}
}
}
}
// update our mounted objects
for (MountPoints::iterator it = m_mountPoints.begin();
it != m_mountPoints.end(); ++it)
{
EquipmentSlotComponent* pComp = it->second.m_pSlot;
if (pComp)
{
size_t nEquip = pComp->getNumEquipment();
for (size_t j=0; j<nEquip; ++j)
{
Mountable* pMounted = pComp->getEquipment(j);
if (pMounted)
pMounted->update(deltaT);
}
}
}
// Avatars might have camera components too
CameraInterfaceComponent* pCamComp;
if (findComponents(
CameraInterfaceComponent::getClassDef(), (Component**)&pCamComp))
{
pCamComp->update(deltaT);
}
return ZoneObject::update(deltaT);
}
return false;
}
void scaleModelWithLigament()
{
// SET OUTPUT FORMATTING
IO::SetDigitsPad(4);
std::string setupFilePath("");
ScaleTool* scaleTool;
Model* model;
// Remove old model if any
FILE* file2Remove = IO::OpenFile(setupFilePath + "toyLigamentModelScaled.osim", "w");
fclose(file2Remove);
// Construct model and read parameters file
scaleTool = new ScaleTool("toyLigamentModel_Setup_Scale.xml");
// Keep track of the folder containing setup file, will be used to locate results to compare against
setupFilePath = scaleTool->getPathToSubject();
model = scaleTool->createModel();
if (!model) {
throw Exception("scale: ERROR- No model specified.",__FILE__,__LINE__);
//cout << "scale: ERROR- No model specified.";
}
SimTK::State& s = model->updWorkingState();
model->getMultibodySystem().realize(s, SimTK::Stage::Position);
ASSERT(!scaleTool->isDefaultModelScaler() && scaleTool->getModelScaler().getApply());
ModelScaler& scaler = scaleTool->getModelScaler();
ASSERT(scaler.processModel(model, setupFilePath, scaleTool->getSubjectMass()));
const std::string& scaledModelFile = scaler.getOutputModelFileName();
const std::string& std_scaledModelFile = "std_toyLigamentModelScaled.osim";
Model comp(scaledModelFile);
Model std(std_scaledModelFile);
std.print("std_toyLigamentModelScaled_latest.osim");
comp.print("comp_toyLigamentModelScaled_latest.osim");
// the latest model will not match the standard because the naming convention has
// been updated to store path names and connecting a model results in connectors
// storing relative paths so that collections of components are more portable.
// The models must be equivalent after being connected.
comp.setup();
std.setup();
ComponentList<Ligament> compLigs = comp.getComponentList<Ligament>();
ComponentList<Ligament> stdLigs = std.getComponentList<Ligament>();
ComponentList<Ligament>::const_iterator itc = compLigs.begin();
ComponentList<Ligament>::const_iterator its = stdLigs.begin();
for (; its != stdLigs.end() && itc != compLigs.end(); ++its, ++itc){
cout << "std:" << its->getName() << "==";
cout << "comp:" << itc->getName() << " : ";
cout << (*its == *itc) << endl;
ASSERT(*its == *itc, __FILE__, __LINE__,
"Scaled ligament " + its->getName() + " did not match standard.");
}
//Finally make sure we didn't incorrectly scale anything else in the model
ASSERT(std == comp);
delete model;
delete scaleTool;
}
/**
* This method scales a model based on user-specified XYZ body scale factors
* and/or a set of marker-to-marker distance measurements.
*
* @param aModel the model to scale.
* @param aSubjectMass the final mass of the model after scaling.
* @return Whether the scaling process was successful or not.
*/
bool ModelScaler::processModel(Model* aModel, const string& aPathToSubject, double aSubjectMass)
{
if (!getApply()) return false;
int i;
ScaleSet theScaleSet;
Vec3 unity(1.0);
cout << endl << "Step 2: Scaling generic model" << endl;
ComponentList<PhysicalFrame> segments
= aModel->getComponentList<PhysicalFrame>();
ComponentList<PhysicalFrame>::const_iterator it = segments.begin();
/* Make a scale set with a Scale for each physical frame.
* Initialize all factors to 1.0.
*/
for (; it != segments.end(); ++it) {
Scale* segmentScale = new Scale();
segmentScale->setSegmentName(it->getName());
segmentScale->setScaleFactors(unity);
segmentScale->setApply(true);
theScaleSet.adoptAndAppend(segmentScale);
}
SimTK::State& s = aModel->initSystem();
aModel->getMultibodySystem().realize(s, SimTK::Stage::Position);
try
{
/* Make adjustments to theScaleSet, in the user-specified order. */
for (i = 0; i < _scalingOrder.getSize(); i++)
{
/* For measurements, measure the distance between a pair of markers
* in the model, and in the static pose. The latter divided by the
* former is the scale factor. Put that scale factor in theScaleSet,
* using the body/axis names specified in the measurement to
* determine in what place[s] to put the factor.
*/
if (_scalingOrder[i] == "measurements")
{
/* Load the static pose marker file, and convert units.
*/
MarkerData *markerData = 0;
if(!_markerFileName.empty() && _markerFileName!=PropertyStr::getDefaultStr()) {
markerData = new MarkerData(aPathToSubject + _markerFileName);
markerData->convertToUnits(aModel->getLengthUnits());
}
/* Now take and apply the measurements. */
for (int j = 0; j < _measurementSet.getSize(); j++)
{
if (_measurementSet.get(j).getApply())
{
if(!markerData)
throw Exception("ModelScaler.processModel: ERROR- "+_markerFileNameProp.getName()+
" not set but measurements are used",__FILE__,__LINE__);
double scaleFactor = computeMeasurementScaleFactor(s,*aModel, *markerData, _measurementSet.get(j));
if (!SimTK::isNaN(scaleFactor))
_measurementSet.get(j).applyScaleFactor(scaleFactor, theScaleSet);
else
cout << "___WARNING___: " << _measurementSet.get(j).getName() << " measurement not used to scale " << aModel->getName() << endl;
}
}
}
/* For manual scales, just copy the XYZ scale factors from
* the manual scale into theScaleSet.
*/
else if (_scalingOrder[i] == "manualScale")
{
for (int j = 0; j < _scaleSet.getSize(); j++)
{
if (_scaleSet[j].getApply())
{
const string& bodyName = _scaleSet[j].getSegmentName();
Vec3 factors(1.0);
_scaleSet[j].getScaleFactors(factors);
for (int k = 0; k < theScaleSet.getSize(); k++)
{
if (theScaleSet[k].getSegmentName() == bodyName)
theScaleSet[k].setScaleFactors(factors);
}
}
}
}
else
{
throw Exception("ModelScaler: ERR- Unrecognized string '"+_scalingOrder[i]+"' in "+_scalingOrderProp.getName()+" property (expecting 'measurements' or 'manualScale').",__FILE__,__LINE__);
}
}
/* Now scale the model. */
aModel->scale(s, theScaleSet, aSubjectMass, _preserveMassDist);
if(_printResultFiles) {
std::string savedCwd = IO::getCwd();
IO::chDir(aPathToSubject);
if (!_outputModelFileNameProp.getValueIsDefault())
{
if (aModel->print(_outputModelFileName))
cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl;
}
if (!_outputScaleFileNameProp.getValueIsDefault())
{
if (theScaleSet.print(_outputScaleFileName))
cout << "Wrote scale file " << _outputScaleFileName << " for model " << aModel->getName() << endl;
}
IO::chDir(savedCwd);
}
}
catch (const Exception& x)
{
x.print(cout);
return false;
}
return true;
}
int main()
{
try {
LoadOpenSimLibrary("osimActuators");
std::string filename = "arm26.osim";
Model model(filename);
model.dumpSubcomponents();
ComponentList<Component> componentsList = model.getComponentList();
std::cout << "list begin: " << componentsList.begin()->getName() << std::endl;
int numComponents = 0;
for (ComponentList<Component>::const_iterator it = componentsList.begin();
it != componentsList.end();
++it) {
std::cout << "Iterator is at: " << it->getFullPathName() << " <" << it->getConcreteClassName() << ">" << std::endl;
numComponents++;
}
ComponentList<OpenSim::Body> bodiesList = model.getComponentList<OpenSim::Body>();
int numBodies = 0;
std::cout << "Bodies list begin: " << bodiesList.begin()->getName() << std::endl;
for (ComponentList<OpenSim::Body>::const_iterator it = bodiesList.begin();
it != bodiesList.end();
++it) {
std::cout << "Iterator is at Body: " << it->getName() << std::endl;
numBodies++;
}
// Now we try the post increment variant of the iterator
std::cout << "Bodies list begin, using post increment: " << bodiesList.begin()->getName() << std::endl;
int numBodiesPost = 0;
for (ComponentList<OpenSim::Body>::const_iterator itPost = bodiesList.begin();
itPost != bodiesList.end();
itPost++) {
std::cout << "Iterator is at Body: " << itPost->getName() << std::endl;
numBodiesPost++;
}
int numMuscles = 0;
std::cout << "Using range-for loop over Muscles: " << std::endl;
ComponentList<Muscle> musclesList = model.getComponentList<Muscle>();
for (const Muscle& muscle : musclesList) {
std::cout << "Iterator is at muscle: " << muscle.getName() << std::endl;
numMuscles++;
}
int numGeomPaths = 0;
ComponentList<GeometryPath> geomPathList = model.getComponentList<GeometryPath>();
for (const GeometryPath& gpath : geomPathList) {
(void)gpath; // Suppress unused variable warning.
numGeomPaths++;
}
const OpenSim::Joint& shoulderJnt = model.getJointSet().get(0);
// cycle through components under shoulderJnt should return the Joint itself and the Coordinate
int numJntComponents = 0;
ComponentList<Component> jComponentsList = shoulderJnt.getComponentList();
std::cout << "Components/subComponents under Shoulder Joint:" << std::endl;
for (ComponentList<Component>::const_iterator it = jComponentsList.begin();
it != jComponentsList.end();
++it) {
std::cout << "Iterator is at: " << it->getConcreteClassName() << " " << it->getFullPathName() << std::endl;
numJntComponents++;
}
cout << "Num all components = " << numComponents << std::endl;
cout << "Num bodies = " << numBodies << std::endl;
cout << "Num Muscles = " << numMuscles << std::endl;
cout << "Num GeometryPath components = " << numGeomPaths << std::endl;
// Components = Model+3Body+3Marker+2(Joint+Coordinate)+6(Muscle+GeometryPath)
// Should test against 1+#Bodies+#Markers+#Joints+#Constraints+#Coordinates+#Forces+#ForcesWithPath+..
// Would that account for internal (split-bodies etc.?)
// To test states we must have added the components to the system
// which is done when the model creates and initializes the system
SimTK::State state = model.initSystem();
int numJointsWithStateVariables = 0;
ComponentList<Joint> jointsWithStates = model.getComponentList<Joint>();
ComponentWithStateVariables myFilter;
jointsWithStates.setFilter(myFilter);
for (const Joint& comp : jointsWithStates) {
cout << comp.getConcreteClassName() << ":" << comp.getFullPathName() << endl;
numJointsWithStateVariables++;
}
int numModelComponentsWithStateVariables = 0;
ComponentList<ModelComponent> comps = model.getComponentList<ModelComponent>();
comps.setFilter(myFilter);
for (const ModelComponent& comp : comps) {
cout << comp.getConcreteClassName() << ":" << comp.getFullPathName() << endl;
numModelComponentsWithStateVariables++;
}
//Now test a std::iterator method
ComponentList<Frame> allFrames = model.getComponentList<Frame>();
ComponentList<Frame>::const_iterator skipIter = allFrames.begin();
int countSkipFrames = 0;
while (skipIter != allFrames.end()){
cout << skipIter->getConcreteClassName() << ":" << skipIter->getFullPathName() << endl;
std::advance(skipIter, 2);
countSkipFrames++;
}
// TODO: Hard-coding these numbers is not ideal. As we introduce more components
// to recompose existing components, this will need continual updating. For example,
// Joint's often add PhysicalOffsetFrames to handle what used to be baked in location
// and orientation offsets.
ASSERT(numComponents == 130);
ASSERT(numBodies == model.getNumBodies());
ASSERT(numBodiesPost == numBodies);
ASSERT(numMuscles == model.getMuscles().getSize());
ASSERT(numJointsWithStateVariables == 2);
ASSERT(numModelComponentsWithStateVariables == 10);
// Below updated from 1 to 3 to account for offset frame and its geometry added to the Joint
ASSERT(numJntComponents == 3);
// Test using the iterator to skip over every other Component (Frame in this case)
// nf = 1 ground + 2 bodies + 2 joint offsets = 5, skipping - 2 = 3
ASSERT(countSkipFrames == 3);
}
catch (Exception &ex) {
ex.print(std::cout);
return 1;
}
cout << "Done" << endl;
return 0;
}
