1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Conlain Kelly
13 // =============================================================================
14 //
15 // Parser utility class for OpenSim input files.
16 //
17 // This uses several frame transforms for the forward kinematics pass. A
18 // transform is denoted by `X_1_2`, where 1 is the initial fram and 2 is the
19 // final frame. The frames are denoted g: global, P: parent, F: joint on parent,
20 // M: joint on body, B: body. Thus, X_P_F is the transform from parent to joint.
21 //
22 // =============================================================================
23
24 #include "chrono/utils/ChParserOpenSim.h"
25 #include "chrono_thirdparty/filesystem/path.h"
26 #include "chrono_thirdparty/filesystem/resolver.h"
27 #include "chrono_thirdparty/rapidxml/rapidxml_print.hpp"
28 #include "chrono_thirdparty/rapidxml/rapidxml_utils.hpp"
29
30 #include "chrono/core/ChCubicSpline.h"
31 #include "chrono/core/ChFrame.h"
32 #include "chrono/physics/ChSystemNSC.h"
33 #include "chrono/physics/ChSystemSMC.h"
34
35 #include "chrono/assets/ChColorAsset.h"
36 #include "chrono/assets/ChCylinderShape.h"
37 #include "chrono/assets/ChObjShapeFile.h"
38 #include "chrono/assets/ChSphereShape.h"
39 #include "chrono/utils/ChUtilsCreators.h"
40
41 #include "chrono/utils/ChUtilsInputOutput.h"
42
43 #include <cstring>
44 #include <utility>
45 using namespace filesystem;
46 using namespace rapidxml;
47
48 namespace chrono {
49 namespace utils {
50
51 using std::cout;
52 using std::endl;
53
54 // -----------------------------------------------------------------------------
55 // Constructor for the OpenSim parser.
56 // Initializes lambda table.
57 // -----------------------------------------------------------------------------
58
ChParserOpenSim()59 ChParserOpenSim::ChParserOpenSim()
60 : m_collide(false),
61 m_family_1(1),
62 m_family_2(2),
63 m_visType(VisType::NONE),
64 m_verbose(false),
65 m_activate_actuators(false),
66 m_friction(0.6f),
67 m_restitution(0.4f),
68 m_young_modulus(2e5f),
69 m_poisson_ratio(0.3f),
70 m_kn(2e5),
71 m_kt(2e5),
72 m_gn(40),
73 m_gt(20),
74 m_datapath("opensim/") {
75 initFunctionTable();
76 }
77
78 // -----------------------------------------------------------------------------
79 // Enable collision and specify collision families.
80 // -----------------------------------------------------------------------------
81
SetCollisionFamilies(int family_1,int family_2)82 void ChParserOpenSim::SetCollisionFamilies(int family_1, int family_2) {
83 assert(family_1 != family_2);
84 assert(family_1 >= 0 && family_1 <= 15);
85 assert(family_2 >= 0 && family_2 <= 15);
86
87 m_family_1 = family_1;
88 m_family_2 = family_2;
89 m_collide = true;
90 }
91
92 // -----------------------------------------------------------------------------
93 // Set contact material properties (SMC contact)
94 // -----------------------------------------------------------------------------
95
SetContactMaterialCoefficients(float kn,float gn,float kt,float gt)96 void ChParserOpenSim::SetContactMaterialCoefficients(float kn, float gn, float kt, float gt) {
97 m_kn = kn;
98 m_gn = gn;
99 m_kt = kt;
100 m_gt = gt;
101 }
102
SetContactMaterialProperties(float young_modulus,float poisson_ratio)103 void ChParserOpenSim::SetContactMaterialProperties(float young_modulus, float poisson_ratio) {
104 m_young_modulus = young_modulus;
105 m_poisson_ratio = poisson_ratio;
106 }
107
108 // -----------------------------------------------------------------------------
109 // Parse an OpenSim file into an existing system.
110 // -----------------------------------------------------------------------------
111
Parse(ChSystem & system,const std::string & filename)112 void ChParserOpenSim::Parse(ChSystem& system, const std::string& filename) {
113 // Use the model name as the directory for obj files. For example,
114 // the osim file opensim/Rajagopal2015.osim should have the referenced obj
115 // files sitting in data/opensim/Rajagopal2015/
116 // Use filesystem for path parsing, we should probably use the library more
117 path filepath(filename);
118 // Set the member path so when we load meshes we can find them
119 // Strip the parent and stem to use as directory for data files
120 m_datapath = filepath.parent_path().str() + "/" + filepath.stem() + "/";
121
122 rapidxml::file<char> file(filename.c_str());
123
124 xml_document<> doc;
125 doc.parse<0>(file.data());
126
127 // Get gravity from model and set it in system
128 auto elems = strToSTLVector<double>(doc.first_node()->first_node("Model")->first_node("gravity")->value());
129 system.Set_G_acc(ChVector<>(elems[0], elems[1], elems[2]));
130
131 // Traverse the list of bodies and parse the information for each one
132 xml_node<>* bodySet = doc.first_node()->first_node("Model")->first_node("BodySet");
133 if (bodySet == NULL && bodySet->first_node("objects") != NULL) {
134 std::cout << "No body set detected for this model." << std::endl;
135 return;
136 }
137 xml_node<>* bodyNode = bodySet->first_node("objects")->first_node();
138 while (bodyNode != NULL) {
139 parseBody(bodyNode, system);
140 bodyNode = bodyNode->next_sibling();
141 }
142
143 ////xml_node<>* controllerSet =
144 //// doc.first_node()->first_node("Model")->first_node("ControllerSet")->first_node("objects");
145 ////xml_node<>* controllerNode = controllerSet->first_node();
146 ////while (controllerNode != NULL) {
147 //// auto actuators = strToSTLVector<std::string>(controllerNode->first_node("actuator_list")->value());
148 //// controllerNode = controllerNode->next_sibling();
149 //// // Read function as controllerNode->first_node("FunctionSet")
150 ////}
151
152 auto loadcontainer = chrono_types::make_shared<ChLoadContainer>();
153 system.Add(loadcontainer);
154
155 xml_node<>* forceSet = doc.first_node()->first_node("Model")->first_node("ForceSet");
156 if (forceSet != NULL && forceSet->first_node("objects") != NULL) {
157 std::cout << forceSet << forceSet->first_node("objects") << std::endl;
158 xml_node<>* forceNode = forceSet->first_node("objects")->first_node();
159 while (forceNode != NULL) {
160 parseForce(forceNode, system, loadcontainer);
161 forceNode = forceNode->next_sibling();
162 }
163 } else {
164 std::cout << "No forces detected." << std::endl;
165 }
166
167 initShapes(bodyNode, system);
168 }
169
170 // -----------------------------------------------------------------------------
171 // Makes a new system and then parses into it
172 // -----------------------------------------------------------------------------
173
Parse(const std::string & filename,ChContactMethod contact_method)174 ChSystem* ChParserOpenSim::Parse(const std::string& filename, ChContactMethod contact_method) {
175 ChSystem* sys = (contact_method == ChContactMethod::NSC) ? static_cast<ChSystem*>(new ChSystemNSC)
176 : static_cast<ChSystem*>(new ChSystemSMC);
177
178 Parse(*sys, filename);
179
180 return sys;
181 }
182
183 // -----------------------------------------------------------------------------
184 // Implementation of ChParserOpenSim::Report methods
185 // -----------------------------------------------------------------------------
186
Print() const187 void ChParserOpenSim::Report::Print() const {
188 std::cout << "Parsed " << bodies.size() << " bodies:\n";
189 for (auto const& body : bodies) {
190 std::cout << " name: \"" << body.first << "\"" << std::endl;
191 }
192
193 std::cout << "Parsed " << joints.size() << " joints:\n";
194 for (auto const& joint : joints) {
195 std::cout << " name: \"" << joint.first << "\", type: \"" << joint.second.type
196 << "\", standin: " << (joint.second.standin ? "yes" : "no") << std::endl;
197 }
198
199 std::cout << "Parsed " << forces.size() << " forces:\n";
200 for (auto const& force : forces) {
201 std::cout << " name: \"" << force.first << "\", type: \"" << force.second.type << "\"" << std::endl;
202 }
203 }
204
GetBody(const std::string & name) const205 std::shared_ptr<ChBodyAuxRef> ChParserOpenSim::Report::GetBody(const std::string& name) const {
206 std::shared_ptr<ChBodyAuxRef> body;
207 auto body_info = bodies.find(name);
208 if (body_info != bodies.end()) {
209 body = body_info->second;
210 }
211 return body;
212 }
213
GetJoint(const std::string & name) const214 std::shared_ptr<ChLink> ChParserOpenSim::Report::GetJoint(const std::string& name) const {
215 std::shared_ptr<ChLink> joint;
216 auto joint_info = joints.find(name);
217 if (joint_info != joints.end()) {
218 joint = joint_info->second.joint;
219 }
220 return joint;
221 }
222
GetForce(const std::string & name) const223 std::shared_ptr<ChLoadBase> ChParserOpenSim::Report::GetForce(const std::string& name) const {
224 std::shared_ptr<ChLoadBase> force;
225 auto force_info = forces.find(name);
226 if (force_info != forces.end()) {
227 force = force_info->second.load;
228 }
229 return force;
230 }
231
232 // -----------------------------------------------------------------------------
233 // Set excitation function for the actuator with the specified name.
234 // -----------------------------------------------------------------------------
235
SetExcitationFunction(const std::string & name,std::shared_ptr<ChFunction> modulation)236 void ChParserOpenSim::SetExcitationFunction(const std::string& name, std::shared_ptr<ChFunction> modulation) {
237 // Get the force element from the report object
238 auto force = m_report.GetForce(name);
239 if (!force)
240 return;
241
242 if (auto b_force = std::dynamic_pointer_cast<ChLoadBodyForce>(force)) {
243 b_force->SetModulationFunction(modulation);
244 } else if (auto bb_torque = std::dynamic_pointer_cast<ChLoadBodyBodyTorque>(force)) {
245 bb_torque->SetModulationFunction(modulation);
246 }
247 }
248
249 // -----------------------------------------------------------------------------
250 // Create and add a new body.
251 // Parse its various properties from its XML child nodes.
252 // -----------------------------------------------------------------------------
253
parseForce(rapidxml::xml_node<> * forceNode,ChSystem & system,std::shared_ptr<ChLoadContainer> container)254 bool ChParserOpenSim::parseForce(rapidxml::xml_node<>* forceNode,
255 ChSystem& system,
256 std::shared_ptr<ChLoadContainer> container) {
257 if (stringStripCStr(forceNode->name()) == std::string("PointActuator")) {
258 std::string name = stringStripCStr(forceNode->first_attribute("name")->value());
259 std::cout << "Actuator " << name << std::endl;
260 // Chrono should be using std::string
261 auto body = system.SearchBody(stringStripCStr(forceNode->first_node("body")->value()).c_str());
262 ChVector<> point = strToChVector<double>(forceNode->first_node("point")->value());
263 auto point_global = CStrToBool(forceNode->first_node("point_is_global")->value());
264 ChVector<> direction = strToChVector<double>(forceNode->first_node("direction")->value());
265 auto force_global = CStrToBool(forceNode->first_node("force_is_global")->value());
266 auto max_force = std::stod(stringStripCStr(forceNode->first_node("optimal_force")->value()));
267
268 auto load = chrono_types::make_shared<ChLoadBodyForce>(body, max_force * direction, !force_global, point, !point_global);
269 if (!m_activate_actuators)
270 load->SetModulationFunction(chrono_types::make_shared<ChFunction_Const>(0));
271 container->Add(load);
272
273 m_report.forces.insert(std::make_pair(name, Report::ForceInfo{std::string("PointActuator"), load}));
274
275 return true;
276 } else if (stringStripCStr(forceNode->name()) == std::string("TorqueActuator")) {
277 std::string name = stringStripCStr(forceNode->first_attribute("name")->value());
278 auto bodyA = system.SearchBody(stringStripCStr(forceNode->first_node("bodyA")->value()).c_str());
279 auto bodyB = system.SearchBody(stringStripCStr(forceNode->first_node("bodyB")->value()).c_str());
280 auto torque_is_global = CStrToBool(forceNode->first_node("torque_is_global")->value());
281 ChVector<> axis = strToChVector<double>(forceNode->first_node("axis")->value());
282 auto max_force = std::stod(forceNode->first_node("optimal_force")->value());
283
284 // Note: OpenSim assumes the specified torque is applied to bodyA (with an equal and opposite
285 // torque applied to bodyB). In ChLoadBodyBodyTorque, the torque is applied to the 2nd body
286 // passed in its constructor.
287 auto load = chrono_types::make_shared<ChLoadBodyBodyTorque>(bodyB, bodyA, max_force * axis, !torque_is_global);
288 if (!m_activate_actuators)
289 load->SetModulationFunction(chrono_types::make_shared<ChFunction_Const>(0));
290 container->Add(load);
291
292 m_report.forces.insert(std::make_pair(name, Report::ForceInfo{std::string("TorqueActuator"), load}));
293
294 return true;
295 }
296
297 std::cout << "Unknown actuator type: " << forceNode->name() << std::endl;
298 return false;
299 }
300
parseBody(xml_node<> * bodyNode,ChSystem & system)301 bool ChParserOpenSim::parseBody(xml_node<>* bodyNode, ChSystem& system) {
302 // Make a new body and name it for later
303 if (m_verbose) {
304 std::cout << "New body " << bodyNode->first_attribute("name")->value() << std::endl;
305 }
306 // Create a new body, consistent with the type of the containing system
307 auto name = std::string(bodyNode->first_attribute("name")->value());
308 auto newBody = std::shared_ptr<ChBodyAuxRef>(system.NewBodyAuxRef());
309 newBody->SetNameString(name);
310 system.AddBody(newBody);
311
312 newBody->SetCollide(m_collide);
313
314 // Traverse the list of fields and parse the information for each one
315 xml_node<>* fieldNode = bodyNode->first_node();
316 while (fieldNode != NULL) {
317 function_table[fieldNode->name()](fieldNode, newBody);
318 fieldNode = fieldNode->next_sibling();
319 }
320
321 m_report.bodies.insert(std::make_pair(name, newBody));
322 return true;
323 }
324
325 // -----------------------------------------------------------------------------
326 // Set up the lambda table for body parsing.
327 // This is where the magic happens -- it sets up a map of strings to lambdas
328 // that do the heavy lifting of osim -> chrono parsing.
329 // Each entry in the function_table is a child of the "Body" XML node.
330 // -----------------------------------------------------------------------------
331
initFunctionTable()332 void ChParserOpenSim::initFunctionTable() {
333 function_table["mass"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
334 if (std::stod(fieldNode->value()) == 0) {
335 // Ground-like body, massless => fixed
336 newBody->SetBodyFixed(true);
337 newBody->SetCollide(false);
338 newBody->SetPos(ChVector<>(0, 0, 0));
339 // Mark as ground body
340 newBody->AddAsset(chrono_types::make_shared<ChColorAsset>(0.0f, 0.0f, 0.0f));
341 } else {
342 // Give new body mass
343 newBody->SetMass(std::stod(fieldNode->value()));
344 // Add to bodies that are collision
345 }
346 };
347
348 function_table["mass_center"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
349 // Set COM in reference frame
350 auto COM = strToChVector<double>(fieldNode->value());
351 // Opensim doesn't really use a rotated COM to REF frame, so unit quaternion
352 newBody->SetFrame_COG_to_REF(ChFrame<>(COM, ChQuaternion<>(1, 0, 0, 0)));
353 // Only add vis balls if we're using primitives
354 };
355
356 function_table["inertia_xx"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
357 // Set xx inertia moment
358 ChVector<> inertiaXX = newBody->GetInertiaXX();
359 inertiaXX.x() = std::stod(fieldNode->value());
360 newBody->SetInertiaXX(inertiaXX);
361 };
362
363 function_table["inertia_yy"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
364 // Set yy inertia moment
365 ChVector<> inertiaXX = newBody->GetInertiaXX();
366 inertiaXX.y() = std::stod(fieldNode->value());
367 newBody->SetInertiaXX(inertiaXX);
368 };
369
370 function_table["inertia_zz"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
371 // Set zz inertia moment
372 ChVector<> inertiaXX = newBody->GetInertiaXX();
373 inertiaXX.z() = std::stod(fieldNode->value());
374 newBody->SetInertiaXX(inertiaXX);
375 };
376
377 function_table["inertia_xy"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
378 // Set xy inertia moment
379 ChVector<> inertiaXY = newBody->GetInertiaXY();
380 inertiaXY.x() = std::stod(fieldNode->value());
381 newBody->SetInertiaXY(inertiaXY);
382 };
383
384 function_table["inertia_xz"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
385 // Set xz inertia moment
386 ChVector<> inertiaXY = newBody->GetInertiaXY();
387 inertiaXY.y() = std::stod(fieldNode->value());
388 newBody->SetInertiaXY(inertiaXY);
389 };
390
391 function_table["inertia_yz"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
392 // Set yz inertia moment
393 ChVector<> inertiaXY = newBody->GetInertiaXY();
394 inertiaXY.z() = std::stod(fieldNode->value());
395 newBody->SetInertiaXY(inertiaXY);
396 };
397
398 function_table["Joint"] = [this](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
399 // If there are no joints, this is hopefully the ground (or another global parent??)
400 if (fieldNode->first_node() == NULL) {
401 if (m_verbose)
402 std::cout << "No joints for this body " << std::endl;
403 return;
404 }
405
406 // Containing system
407 auto system = newBody->GetSystem();
408
409 // Deduce child body from joint orientation
410 xml_node<>* jointNode = fieldNode->first_node();
411
412 // Name/type of OpenSim joint
413 std::string name = stringStripCStr(jointNode->first_attribute("name")->value());
414 std::string type = stringStripCStr(jointNode->name());
415
416 // Make a joint here
417 if (m_verbose)
418 std::cout << "Making a " << type << " with " << jointNode->first_node("parent_body")->value() << std::endl;
419
420 // Get other body for joint
421 auto parent = system->SearchBody(stringStripCStr(jointNode->first_node("parent_body")->value()).c_str());
422
423 if (parent != nullptr) {
424 if (m_verbose)
425 std::cout << "other body found!" << std::endl;
426 } else {
427 // really bad, either file is invalid or we messed up
428 if (m_verbose)
429 std::cout << "Parent not found!!!! Exiting." << std::endl;
430 exit(1);
431 }
432
433 // Global to parent transform
434 auto X_G_P = parent->GetFrame_REF_to_abs();
435
436 // Get joint's position in parent frame
437 auto elems = ChParserOpenSim::strToSTLVector<double>(jointNode->first_node("location_in_parent")->value());
438 ChVector<> jointPosInParent = ChVector<>(elems[0], elems[1], elems[2]);
439
440 // Get joint's orientation in parent frame
441 elems = ChParserOpenSim::strToSTLVector<double>(jointNode->first_node("orientation_in_parent")->value());
442 ChQuaternion<> jointOrientationInParent = Q_from_AngX(elems[0]) * Q_from_AngY(elems[1]) * Q_from_AngZ(elems[2]);
443
444 // Parent to joint in parent transform
445 ChFrame<> X_P_F(jointPosInParent, jointOrientationInParent);
446
447 // Offset location and orientation caused by joint initial configuration, default is identity
448 ChFrame<> X_F_M(ChVector<>(0, 0, 0), ChQuaternion<>(1, 0, 0, 0));
449 // Get offsets, depending on joint type
450 if (type == std::string("PinJoint")) {
451 xml_node<>* coordinates =
452 jointNode->first_node("CoordinateSet")->first_node("objects")->first_node("Coordinate");
453 // Just a rotation about Z
454 double thetaZ = std::stod(coordinates->first_node("default_value")->value());
455 X_F_M = Q_from_AngZ(thetaZ) * X_F_M;
456 } else if (type == std::string("WeldJoint")) {
457 // Do absolutely nothing, they're stuck together
458 } else if (type == std::string("UniversalJoint")) {
459 xml_node<>* coordinates =
460 jointNode->first_node("CoordinateSet")->first_node("objects")->first_node("Coordinate");
461 // Body-fixed X,Y rotations
462 double thetaX = std::stod(coordinates->first_node("default_value")->value());
463 coordinates = coordinates->next_sibling();
464 double thetaY = std::stod(coordinates->first_node("default_value")->value());
465 X_F_M = Q_from_AngX(thetaX) * Q_from_AngY(thetaY) * X_F_M;
466 } else if (type == std::string("BallJoint")) {
467 xml_node<>* coordinates =
468 jointNode->first_node("CoordinateSet")->first_node("objects")->first_node("Coordinate");
469 // Body-fixed X,Y,Z rotations
470 double thetaX = std::stod(coordinates->first_node("default_value")->value());
471 coordinates = coordinates->next_sibling();
472 double thetaY = std::stod(coordinates->first_node("default_value")->value());
473 coordinates = coordinates->next_sibling();
474 double thetaZ = std::stod(coordinates->first_node("default_value")->value());
475 X_F_M = Q_from_AngX(thetaX) * Q_from_AngY(thetaY) * Q_from_AngZ(thetaZ) * X_F_M;
476 } else if (type == std::string("CustomJoint")) {
477 // Make a map of generalized coordinates' default values so we can get initial position
478 std::map<std::string, double> coordVals;
479 xml_node<>* coordinates =
480 jointNode->first_node("CoordinateSet")->first_node("objects")->first_node("Coordinate");
481 // Take in coordinate set
482 while (coordinates != nullptr) {
483 // Map coordinate name to its default value
484 coordVals[std::string(coordinates->first_attribute("name")->value())] =
485 std::stod(coordinates->first_node("default_value")->value());
486 coordinates = coordinates->next_sibling();
487 }
488 xml_node<>* transforms = jointNode->first_node("SpatialTransform")->first_node("TransformAxis");
489 // First 3 are rotation, next 3 are translation
490 int transformIndex = 0;
491 while (transforms != nullptr) {
492 // Type of OpenSim function
493 std::string functionType(transforms->first_node("function")->first_node()->name());
494
495 // Get coordinates involved
496 auto coordNames =
497 ChParserOpenSim::strToSTLVector<std::string>(transforms->first_node("coordinates")->value());
498 // There is no point in going on if the transform is empty
499 if ((coordNames.size() == 0) && (functionType != std::string("Constant"))) {
500 // Still need to increment
501 transformIndex++;
502 transforms = transforms->next_sibling();
503 continue;
504 }
505
506 // Value to transform about/along axis, set later
507 double transformValue;
508
509 // Get axis to transform about/along
510 auto axisElems = ChParserOpenSim::strToSTLVector<double>(transforms->first_node("axis")->value());
511 ChVector<> axis(axisElems[0], axisElems[1], axisElems[2]);
512
513 if (functionType == std::string("LinearFunction")) {
514 // ax + b style linear mapping
515 elems = ChParserOpenSim::strToSTLVector<double>(
516 transforms->first_node("function")->first_node()->first_node("coefficients")->value());
517 transformValue = elems[0] * coordVals[coordNames.at(0)] + elems[1];
518 } else if (functionType == std::string("Constant")) {
519 // Just a constant
520 transformValue =
521 std::stod(transforms->first_node("function")->first_node()->first_node("value")->value());
522
523 // } else if (functionType == std::string("SimmSpline")) {
524 } else if (functionType == std::string("NaturalCubicSpline") ||
525 functionType == std::string("SimmSpline")) {
526 // In opensim, both types of spline are treated as SimmSplines, which we approximate with
527 // Natural Cubic Splines
528 auto vectX = ChParserOpenSim::strToSTLVector<double>(
529 transforms->first_node("function")->first_node()->first_node("x")->value());
530 auto vectY = ChParserOpenSim::strToSTLVector<double>(
531 transforms->first_node("function")->first_node()->first_node("y")->value());
532 // If not, the following logic will fail
533 assert(vectX.size() == vectY.size());
534
535 // Make a Natural Cubic Spline
536 ChCubicSpline transformFunc(vectX, vectY);
537 transformFunc.SetLeftBC(ChCubicSpline::BCType::SECOND_BC, 0);
538 transformFunc.SetRightBC(ChCubicSpline::BCType::SECOND_BC, 0);
539 // These are the first and second derivatives which we do not need
540 double a, b;
541 // Drop spline's value into the transformValue
542 transformFunc.Evaluate(coordVals[coordNames.at(0)], transformValue, a, b);
543
544 } else {
545 if (m_verbose)
546 std::cout << "Unknown transform type: " << functionType
547 << ", skipping initial position calculations." << std::endl;
548 continue;
549 }
550
551 if (transformIndex < 3) {
552 // Rotation
553 X_F_M = Q_from_AngAxis(transformValue, axis) * X_F_M;
554 } else {
555 // Translation
556 X_F_M = (transformValue * axis) * X_F_M;
557 }
558
559 // Iterate through linked list
560 transforms = transforms->next_sibling();
561 transformIndex++;
562 }
563 }
564
565 // Get joint's position in child frame
566 elems = ChParserOpenSim::strToSTLVector<double>(jointNode->first_node("location")->value());
567 ChVector<> jointPosInChild = ChVector<>(elems[0], elems[1], elems[2]);
568
569 // Get joint's orientation in child frame
570 elems = ChParserOpenSim::strToSTLVector<double>(jointNode->first_node("orientation")->value());
571 ChQuaternion<> jointOrientationInChild = Q_from_AngX(elems[0]) * Q_from_AngY(elems[1]) * Q_from_AngZ(elems[2]);
572
573 // Joint in child to child
574 ChFrame<> X_B_M(jointPosInChild, jointOrientationInChild);
575
576 // Multiply transforms through
577 auto X_G_B = X_G_P * X_P_F * X_F_M * X_B_M.GetInverse();
578
579 // Set body frame, not necessarily centroidal
580 newBody->SetFrame_REF_to_abs(X_G_B);
581
582 // Used for following output
583 ChFrame<> jointFrame = X_G_P * X_P_F;
584
585 assert(std::abs(newBody->GetRot().Length() - 1) < 1e-10);
586
587 if (m_verbose) {
588 std::cout << "Parent is at global " << parent->GetPos().x() << "," << parent->GetPos().y() << ","
589 << parent->GetPos().z() << "|" << parent->GetRot().e0() << "," << parent->GetRot().e1() << ","
590 << parent->GetRot().e2() << "," << parent->GetRot().e3() << std::endl;
591 std::cout << "Joint is at parent " << jointPosInParent.x() << "," << jointPosInParent.y() << ","
592 << jointPosInParent.z() << "|" << jointOrientationInParent.e0() << ","
593 << jointOrientationInParent.e1() << "," << jointOrientationInParent.e2() << ","
594 << jointOrientationInParent.e3() << std::endl;
595 std::cout << "Joint is at global " << jointFrame.GetPos().x() << "," << jointFrame.GetPos().y() << ","
596 << jointFrame.GetPos().z() << "|" << jointFrame.GetRot().e0() << "," << jointFrame.GetRot().e1()
597 << "," << jointFrame.GetRot().e2() << "," << jointFrame.GetRot().e3() << std::endl;
598 std::cout << "Joint is at child " << jointPosInChild.x() << "," << jointPosInChild.y() << ","
599 << jointPosInChild.z() << "|" << jointOrientationInChild.e0() << ","
600 << jointOrientationInChild.e1() << "," << jointOrientationInChild.e2() << ","
601 << jointOrientationInChild.e3() << std::endl;
602 std::cout << "Putting body " << newBody->GetName() << " at " << newBody->GetFrame_REF_to_abs().GetPos().x()
603 << "," << newBody->GetFrame_REF_to_abs().GetPos().y() << ","
604 << newBody->GetFrame_REF_to_abs().GetPos().z() << std::endl;
605 std::cout << "Orientation is " << newBody->GetFrame_REF_to_abs().GetRot().e0() << ","
606 << newBody->GetFrame_REF_to_abs().GetRot().e1() << ","
607 << newBody->GetFrame_REF_to_abs().GetRot().e2() << ","
608 << newBody->GetFrame_REF_to_abs().GetRot().e3() << std::endl;
609 }
610
611 // This is slightly cleaner than before, but still gross
612 std::shared_ptr<ChLink> joint;
613 bool standin = false;
614 // Make a joint, depending on what it actually is
615 if (type == std::string("PinJoint")) {
616 auto revJoint = chrono_types::make_shared<ChLinkLockRevolute>();
617 revJoint->Initialize(parent, newBody, jointFrame.GetCoord());
618 joint = revJoint;
619 } else if (type == std::string("WeldJoint")) {
620 auto weldJoint = chrono_types::make_shared<ChLinkLockLock>();
621 weldJoint->Initialize(parent, newBody, jointFrame.GetCoord());
622 joint = weldJoint;
623 } else if (type == std::string("UniversalJoint")) {
624 // Do some universal magic here
625 auto uniJoint = chrono_types::make_shared<ChLinkUniversal>();
626 uniJoint->Initialize(parent, newBody, jointFrame);
627 joint = uniJoint;
628 } else if (type == std::string("BallJoint")) {
629 // Add a spherical joint
630 auto spherJoint = chrono_types::make_shared<ChLinkLockSpherical>();
631 spherJoint->Initialize(parent, newBody, jointFrame.GetCoord());
632 joint = spherJoint;
633 } else {
634 standin = true;
635 // Unknown joint type. Replace with a spherical
636 std::cout << "Unknown Joint type " << type << " between " << parent->GetName() << " and "
637 << newBody->GetName() << " -- making spherical standin." << std::endl;
638 auto customJoint = chrono_types::make_shared<ChLinkLockSpherical>();
639 customJoint->Initialize(parent, newBody, jointFrame.GetCoord());
640 joint = customJoint;
641 }
642
643 joint->SetNameString(name);
644 system->AddLink(joint);
645
646 m_jointList.push_back(joint);
647 m_report.joints.insert(std::make_pair(name, Report::JointInfo{type, joint, standin}));
648 };
649
650 function_table["VisibleObject"] = [this](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
651 // Add visualization assets to body, they must be in data/opensim
652 if (m_visType == VisType::MESH) {
653 auto geometry = fieldNode->first_node("GeometrySet")->first_node("objects")->first_node();
654 while (geometry != nullptr) {
655 std::string meshFilename(geometry->first_node("geometry_file")->value());
656 auto bodyMesh = chrono_types::make_shared<ChObjShapeFile>();
657 bodyMesh->SetFilename(m_datapath + meshFilename);
658 newBody->AddAsset(bodyMesh);
659 geometry = geometry->next_sibling();
660 }
661 }
662 };
663
664 function_table["WrapObjectSet"] = [](xml_node<>* fieldNode, std::shared_ptr<ChBodyAuxRef> newBody) {
665 // I don't think we need this
666 };
667 }
668
669 // -----------------------------------------------------------------------------
670 // Initialize collision shapes and attach visualization assets.
671 // -----------------------------------------------------------------------------
672
673 // Holds info for a single collision cylinder
674 struct collision_cylinder_specs {
675 double rad;
676 double hlen;
677 ChVector<> pos;
678 ChQuaternion<> rot;
679 };
680
681 // Holds all cylinder info and family info for a single body
682 struct body_collision_struct {
683 ChBodyAuxRef* body;
684 std::vector<collision_cylinder_specs> cylinders;
685 int family;
686 int family_mask_nocollide;
687 int level; // Depth in tree
688 };
689
690 // Used for vis shapes
691 static std::map<std::string, body_collision_struct> body_collision_info;
692
initShapes(rapidxml::xml_node<> * node,ChSystem & system)693 void ChParserOpenSim::initShapes(rapidxml::xml_node<>* node, ChSystem& system) {
694 if (m_verbose)
695 std::cout << "Creating collision and visualization shapes " << std::endl;
696
697 // Keep the maximum depth so that we can color appropriately
698 int max_depth_level = 0;
699
700 // Go through the motions of constructing collision models, but don't do it until we have all of the necessary info
701 for (auto link : m_jointList) {
702 auto linkCoords = link->GetLinkAbsoluteCoords();
703 // These need to be ChBodyAuxRef, but hopefully that's all that we'll have
704 auto parent = dynamic_cast<ChBodyAuxRef*>(link->GetBody1());
705 auto child = dynamic_cast<ChBodyAuxRef*>(link->GetBody2());
706
707 // Add pointers to struct if they don't exist, struct entry should be default-constructed
708 if (body_collision_info.find(parent->GetName()) == body_collision_info.end()) {
709 body_collision_info[parent->GetName()].body = parent;
710 }
711 if (body_collision_info.find(child->GetName()) == body_collision_info.end()) {
712 body_collision_info[child->GetName()].body = child;
713 }
714 body_collision_struct& parent_col_info = body_collision_info[parent->GetName()];
715 body_collision_struct& child_col_info = body_collision_info[child->GetName()];
716
717 // If a body is fixed, treat it as level 0
718 if (parent->GetBodyFixed()) {
719 parent_col_info.level = 0;
720 }
721 // Child is one more level than parent
722 int child_level = parent_col_info.level + 1;
723 // Keep a max depth so we know how to color it
724 if (child_level > max_depth_level) {
725 max_depth_level = child_level;
726 }
727 child_col_info.level = child_level;
728
729 ChVector<> p1, p2;
730 // Make cylinder from parent to outbound joint
731 // First end is at parent COM
732 p1 = parent->GetFrame_COG_to_REF().GetPos();
733 // Second end is at joint location
734 p2 = parent->GetFrame_REF_to_abs().TransformPointParentToLocal(linkCoords.pos);
735
736 // Don't make a connection between overlapping bodies
737 if ((p2 - p1).Length() > 1e-5) {
738 ChMatrix33<> rot;
739 rot.Set_A_Xdir(p1 - p2);
740 // Center of cylinder is halfway between points
741 collision_cylinder_specs new_cyl;
742 new_cyl.rad = .02;
743 new_cyl.hlen = (p1 - p2).Length() / 2;
744 new_cyl.pos = (p2 - p1) / 2;
745 new_cyl.rot = rot.Get_A_quaternion() * Q_from_AngZ(-CH_C_PI / 2);
746 body_collision_info[parent->GetName()].cylinders.push_back(new_cyl);
747 }
748
749 // First end is at joint location
750 p1 = child->GetFrame_REF_to_abs().TransformPointParentToLocal(linkCoords.pos);
751 // Second end is at child COM
752 p2 = child->GetFrame_COG_to_REF().GetPos();
753
754 // Don't make a connection between overlapping bodies
755 if ((p2 - p1).Length() > 1e-5) {
756 ChMatrix33<> rot;
757 rot.Set_A_Xdir(p2 - p1);
758 // Center of cylinder is halfway between points
759 collision_cylinder_specs new_cyl;
760 new_cyl.rad = .02;
761 new_cyl.hlen = (p2 - p1).Length() / 2;
762 new_cyl.pos = (p1 - p2) / 2;
763 new_cyl.rot = rot.Get_A_quaternion() * Q_from_AngZ(-CH_C_PI / 2);
764 body_collision_info[child->GetName()].cylinders.push_back(new_cyl);
765 }
766
767 // Make cylinder from child to inbound joint
768 if (m_verbose)
769 std::cout << parent->GetName() << " family is " << body_collision_info[parent->GetName()].family
770 << std::endl;
771
772 if ((parent->GetCollide() == false) || (body_collision_info[parent->GetName()].family == m_family_2)) {
773 if (m_verbose)
774 std::cout << "Setting " << child->GetName() << " to family " << m_family_1 << std::endl;
775 body_collision_info[child->GetName()].family = m_family_1;
776 body_collision_info[child->GetName()].family_mask_nocollide = m_family_2;
777 } else {
778 if (m_verbose)
779 std::cout << "Setting " << child->GetName() << " to family " << m_family_2 << std::endl;
780 body_collision_info[child->GetName()].family = m_family_2;
781 body_collision_info[child->GetName()].family_mask_nocollide = m_family_1;
782 }
783 }
784
785 // Loop through the list of bodies in the model and create visualization and collision shapes
786 for (auto body_info_pair : body_collision_info) {
787 auto body_info = body_info_pair.second;
788
789 // Create primitive visualization assets
790 if (m_visType == VisType::PRIMITIVES) {
791 // Assign a color based on the body's level in the tree hierarchy
792 float colorVal = (1.0f * body_info.level) / max_depth_level;
793 body_info.body->AddAsset(chrono_types::make_shared<ChColorAsset>(colorVal, 1.0f - colorVal, 0.0f));
794
795 // Create a sphere at the body COM
796 auto sphere = chrono_types::make_shared<ChSphereShape>();
797 sphere->GetSphereGeometry().rad = 0.1;
798 sphere->Pos = body_info.body->GetFrame_COG_to_REF().GetPos();
799 body_info.body->AddAsset(sphere);
800
801 // Create visualization cylinders
802 for (auto cyl_info : body_info.cylinders) {
803 auto cylinder = chrono_types::make_shared<ChCylinderShape>();
804 cylinder->GetCylinderGeometry().rad = cyl_info.rad;
805 cylinder->GetCylinderGeometry().p1 = ChVector<>(0, cyl_info.hlen, 0);
806 cylinder->GetCylinderGeometry().p2 = ChVector<>(0, -cyl_info.hlen, 0);
807 cylinder->Pos = cyl_info.pos;
808 cylinder->Rot = cyl_info.rot;
809 body_info.body->AddAsset(cylinder);
810 }
811 }
812
813 // Create a contact material that will be shared by all shapes for this body
814 std::shared_ptr<ChMaterialSurface> mat;
815 switch (system.GetContactMethod()) {
816 case ChContactMethod::NSC: {
817 auto matNSC = chrono_types::make_shared<ChMaterialSurfaceNSC>();
818 matNSC->SetFriction(m_friction);
819 matNSC->SetRestitution(m_restitution);
820 mat = matNSC;
821 break;
822 }
823 case ChContactMethod::SMC: {
824 auto matSMC = chrono_types::make_shared<ChMaterialSurfaceSMC>();
825 matSMC->SetFriction(m_friction);
826 matSMC->SetRestitution(m_restitution);
827 matSMC->SetYoungModulus(m_young_modulus);
828 matSMC->SetPoissonRatio(m_poisson_ratio);
829 matSMC->SetKn(m_kn);
830 matSMC->SetGn(m_gn);
831 matSMC->SetKt(m_kt);
832 matSMC->SetGt(m_gt);
833 mat = matSMC;
834 break;
835 }
836 }
837
838 // Set collision shapes
839 if (body_info.body->GetCollide()) {
840 body_info.body->GetCollisionModel()->ClearModel();
841
842 for (auto cyl_info : body_info.cylinders) {
843 utils::AddCylinderGeometry(body_info.body, mat, cyl_info.rad, cyl_info.hlen, cyl_info.pos, cyl_info.rot,
844 false);
845 }
846
847 body_info.body->GetCollisionModel()->SetFamily(body_info.family);
848 body_info.body->GetCollisionModel()->SetFamilyMaskNoCollisionWithFamily(body_info.family_mask_nocollide);
849
850 body_info.body->GetCollisionModel()->BuildModel();
851 }
852 }
853 }
854
855 } // end namespace utils
856 } // end namespace chrono
857