1 /* --------------------------------------------------------------------------*
2 *                         OpenSim:  PhysicalFrame.cpp                        *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
5 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
6 * OpenSim is developed at Stanford University and supported by the US        *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
8 * through the Warrior Web program.                                           *
9 *                                                                            *
10 * Copyright (c) 2005-2017 Stanford University and the Authors                *
11 * Author(s): Matt DeMers, Ayman Habib, Ajay Seth                             *
12 *                                                                            *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
14 * not use this file except in compliance with the License. You may obtain a  *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
16 *                                                                            *
17 * Unless required by applicable law or agreed to in writing, software        *
18 * distributed under the License is distributed on an "AS IS" BASIS,          *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
20 * See the License for the specific language governing permissions and        *
21 * limitations under the License.                                             *
22 * -------------------------------------------------------------------------- */
23 
24 //=============================================================================
25 // INCLUDES
26 //=============================================================================
27 #include "PhysicalFrame.h"
28 #include "Model.h"
29 
30 
31 //=============================================================================
32 // STATICS
33 //=============================================================================
34 using namespace std;
35 using namespace OpenSim;
36 
37 //=============================================================================
38 // CONSTRUCTOR(S)
39 //=============================================================================
40 //_____________________________________________________________________________
41 /**
42 * Default constructor.
43 */
PhysicalFrame()44 PhysicalFrame::PhysicalFrame() : Frame()
45 {
46     setAuthors("Matt DeMers, Ayman Habib, Ajay Seth");
47     constructProperties();
48 }
49 
constructProperties()50 void PhysicalFrame::constructProperties()
51 {
52     constructProperty_WrapObjectSet(WrapObjectSet());
53 }
54 
extendFinalizeFromProperties()55 void PhysicalFrame::extendFinalizeFromProperties()
56 {
57     Super::extendFinalizeFromProperties();
58 }
59 
getMobilizedBody() const60 const SimTK::MobilizedBody& PhysicalFrame::getMobilizedBody() const
61 {
62     return getModel().getMatterSubsystem().getMobilizedBody(_mbIndex);
63 }
64 
updMobilizedBody()65 SimTK::MobilizedBody& PhysicalFrame::updMobilizedBody()
66 {
67     return updModel().updMatterSubsystem().updMobilizedBody(_mbIndex);
68 }
69 
setMobilizedBodyIndex(const SimTK::MobilizedBodyIndex & mbix) const70 void PhysicalFrame::setMobilizedBodyIndex(const SimTK::MobilizedBodyIndex& mbix) const
71 {
72     OPENSIM_THROW_IF_FRMOBJ(!mbix.isValid(), Exception,
73         "Attempting to assign an invalid SimTK::MobilizedBodyIndex");
74     const_cast<Self*>(this)->_mbIndex = mbix;
75 }
76 
77 /*
78 * Implementation of Frame interface by PhysicalFrame.
79 *
80 */
81 SimTK::Transform PhysicalFrame::
calcTransformInGround(const SimTK::State & s) const82     calcTransformInGround(const SimTK::State& s) const
83 {
84     // return X_GF = X_GB * X_BF;
85     return getMobilizedBody().getBodyTransform(s);
86 }
87 
88 SimTK::SpatialVec PhysicalFrame::
calcVelocityInGround(const SimTK::State & state) const89 calcVelocityInGround(const SimTK::State& state) const
90 {
91     return getMobilizedBody().getBodyVelocity(state);
92 }
93 
94 SimTK::SpatialVec PhysicalFrame::
calcAccelerationInGround(const SimTK::State & state) const95 calcAccelerationInGround(const SimTK::State& state) const
96 {
97     return getMobilizedBody().getBodyAcceleration(state);
98 }
99 
100 
extendFindTransformInBaseFrame() const101 SimTK::Transform PhysicalFrame::extendFindTransformInBaseFrame() const
102 {
103     return SimTK::Transform();
104 }
105 
extendConnectToModel(Model & aModel)106 void PhysicalFrame::extendConnectToModel(Model& aModel)
107 {
108     Super::extendConnectToModel(aModel);
109 
110     for (int i = 0; i < get_WrapObjectSet().getSize(); i++)
111         get_WrapObjectSet()[i].setFrame(*this);
112 }
113 
getWrapObject(const string & aName) const114 const WrapObject* PhysicalFrame::getWrapObject(const string& aName) const
115 {
116     int i;
117 
118     for (i = 0; i < get_WrapObjectSet().getSize(); i++) {
119         if (aName == get_WrapObjectSet()[i].getName())
120             return &get_WrapObjectSet()[i];
121     }
122     return nullptr;
123 }
124 
addWrapObject(WrapObject * wrap)125 void PhysicalFrame::addWrapObject(WrapObject* wrap) {
126     upd_WrapObjectSet().adoptAndAppend(wrap);
127 }
128 
129 //=============================================================================
130 // XML Deserialization
131 //=============================================================================
132 
updateFromXMLNode(SimTK::Xml::Element & aNode,int versionNumber)133 void PhysicalFrame::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber)
134 {
135     if (versionNumber < XMLDocument::getLatestVersion()) {
136         if (versionNumber < 30502) {
137             // Find node for <VisibleObject> remove it then create Geometry for the
138             SimTK::Xml::element_iterator iIter = aNode.element_begin("VisibleObject");
139             if (iIter != aNode.element_end()) {
140                 SimTK::Xml::Element visObjElement = SimTK::Xml::Element::getAs(aNode.removeNode(iIter));
141                 // Scale factors
142                 SimTK::Vec3 outerScaleFactors(1.0);
143                 SimTK::Xml::element_iterator outerScaleFactortIter = visObjElement.element_begin("scale_factors");
144                 if (outerScaleFactortIter != visObjElement.element_end()) {
145                     outerScaleFactors = outerScaleFactortIter->getValueAs<SimTK::Vec3>();
146                 }
147                 SimTK::Vec6 outerTransform(0.0);
148                 SimTK::Xml::element_iterator outerTransformIter = visObjElement.element_begin("transform");
149                 if (outerTransformIter != visObjElement.element_end()) {
150                     outerTransform = outerTransformIter->getValueAs<SimTK::Vec6>();
151                 }
152 
153                 if (versionNumber < 20101) {
154                     SimTK::Xml::element_iterator geometryIter = visObjElement.element_begin("geometry_files");
155                     SimTK::Array_<SimTK::String> oldGeometryFiles;
156                     if (geometryIter != aNode.element_end()) {
157                         geometryIter->getValueAs(oldGeometryFiles);
158                     }
159                     std::string bodyName = aNode.getRequiredAttribute("name").getValue();
160                     SimTK::Xml::Element geometrySetNode("attached_geometry");
161                     aNode.insertNodeAfter(aNode.element_end(), geometrySetNode);
162                     // Create Mesh node for each item in oldGeometryFiles
163                     for (unsigned ng = 0; ng < oldGeometryFiles.size(); ng++) {
164                         SimTK::Xml::Element meshNode("Mesh");
165                         std::string geomName = bodyName + "_geom_" + to_string(ng);
166                         meshNode.setAttributeValue("name", geomName);
167                         SimTK::Xml::Element meshFileNode("mesh_file", oldGeometryFiles[ng]);
168                         std::stringstream localScaleStr;
169                         localScaleStr << outerScaleFactors[0] << " " << outerScaleFactors[1]
170                             << " " << outerScaleFactors[2];
171                         SimTK::Xml::Element scaleFactorsNode("scale_factors", localScaleStr.str());
172                         meshNode.insertNodeAfter(meshNode.element_end(), scaleFactorsNode);
173                         meshNode.insertNodeAfter(meshNode.element_end(), meshFileNode);
174                         // The transform is relative to the owning component
175                         // (this PhysicalFrame), so the connectee name just
176                         // points up to this component ("..").
177                         XMLDocument::addConnector(meshNode, "Connector_Frame_",
178                                 "frame", "..");
179                         geometrySetNode.insertNodeAfter(geometrySetNode.element_end(), meshNode);
180                     }
181                 }
182                 else {
183                     SimTK::Xml::element_iterator geomSetIter = visObjElement.element_begin("GeometrySet");
184                     if (geomSetIter != visObjElement.element_end()) {
185                         convertDisplayGeometryToGeometryXML(aNode, outerScaleFactors, outerTransform, *geomSetIter);
186                         geomSetIter->setElementTag("geometry");
187                     }
188                 }
189             }
190         }
191     }
192     Super::updateFromXMLNode(aNode, versionNumber);
193 }
194 
convertDisplayGeometryToGeometryXML(SimTK::Xml::Element & bodyNode,const SimTK::Vec3 & outerScaleFactors,const SimTK::Vec6 & outerTransformVec6,SimTK::Xml::Element & geomSetElement)195 void PhysicalFrame::convertDisplayGeometryToGeometryXML(SimTK::Xml::Element& bodyNode,
196     const SimTK::Vec3& outerScaleFactors, const SimTK::Vec6& outerTransformVec6,
197     SimTK::Xml::Element& geomSetElement)
198 {
199     std::string bodyName = bodyNode.getRequiredAttribute("name").getValue();
200 
201     SimTK::Xml::element_iterator objectsIter = geomSetElement.element_begin("objects");
202 
203     if (objectsIter != geomSetElement.element_end()) {
204         SimTK::Xml::Element geometrySetNode("attached_geometry");
205         bodyNode.insertNodeAfter(bodyNode.element_end(), geometrySetNode);
206 
207         SimTK::Xml::element_iterator displayGeomIter = objectsIter->element_begin("DisplayGeometry");
208         int counter = 1;
209         while (displayGeomIter != objectsIter->element_end()) {
210             // Create a <Mesh> Element and populate it
211             bool attachToThisFrame = true;
212             SimTK::Xml::Element meshNode("Mesh");
213             std::string geomName = bodyName + "_geom_" + to_string(counter);
214             meshNode.setAttributeValue("name", geomName);
215             // geometry_file
216             std::string geomFile = "";
217             SimTK::Xml::element_iterator geomFileIter = displayGeomIter->element_begin("geometry_file");
218             if (geomFileIter != displayGeomIter->element_end()) {
219                 geomFile = geomFileIter->getValueAs<SimTK::String>();
220             }
221             // transform
222             SimTK::Vec6 innerXformVec6(0.);
223             SimTK::Xml::element_iterator innerXformIter = displayGeomIter->element_begin("transform");
224             if (innerXformIter != displayGeomIter->element_end()) {
225                 innerXformVec6 = innerXformIter->getValueAs<SimTK::Vec6>();
226             }
227             // Old geometry/visibleObject could specify 2 transforms one per DisplayGeometry
228             // and one for the whole visibleObject.
229             // We'll attach to the frame only if both transforms are identity
230             attachToThisFrame = (innerXformVec6.norm() < SimTK::Eps)
231                 && (outerTransformVec6.norm() < SimTK::Eps);
232             // In practice no models were found where both innerTransform and outerTransform were
233             // non trivial or scale factors were specified in both
234             if (!attachToThisFrame) {
235                 // Create a Vec6 by composing inner and outer transforms
236                 SimTK::Transform outerTransform(outerTransformVec6.getSubVec<3>(3));
237                 outerTransform.updR().setRotationToBodyFixedXYZ(outerTransformVec6.getSubVec<3>(0));
238                 SimTK::Transform innerTransform(innerXformVec6.getSubVec<3>(3));
239                 innerTransform.updR().setRotationToBodyFixedXYZ(innerXformVec6.getSubVec<3>(0));
240                 SimTK::Transform composed = outerTransform.compose(innerTransform);
241                 SimTK::Vec6 composedXformVec6(0.);
242                 composedXformVec6.updSubVec<3>(3) = composed.p();
243                 composedXformVec6.updSubVec<3>(0) = composed.R().convertRotationToBodyFixedXYZ();
244                 // Create a new frame and attach geometry to it
245                 std::string frameName = bodyName + "_geom_frame_" + to_string(counter);
246                 SimTK::Xml::Element frameNode("frame_name", frameName);
247                 meshNode.insertNodeAfter(meshNode.element_end(), frameNode);
248 
249                 SimTK::Xml::element_iterator componentsNode =
250                     bodyNode.element_begin("components");
251 
252                 if (componentsNode == bodyNode.element_end()) {
253                     SimTK::Xml::Element componentsElement("components");
254                     bodyNode.insertNodeBefore(bodyNode.element_end(), componentsElement);
255                     componentsNode = bodyNode.element_begin("components");
256                 }
257 
258                 // Create Frame from composed transform and insert in
259                 // components list.
260                 // The transform is relative to the owning component (this
261                 // PhysicalFrame), so the connectee name just points up to this
262                 // component ("..").
263                 createFrameForXform(componentsNode, frameName,
264                         composedXformVec6, "..");
265 
266                 // For Geometry under 'attached_geometry', the 'frame' is
267                 // always the owning component.
268                 XMLDocument::addConnector(meshNode, "Connector_Frame_", "frame", "..");
269                 SimTK::Xml::element_iterator parentFrame = componentsNode->element_begin("PhysicalOffsetFrame");
270                 while (parentFrame->getRequiredAttributeValue("name") != frameName)
271                     parentFrame++;
272 
273                 // if parentFrame has no "attached_geometry" child, add one else find it
274                 SimTK::Xml::element_iterator frameAttachedGeometry = parentFrame->element_begin("attached_geometry");
275                 if (frameAttachedGeometry == parentFrame->element_end()) {
276                     SimTK::Xml::Element attachedGeomNode("attached_geometry");
277                     parentFrame->insertNodeAfter(parentFrame->node_end(), attachedGeomNode);
278                     frameAttachedGeometry = parentFrame->element_begin("attached_geometry");
279                 }
280                 frameAttachedGeometry->insertNodeAfter(frameAttachedGeometry->element_end(), meshNode);
281 
282             }
283             else
284                 // For Geometry under 'attached_geometry', the 'frame' is
285                 // always the owning component.
286                 XMLDocument::addConnector(meshNode, "Connector_Frame_",
287                         "frame", "..");
288             // scale_factor
289             SimTK::Vec3 localScale(1.);
290             SimTK::Xml::element_iterator localScaleIter = displayGeomIter->element_begin("scale_factors");
291             if (localScaleIter != displayGeomIter->element_end()) {
292                 localScale = localScaleIter->getValueAs<SimTK::Vec3>();
293             }
294             // Now compose scale factors and transforms and create new node to insert into bodyNode
295             SimTK::Xml::Element meshFileNode("mesh_file", geomFile);
296             std::stringstream localScaleStr;
297             localScaleStr << localScale[0] * outerScaleFactors[0] << " " << localScale[1] * outerScaleFactors[1]
298                 << " " << localScale[2] * outerScaleFactors[2];
299             SimTK::Xml::Element scaleFactorsNode("scale_factors", localScaleStr.str());
300             meshNode.insertNodeAfter(meshNode.element_end(), scaleFactorsNode);
301             meshNode.insertNodeAfter(meshNode.element_end(), meshFileNode);
302             SimTK::Xml::Element appearanceNode("Appearance");
303             // Move color and opacity under Appearance
304             SimTK::Xml::element_iterator colorIter = displayGeomIter->element_begin("color");
305             if (colorIter != displayGeomIter->element_end()) {
306                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), displayGeomIter->removeNode(colorIter));
307             }
308             SimTK::Xml::element_iterator opacityIter = displayGeomIter->element_begin("opacity");
309             if (opacityIter != displayGeomIter->element_end()) {
310                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), displayGeomIter->removeNode(opacityIter));
311             }
312             SimTK::Xml::element_iterator reprIter = displayGeomIter->element_begin("display_preference");
313             if (reprIter != displayGeomIter->element_end()) {
314                 reprIter->setElementTag("representation");
315                 if (reprIter->getValue() == "4") {
316                     // Enum changed to go 0-3 instead of 0-4
317                     SimTK::String rep = "3";
318                     reprIter->setValue(rep);
319                 }
320                 appearanceNode.insertNodeAfter(appearanceNode.element_end(), displayGeomIter->removeNode(reprIter));
321             }
322             meshNode.insertNodeAfter(meshNode.element_end(), appearanceNode);
323             // Insert Mesh into parent
324             if (attachToThisFrame)
325                 geometrySetNode.insertNodeAfter(geometrySetNode.element_end(), meshNode);
326 
327             displayGeomIter++;
328             counter++;
329         }
330     }
331 }
332 
createFrameForXform(const SimTK::Xml::element_iterator & ownerIter,const std::string & frameName,const SimTK::Vec6 & localXform,const std::string & parentConnecteeName)333 void PhysicalFrame::createFrameForXform(
334         const SimTK::Xml::element_iterator& ownerIter,
335         const std::string& frameName, const SimTK::Vec6& localXform,
336         const std::string& parentConnecteeName)
337 {
338     SimTK::Xml::Element frameNode("PhysicalOffsetFrame");
339     frameNode.setAttributeValue("name", frameName);
340     stringstream ssT;
341     ssT << localXform[3] << " " << localXform[4] << " " << localXform[5];
342     SimTK::Xml::Element translationNode("translation", ssT.str());
343     stringstream ssO;
344     ssO << localXform[0] << " " << localXform[1] << " " << localXform[2];
345     SimTK::Xml::Element orientationNode("orientation", ssO.str());
346     frameNode.insertNodeAfter(frameNode.element_end(), translationNode);
347     frameNode.insertNodeAfter(frameNode.element_end(), orientationNode);
348     ownerIter->insertNodeAfter(ownerIter->element_end(), frameNode);
349     XMLDocument::addConnector(frameNode, "Connector_PhysicalFrame_", "parent",
350             parentConnecteeName);
351 }
352