From ff5fb01cab2db7610c1663d0b9ccbc6f75866f48 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 19 May 2022 14:07:27 -0500 Subject: [PATCH 01/55] Draft of OpenSim::ExponentialSpringForce OpenSim::ExponentialSpringForce compiles and links. No default constructor. Properties are: 1. SimTK::Transform - contact_plane_transform 2. PhysicalFrame - body 3. SimTK::Vec3 - body_station For now, the default parameters will be used. Next step is to create an example to test simulation, serialization, and reporting. Once these aspects look good, I'll add ExponentialSpringParameters so that a spring instance can be customized. --- .../Model/ExponentialSpringForce.cpp | 151 ++++++++++++++++++ .../Simulation/Model/ExponentialSpringForce.h | 96 +++++++++++ 2 files changed, 247 insertions(+) create mode 100644 OpenSim/Simulation/Model/ExponentialSpringForce.cpp create mode 100644 OpenSim/Simulation/Model/ExponentialSpringForce.h diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp new file mode 100644 index 0000000000..265d8e8602 --- /dev/null +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -0,0 +1,151 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ExponentialSpringForce.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): F. C. Anderson * + * * + * 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. * + * -------------------------------------------------------------------------- */ + +#include "ExponentialSpringForce.h" +#include "Model.h" + +#include "simbody/internal/ExponentialSpringForce.h" + +namespace OpenSim { + +//============================================================================= +// Class ExponentialSpringForce +//============================================================================= + +//----------------------------------------------------------------------------- +// Construction +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +ExponentialSpringForce:: +ExponentialSpringForce(const SimTK::Transform& XContactPlane, + const PhysicalFrame& body, const SimTK::Vec3& station) +{ + constructProperties(XContactPlane, body, station); +} + + +//----------------------------------------------------------------------------- +// Properties +//----------------------------------------------------------------------------- +// Questions: +// 1. Is it ok not to have a default constructor? There really isn't a +// situation in which a default constructor makes sense. There needs to +// be a body to which the force is applied. I suppose the default constructor +// could apply a force between Ground and Ground. +//_____________________________________________________________________________ +void +ExponentialSpringForce:: +constructProperties(const SimTK::Transform& XContactPlane, + const PhysicalFrame& body, const SimTK::Vec3& station) +{ + constructProperty_contact_plane_transform(XContactPlane); + constructProperty_body(body); + constructProperty_body_station(station); +} + + +//----------------------------------------------------------------------------- +// ADD TO THE MULTIBODY SYSTEM +//----------------------------------------------------------------------------- +// Questions: +// 1. Does "spr" not need to be allocated from the heap? +// 2. What happens to the force subsystem when spr goes out of scope? +// +// Guesses: +// 1. The GeneralForceSubsystem makes a clone of spr in a way that +// keeps references to it still valid. +// +//_____________________________________________________________________________ +// Connect to SimTK::ExponentialSpringForce +void +ExponentialSpringForce:: +extendAddToSystem(SimTK::MultibodySystem& system) const { + // Extend the OpenSim::Force parent + Super::extendAddToSystem(system); + + // Construct the SimTK::ExponentialSpringForce object + SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); + const SimTK::Transform& XContactPlane = get_contact_plane_transform(); + const OpenSim::PhysicalFrame& body = get_body(); + const SimTK::Vec3& station = get_body_station(); + SimTK::ExponentialSpringForce + spr(forces, XContactPlane, body.getMobilizedBody(), station); + + // Get the subsystem index so we can access the SimTK::Force later. + ExponentialSpringForce* mutableThis = + const_cast(this); + mutableThis->_index = spr.getForceIndex(); +} + + +//----------------------------------------------------------------------------- +// ACCESSORS +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +const SimTK::Transform& +ExponentialSpringForce::getContactPlaneTransform() const { + return get_contact_plane_transform(); +} +//_____________________________________________________________________________ +void ExponentialSpringForce::setContactPlaneTransform( + const SimTK::Transform& XContactPlane) { + set_contact_plane_transform(XContactPlane); +} + + +//----------------------------------------------------------------------------- +// Reporting +//----------------------------------------------------------------------------- +// Questions: +// 1. Do I need to conform to a certain reporting format since this object +// will be treated as an OpenSim::Force ? +//_____________________________________________________________________________ +OpenSim::Array +ExponentialSpringForce:: +getRecordLabels() const { + OpenSim::Array labels(""); + std::string frameName = get_body().getName(); + labels.append(getName()+"."+frameName+".force.X"); + labels.append(getName()+"."+frameName+".force.Y"); + labels.append(getName()+"."+frameName+".force.Z"); + + return labels; +} +//_____________________________________________________________________________ +OpenSim::Array +ExponentialSpringForce:: +getRecordValues(const SimTK::State& state) const +{ + OpenSim::Array values(1); + + const auto& forceSubsys = getModel().getForceSubsystem(); + const SimTK::Force& abstractForce = forceSubsys.getForce(_index); + const auto& spr = (SimTK::ExponentialSpringForce&)(abstractForce); + + SimTK::Vec3 force = spr.getForce(state); + values.append(3, &force[0]); + + return values; +} + +}// end of namespace OpenSim diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialSpringForce.h new file mode 100644 index 0000000000..9927a67640 --- /dev/null +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.h @@ -0,0 +1,96 @@ +#ifndef OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ +#define OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ +/* -------------------------------------------------------------------------- * + * OpenSim: HuntCrossleyForce.h * + * -------------------------------------------------------------------------- * + * 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): Peter Eastman * + * * + * 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. * + * -------------------------------------------------------------------------- */ +// INCLUDE +#include "Force.h" +#include "OpenSim/Common/Set.h" + +namespace OpenSim { + +//============================================================================== +// EXPONENTIAL SPRING FORCE +//============================================================================== +/** This force subclass implements an ExponentialSpringForce to model contact +of a specified point on a body (i.e., a "station" in Simbody vocabulary) with +a contact plane that is anchored to Ground. + +@author F. C. Anderson **/ +class OSIMSIMULATION_API ExponentialSpringForce : public Force { + OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialSpringForce, Force); + +public: + //============================================================================== + // PROPERTIES + //============================================================================== + OpenSim_DECLARE_PROPERTY(contact_plane_transform, SimTK::Transform, + "Rotation and translation of the contact plane wrt Ground."); + OpenSim_DECLARE_PROPERTY( + body, PhysicalFrame, "Body to which the force is applied."); + OpenSim_DECLARE_PROPERTY(body_station, SimTK::Vec3, + "Point on the body at which the force is applied."); + + //______________________________________________________________________________ + /** Construct an ExponentialSpringForce. + @param XContactPlane Transform specifying the location and orientation of + the contact plane in Ground. + @param body Body to which the force is applied. + @param station Point on the body at which the force is applied. */ + explicit ExponentialSpringForce(const SimTK::Transform& XContactPlane, + const PhysicalFrame& body, const SimTK::Vec3& station); + + //----------------------------------------------------------------------------- + // Accessors + //----------------------------------------------------------------------------- + /** Get the tranform that specifies the location and orientation of the + contact plane in the Ground frame. */ + const SimTK::Transform& getContactPlaneTransform() const; + /** Set the tranform that specifies the location and orientation of the + contact plane in the Ground frame. */ + void setContactPlaneTransform(const SimTK::Transform& XContactPlane); + + //----------------------------------------------------------------------------- + // Reporting + //----------------------------------------------------------------------------- + /** Provide name(s) of the quantities (column labels) of the value(s) to be + reported. */ + OpenSim::Array getRecordLabels() const override; + /** Provide the value(s) to be reported that correspond to the labels. */ + OpenSim::Array getRecordValues( + const SimTK::State& state) const override; + +protected: + /** Create a SimTK::Force which implements this Force. */ + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + +private: + void constructProperties(const SimTK::Transform& XContactPlane, + const PhysicalFrame& body, const SimTK::Vec3& station); + +}; // END of class ExponentialSpringForce + +} // end of namespace OpenSim + +#endif // OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ + + From 60a82d66a0e0937a86e58d2adbbc19da7b4c0260 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 24 May 2022 00:47:51 -0500 Subject: [PATCH 02/55] First draft. Just has HuntCrossleyForce at the moment. Just getting the bones of the class together. --- .../Simulation/Test/testExponentialSpring.cpp | 360 ++++++++++++++++++ 1 file changed, 360 insertions(+) create mode 100644 OpenSim/Simulation/Test/testExponentialSpring.cpp diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp new file mode 100644 index 0000000000..370f5275fc --- /dev/null +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -0,0 +1,360 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testContactExponentialSpring.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) 2022 Stanford University and the Authors * + * Author(s): F. C. Anderson * + * * + * 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. * + * -------------------------------------------------------------------------- */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SimTKsimbody.h" + +using namespace SimTK; +using namespace std; + +namespace OpenSim { + + +//============================================================================= +/** Class ExponentialSpringTester provides a thread-safe scope and framework +for evaluating and testing the ExponentialSpringForce class. */ +class ExponentialSpringTester { + +public: + // Contact choices + enum ContactChoice { + ExpSpr = 0, + HuntCross, + Both + }; + + // Initial condition choices + enum InitialConditionsChoice { + Static = 0, + Bounce, + Slide, + SpinSlide, + Tumble + }; + + // Constructor + ExponentialSpringTester(){}; + + // Destructor + ~ExponentialSpringTester() { + model->disownAllComponents(); + delete model; + } + + // Command line parsing and usage + int parseCommandLine(int argc, char** argv); + void printUsage(); + + // Model Creation + void buildModel(); + OpenSim::Body* addBlock(Model* model, const String& suffix); + //void addExponentialSprings(Model* model, OpenSim::Body* body); + void addHuntCrossleyContact(Model* model, OpenSim::Body* body); + + // Simulation + void setInitialConditions(); + void simulate(Model* model); + + // MEMBER VARIABLES + // Simulation related + double integ_accuracy{1.0e-5}; + SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; + double mass{10.0}; + double halfSide{0.10}; + double tf{5.0}; + + // Command line options and their defaults + ContactChoice whichContact{ExpSpr}; + InitialConditionsChoice whichInit{Slide}; + bool noDamp{false}; + bool applyFx{false}; + bool visuals{false}; + + // Other member variables + Model* model{NULL}; + Body* blockES{NULL}; + Body* blockHC{NULL}; + +}; // End class ExponentialSpringTester declarations + +} // End namespace OpenSim + + +using namespace OpenSim; + +//_____________________________________________________________________________ +int +ExponentialSpringTester:: +parseCommandLine(int argc, char** argv) { + + string option; + for (int i = 1; i < argc; ++i) { + + option = argv[i]; + + // Contact choice + if (option == "ExpSpr") + whichContact = ExpSpr; + else if (option == "HuntCross") + whichContact = HuntCross; + else if (option == "Both") + whichContact = Both; + + // Initial condition choice + else if (option == "Static") + whichInit = Static; + else if (option == "Bounce") + whichInit = Bounce; + else if (option == "Slide") + whichInit = Slide; + else if (option == "SpinSlide") + whichInit = SpinSlide; + else if (option == "Tumble") + whichInit = Tumble; + + // Turn off all dissipative terms + else if (option == "NoDamp") + noDamp = true; + + // Apply a horizontal ramping force + else if (option == "ApplyFx") + applyFx = true; + + // Show the visuals + else if (option == "Vis") + visuals = true; + + // Unrecognized + else { + printUsage(); + return -1; + } + } + return 0; +} + +//_____________________________________________________________________________ +void +ExponentialSpringTester:: +printUsage() { + cout << endl << "Usage:" << endl; + cout << "$ testExponetialSpring " + << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; + cout << "\tInitCond (choose one): Static Bounce Slide SpinSlide Tumble"; + cout << endl; + cout << "\t Contact (choose one): ExpSpr HuntCross Both" << endl << endl; + + cout << "All arguments are optional. If no arguments are specified, "; + cout << "a 'Slide' will" << endl; + cout << "be simulated with one block that uses "; + cout << "ExponentialSpringForce contact," << endl; + cout << "with typical damping settings, "; + cout << "with no extnerally applied force, " << endl; + cout << "and with no visuals." << endl << endl; + + cout << "Example:" << endl; + cout << "To simulated 2 blocks (one with Exponential Springs and one"; + cout << " with Hunt-Crossley)" << endl; + cout << "that bounce without energy dissipation and with Visuals, "; + cout << "enter the following: " << endl << endl; + + cout << "$ testExponentialSpring Bounce Both NoDamp Vis" << endl << endl; +} + +//_____________________________________________________________________________ +// Build the model +void ExponentialSpringTester::buildModel() { + // Create the model(s) + Model* model = new Model(); + model->setGravity(gravity); + model->setName("TestExponentialSpring"); + switch (whichContact) { + case ExpSpr: + blockES = addBlock(model, "ES"); + // addExponentialSprings(model, blockES); + case HuntCross: + blockHC = addBlock(model, "HC"); + addHuntCrossleyContact(model, blockHC); + case Both: + blockES = addBlock(model, "ES"); + // addExponentialSprings(model, blockES); + blockHC = addBlock(model, "HC"); + addHuntCrossleyContact(model, blockHC); + } +} +//______________________________________________________________________________ +OpenSim::Body* +ExponentialSpringTester:: +addBlock(Model* model, const String& suffix) { + + Ground& ground = model->updGround(); + + // Body + OpenSim::Body* block = new OpenSim::Body(); + block->setName("Block"+suffix); + block->set_mass(mass); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + + // Joint + FreeJoint free("Free"+suffix, + ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addBody(block); + model->addJoint(&free); + + return block; +} +//______________________________________________________________________________ +void +ExponentialSpringTester:: +addHuntCrossleyContact(Model* model, OpenSim::Body* block) { + + Ground& ground = model->updGround(); + + // Create ContactGeometry. + ContactHalfSpace* floor = new ContactHalfSpace( + Vec3(0), Vec3(0, 0, -0.5 * SimTK_PI), ground, "floor"); + model->addContactGeometry(floor); + OpenSim::ContactGeometry* geometry; + geometry = new ContactSphere(halfSide, Vec3(0), *block, "sphere"); + model->addContactGeometry(geometry); + + // Add a HuntCrossleyForce. + auto* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters( + 1.0e7, 2e-1, 0.0, 0.0, 0.0); + contactParams->addGeometry("sphere"); + contactParams->addGeometry("floor"); + OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); + model->addForce(force); +} + +//_____________________________________________________________________________ +void +ExponentialSpringTester:: +setInitialConditions() { + +} + +//_____________________________________________________________________________ +void +ExponentialSpringTester:: +simulate(Model* model) +{ + SimTK::State& state = model->initSystem(); + model->getMultibodySystem().realize(state, Stage::Velocity ); + + cout << "state =" << state << std::endl; + + Manager manager(*model); + manager.setIntegratorAccuracy(integ_accuracy); + state.setTime(0.0); + manager.initialize(state); + state = manager.integrate(tf); +} + + +//_____________________________________________________________________________ +/* Entry Point (i.e., main()) + +The motion of a 10 kg, 6 degree-of-freedom block and its force interaction +with a laboratory floor are simulated. + +Contact with the floor is modeled using either + 1) 8 ExponentialSpringForce instances, one at each corner of the block, or + 2) 8 HuntCrossleyForce instances, one at each corner of the block. + +For a side-by-side comparison of simulated motions, two blocks (one using +the ExponentialSpringForce class for contact and the other using the +HuntCrossleyForce class) can be created and visualized simultaneously. + +For an assessment of computational performance, just one block should be +simulated at a time. Number of integration trys and steps, as well as cpu time, +are reported. + +Choice of initial conditions can be made in order to generate the following +motions: + 1) Static (y = 0.1 m, sitting at rest on the floor) + 2) Bouncing (y = 1.0 m, dropped) + 3) Sliding (y = 0.2 m, vx = -2.0 m/s) + 4) Spinning & Sliding (y = 0.2 m, vx = -2.0 m/s, wy = 2.0 pi rad/sec) + 5) Tumbling (py = 2.0 m, vx = -2.0 m/s, wz = 2.0 pi rad/sec) + +Additional options allow the following to be specified: + NoDamp Parameters are chosen to eliminate all energy dissipation. + ApplyFx A ramping horizontal force (Fx) is applied after 5.0 sec. + +If no external force is applied, tf = 5.0 s. + +If an external force is applied, tf = 10.0 s and this force ramps up +linearly from a value of fx = 0.0 at t = 5.0 s to a value of +fx = |mass*g| at t = 10.0 s. The force is not ramped up prior to t = 5.0 s +in order to allow the block an opportunity to come more fully to rest. +This ramping profile was done with the "Static" initial condition choice +in mind so that friction models could be evaluated more critically. +In particular, a static block should not start sliding until fx > μₛ Fₙ. + +For ExponentialSpringForce, the following things are tested: + a) instantiation + b) model initialization + c) energy conservation + d) data cache access + e) realization stage invalidation + f) reporting + g) serialization + +The HuntCrossleyForce class is tested elsewhere (e.g., see +testContactGeometry.cpp). */ +int main(int argc, char** argv) { + try { + ExponentialSpringTester tester; + if(tester.parseCommandLine(argc, argv) < 0) return 1; + tester.buildModel(); + //tester.setInitialConditions(); + //tester.simulate(); + + } catch (const OpenSim::Exception& e) { + e.print(cerr); + return 1; + } + cout << "Done" << endl; + return 0; +} From 39aeed477de264106abba26b850595cf8690f52f Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 24 May 2022 00:48:25 -0500 Subject: [PATCH 03/55] Minor. Just line length corrections. --- OpenSim/Simulation/Model/ExponentialSpringForce.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp index 265d8e8602..4e51c74b58 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -1,5 +1,5 @@ /* -------------------------------------------------------------------------- * - * OpenSim: ExponentialSpringForce.cpp * + * OpenSim: ExponentialSpringForce.cpp * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * * See http://opensim.stanford.edu and the NOTICE file for more information. * @@ -8,7 +8,7 @@ * through the Warrior Web program. * * * * Copyright (c) 2005-2017 Stanford University and the Authors * - * Author(s): F. C. Anderson * + * Author(s): F. C. Anderson * * * * 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 * From 9f73ff3a2597bd95d9ff56780bf8381b843b6353 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 24 May 2022 18:48:37 -0500 Subject: [PATCH 04/55] Completed the basic framework. Started adding the Visualizer. --- .../Simulation/Test/testExponentialSpring.cpp | 130 +++++++++++++----- 1 file changed, 99 insertions(+), 31 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index 370f5275fc..df1cf68996 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -63,7 +63,7 @@ class ExponentialSpringTester { }; // Initial condition choices - enum InitialConditionsChoice { + enum InitialConditionsChoice{ Static = 0, Bounce, Slide, @@ -72,7 +72,10 @@ class ExponentialSpringTester { }; // Constructor - ExponentialSpringTester(){}; + ExponentialSpringTester() { + runMenuItems.push_back(std::make_pair("Go", 1)); + runMenuItems.push_back(std::make_pair("Replay", 2)); + }; // Destructor ~ExponentialSpringTester() { @@ -86,13 +89,17 @@ class ExponentialSpringTester { // Model Creation void buildModel(); - OpenSim::Body* addBlock(Model* model, const String& suffix); + OpenSim::Body* addBlock(const String& suffix); //void addExponentialSprings(Model* model, OpenSim::Body* body); - void addHuntCrossleyContact(Model* model, OpenSim::Body* body); + void addHuntCrossleyContact(OpenSim::Body* body); + + // Vizualizer + void addVisualizer(); // Simulation - void setInitialConditions(); - void simulate(Model* model); + void setInitialConditions(SimTK::State& state, + const SimTK::MobilizedBody& body, double dz); + void simulate(); // MEMBER VARIABLES // Simulation related @@ -103,17 +110,23 @@ class ExponentialSpringTester { double tf{5.0}; // Command line options and their defaults - ContactChoice whichContact{ExpSpr}; + ContactChoice whichContact{HuntCross}; InitialConditionsChoice whichInit{Slide}; bool noDamp{false}; bool applyFx{false}; bool visuals{false}; - // Other member variables + // Model and parts Model* model{NULL}; Body* blockES{NULL}; Body* blockHC{NULL}; + // Visualization + SimTK::Visualizer* viz{NULL}; + SimTK::Visualizer::InputSilo* silo{NULL}; + SimTK::Array_> runMenuItems; + + }; // End class ExponentialSpringTester declarations } // End namespace OpenSim @@ -202,51 +215,55 @@ printUsage() { //_____________________________________________________________________________ // Build the model -void ExponentialSpringTester::buildModel() { +void +ExponentialSpringTester:: +buildModel() { // Create the model(s) - Model* model = new Model(); + model = new Model(); model->setGravity(gravity); model->setName("TestExponentialSpring"); switch (whichContact) { case ExpSpr: - blockES = addBlock(model, "ES"); - // addExponentialSprings(model, blockES); + blockES = addBlock("ES"); + //addExponentialSprings(model, blockES); + break; case HuntCross: - blockHC = addBlock(model, "HC"); - addHuntCrossleyContact(model, blockHC); + blockHC = addBlock("HC"); + addHuntCrossleyContact(blockHC); + break; case Both: - blockES = addBlock(model, "ES"); - // addExponentialSprings(model, blockES); - blockHC = addBlock(model, "HC"); - addHuntCrossleyContact(model, blockHC); + blockES = addBlock("ES"); + //addExponentialSprings(model, blockES); + blockHC = addBlock("HC"); + addHuntCrossleyContact(blockHC); } } //______________________________________________________________________________ OpenSim::Body* ExponentialSpringTester:: -addBlock(Model* model, const String& suffix) { +addBlock(const String& suffix) { Ground& ground = model->updGround(); // Body OpenSim::Body* block = new OpenSim::Body(); - block->setName("Block"+suffix); + block->setName("block"); block->set_mass(mass); block->set_mass_center(Vec3(0)); block->setInertia(Inertia(1.0)); // Joint - FreeJoint free("Free"+suffix, - ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + FreeJoint *free = new + FreeJoint("free", ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); model->addBody(block); - model->addJoint(&free); + model->addJoint(free); return block; } //______________________________________________________________________________ void ExponentialSpringTester:: -addHuntCrossleyContact(Model* model, OpenSim::Body* block) { +addHuntCrossleyContact(OpenSim::Body* block) { Ground& ground = model->updGround(); @@ -259,8 +276,8 @@ addHuntCrossleyContact(Model* model, OpenSim::Body* block) { model->addContactGeometry(geometry); // Add a HuntCrossleyForce. - auto* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters( - 1.0e7, 2e-1, 0.0, 0.0, 0.0); + auto* contactParams = new OpenSim::HuntCrossleyForce:: + ContactParameters(1.0e7, 2e-1, 0.0, 0.0, 0.0); contactParams->addGeometry("sphere"); contactParams->addGeometry("floor"); OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); @@ -270,18 +287,70 @@ addHuntCrossleyContact(Model* model, OpenSim::Body* block) { //_____________________________________________________________________________ void ExponentialSpringTester:: -setInitialConditions() { +setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, + double dz) +{ + SimTK::Vec3 pos(0.0, 0.0, dz); + SimTK::Vec3 vel(0.0); + SimTK::Vec3 angvel(0.0); + + switch (whichInit) { + case Static: + pos[0] = 0.0; + pos[1] = halfSide + 0.004; + body.setQToFitTranslation(state, pos); + break; + case Bounce: + pos[0] = 0.0; + pos[1] = 1.0; + body.setQToFitTranslation(state, pos); + break; + case Slide: + pos[0] = 2.0; + pos[1] = 2.0 * halfSide; + vel[0] = -2.0; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + break; + case SpinSlide: + pos[0] = 2.0; + pos[1] = 2.0 * halfSide; + vel[0] = -2.0; + angvel[1] = 2.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + case Tumble: + pos[0] = 2.0; + pos[1] = 2.0 * halfSide; + vel[0] = -2.0; + angvel[2] = 2.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + default: + cout << "Unrecognized set of initial conditions!" << endl; + } } //_____________________________________________________________________________ void ExponentialSpringTester:: -simulate(Model* model) +simulate() { + // Initialize the System SimTK::State& state = model->initSystem(); model->getMultibodySystem().realize(state, Stage::Velocity ); - + + // Set initial conditions + double dz = 1.0; + if (blockES != NULL) + setInitialConditions(state, blockES->getMobilizedBody(), dz); + if (blockHC != NULL) + setInitialConditions(state, blockHC->getMobilizedBody(), -dz); cout << "state =" << state << std::endl; Manager manager(*model); @@ -348,8 +417,7 @@ int main(int argc, char** argv) { ExponentialSpringTester tester; if(tester.parseCommandLine(argc, argv) < 0) return 1; tester.buildModel(); - //tester.setInitialConditions(); - //tester.simulate(); + tester.simulate(); } catch (const OpenSim::Exception& e) { e.print(cerr); From 569e1cf106ce3b8e2b503a159b1cfa99261571e9 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 25 May 2022 06:02:32 -0500 Subject: [PATCH 05/55] Added draft of a Visualizer class. I'm getting run-time exceptions. It looks like I cannot run the SimTK::Visualizer in OpenSim by accessing it in a raw way. It looks like I should be using OpenSim::ModelVisualizer, which creates its own SimTK::Visualizer and SimTK::Visualizer::InputSilo. --- .../Simulation/Test/testExponentialSpring.cpp | 184 +++++++++++++++--- 1 file changed, 152 insertions(+), 32 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index df1cf68996..7d1628db50 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -45,13 +45,100 @@ using namespace SimTK; using namespace std; +using namespace OpenSim; + +class TestExpSprVisuals; + +//============================================================================= +/** Record the System state at regular intervals. */ +class VisualsReporter : public PeriodicEventReporter { +public: + VisualsReporter(TestExpSprVisuals& visuals, Real reportInterval) : + PeriodicEventReporter(reportInterval), vis(visuals) {} + + void handleEvent(const State& state) const override; + +private: + TestExpSprVisuals& vis; +}; + +//============================================================================= +/** Class Visuals encapsulates the variables needed for running a +SimTK::Visualizer and provides the polling methods for starting a +simulation and replaying the simulated motion. */ +class TestExpSprVisuals { +public: + // Constructor + TestExpSprVisuals(MultibodySystem& system) { + // Menu Items + runMenuItems.push_back(std::make_pair("Go", goItem)); + runMenuItems.push_back(std::make_pair("Replay", replayItem)); + + // SimTK::Visualizer + vis = new Visualizer(system); + vis->setShowShadows(true); + + // Input Silo + silo = new Visualizer::InputSilo(); + vis->addInputListener(silo); + vis->addMenu("Run", runMenuID, runMenuItems); + + // Reporters + system.addEventReporter(new Visualizer::Reporter(*vis, reportInterval)); + system.addEventReporter(new VisualsReporter(*this, reportInterval)); + + // Reserve memory for the System states to be recorded + states.reserve(50000); + } -namespace OpenSim { + // State storage + void appendState(const State& state) { states.emplace_back(state); } + + // Polling + void pollForStart() { + cout << "\nChoose 'Go' from the Run menu to simulate.\n"; + int menuID, item; + do { + silo->waitForMenuPick(menuID, item); + if (menuID != runMenuID || item != goItem) + cout << "\aDude ... follow instructions!\n"; + } while (menuID != runMenuID || item != goItem); + } + void pollForReplay() { + silo->clear(); + while (true) { + cout << "Choose Replay to see that again ...\n"; + int menuID, item; + silo->waitForMenuPick(menuID, item); + for (double i = 0; i < (int)states.size(); i++) { + vis->report(states[(int)i]); + } + } + } + +private: + SimTK::Visualizer* vis{NULL}; + SimTK::Visualizer::InputSilo* silo{NULL}; + SimTK::Array_> runMenuItems; + const int runMenuID = 3; + const int goItem{1}, replayItem{2}, quitItem{3}; + double reportInterval = 0.01; + SimTK::Array_ states; +}; + +//_____________________________________________________________________________ +// Definition had to follow the declaration for TestExpSprVisuals +void +VisualsReporter:: +handleEvent(const State& state) const { + vis.appendState(state); +} //============================================================================= -/** Class ExponentialSpringTester provides a thread-safe scope and framework -for evaluating and testing the ExponentialSpringForce class. */ +/** Class ExponentialSpringTester provides a scope and framework for +evaluating and testing the ExponentialSpringForce class. Using a class gets +a lot of variables out of the global scope. */ class ExponentialSpringTester { public: @@ -72,10 +159,7 @@ class ExponentialSpringTester { }; // Constructor - ExponentialSpringTester() { - runMenuItems.push_back(std::make_pair("Go", 1)); - runMenuItems.push_back(std::make_pair("Replay", 2)); - }; + ExponentialSpringTester() {}; // Destructor ~ExponentialSpringTester() { @@ -93,15 +177,17 @@ class ExponentialSpringTester { //void addExponentialSprings(Model* model, OpenSim::Body* body); void addHuntCrossleyContact(OpenSim::Body* body); - // Vizualizer - void addVisualizer(); - // Simulation + void addDecorations(); void setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz); void simulate(); - // MEMBER VARIABLES + //------------------------------------------------------------------------- + // Member variables + //------------------------------------------------------------------------- + private: + // Simulation related double integ_accuracy{1.0e-5}; SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; @@ -114,25 +200,18 @@ class ExponentialSpringTester { InitialConditionsChoice whichInit{Slide}; bool noDamp{false}; bool applyFx{false}; - bool visuals{false}; + bool showVisuals{true}; // Model and parts Model* model{NULL}; - Body* blockES{NULL}; - Body* blockHC{NULL}; + OpenSim::Body* blockES{NULL}; + OpenSim::Body* blockHC{NULL}; // Visualization - SimTK::Visualizer* viz{NULL}; - SimTK::Visualizer::InputSilo* silo{NULL}; - SimTK::Array_> runMenuItems; - + TestExpSprVisuals* visuals{NULL}; }; // End class ExponentialSpringTester declarations -} // End namespace OpenSim - - -using namespace OpenSim; //_____________________________________________________________________________ int @@ -174,7 +253,7 @@ parseCommandLine(int argc, char** argv) { // Show the visuals else if (option == "Vis") - visuals = true; + showVisuals = true; // Unrecognized else { @@ -184,7 +263,6 @@ parseCommandLine(int argc, char** argv) { } return 0; } - //_____________________________________________________________________________ void ExponentialSpringTester:: @@ -212,7 +290,6 @@ printUsage() { cout << "$ testExponentialSpring Bounce Both NoDamp Vis" << endl << endl; } - //_____________________________________________________________________________ // Build the model void @@ -283,7 +360,37 @@ addHuntCrossleyContact(OpenSim::Body* block) { OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); model->addForce(force); } +//_____________________________________________________________________________ +void +ExponentialSpringTester:: +addDecorations() { + // Ground + Ground& ground = model->updGround(); + SimTK::DecorativeBrick* floor = + new SimTK::DecorativeBrick(Vec3(2.0, 0.5, 2.0)); + floor->setColor(Green); + floor->setOpacity(0.1); + SimTK::Body& grndBody = ground.updMobilizedBody().updBody(); + grndBody.addDecoration(Transform(Vec3(0, -0.5, 0)), *floor); + + // Exponential Springs Block + if (blockES) { + SimTK::Body& body = blockES->updMobilizedBody().updBody(); + SimTK::DecorativeBrick* brick = + new SimTK::DecorativeBrick(SimTK::Vec3(halfSide)); + brick->setColor(SimTK::Blue); + body.addDecoration(SimTK::Transform(), *brick); + } + // Hunt-Crossley Block + if (blockHC) { + SimTK::Body& body = blockHC->updMobilizedBody().updBody(); + SimTK::DecorativeBrick* brick = + new SimTK::DecorativeBrick(SimTK::Vec3(halfSide)); + brick->setColor(SimTK::Red); + body.addDecoration(SimTK::Transform(), *brick); + } +} //_____________________________________________________________________________ void ExponentialSpringTester:: @@ -335,29 +442,41 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, } } - //_____________________________________________________________________________ void ExponentialSpringTester:: -simulate() -{ +simulate() { + // Initialize the System SimTK::State& state = model->initSystem(); - model->getMultibodySystem().realize(state, Stage::Velocity ); - + SimTK::MultibodySystem& system = model->updMultibodySystem(); + + // Visuals + if (false) { + addDecorations(); + visuals = new TestExpSprVisuals(system); + } + // Set initial conditions double dz = 1.0; if (blockES != NULL) - setInitialConditions(state, blockES->getMobilizedBody(), dz); + setInitialConditions(state, blockES->getMobilizedBody(), dz); if (blockHC != NULL) setInitialConditions(state, blockHC->getMobilizedBody(), -dz); cout << "state =" << state << std::endl; + // Visuals Start + if(visuals) visuals->pollForStart(); + + // Integrate Manager manager(*model); manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); state = manager.integrate(tf); + + // Visuals Replay + if (visuals) visuals->pollForReplay(); } @@ -415,7 +534,8 @@ testContactGeometry.cpp). */ int main(int argc, char** argv) { try { ExponentialSpringTester tester; - if(tester.parseCommandLine(argc, argv) < 0) return 1; + int status = tester.parseCommandLine(argc, argv); + if (status<0) return 1; tester.buildModel(); tester.simulate(); From eb632cacb8f0fc27262e868cb061f1e83a79a00c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 25 May 2022 12:13:44 -0500 Subject: [PATCH 06/55] Adding 8 HuntCrossley at corners. Adding SimTK decorations is not working, so I avoid these though the code is still there. --- .../Simulation/Test/testExponentialSpring.cpp | 72 ++++++++++++------- 1 file changed, 45 insertions(+), 27 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index 7d1628db50..2b2c2fdbe7 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -176,9 +176,9 @@ class ExponentialSpringTester { OpenSim::Body* addBlock(const String& suffix); //void addExponentialSprings(Model* model, OpenSim::Body* body); void addHuntCrossleyContact(OpenSim::Body* body); + void addDecorations(); // Simulation - void addDecorations(); void setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz); void simulate(); @@ -192,7 +192,7 @@ class ExponentialSpringTester { double integ_accuracy{1.0e-5}; SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; double mass{10.0}; - double halfSide{0.10}; + double hs{0.10}; // half of a side of a cube (kind of like a radius) double tf{5.0}; // Command line options and their defaults @@ -344,26 +344,46 @@ addHuntCrossleyContact(OpenSim::Body* block) { Ground& ground = model->updGround(); - // Create ContactGeometry. + // Geometry for the floor ContactHalfSpace* floor = new ContactHalfSpace( Vec3(0), Vec3(0, 0, -0.5 * SimTK_PI), ground, "floor"); model->addContactGeometry(floor); - OpenSim::ContactGeometry* geometry; - geometry = new ContactSphere(halfSide, Vec3(0), *block, "sphere"); - model->addContactGeometry(geometry); - - // Add a HuntCrossleyForce. - auto* contactParams = new OpenSim::HuntCrossleyForce:: - ContactParameters(1.0e7, 2e-1, 0.0, 0.0, 0.0); - contactParams->addGeometry("sphere"); - contactParams->addGeometry("floor"); - OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); - model->addForce(force); + + // Corners of the block + const int n = 8; + Vec3 corner[n]; + corner[0] = Vec3(hs, -hs, hs); + corner[1] = Vec3(hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3(hs, hs, hs); + corner[5] = Vec3(hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + + // Loop over the corners + std::string name = ""; + for (int i = 0; i < n; ++i) { + // Geometry + name = "sphere" + std::to_string(i); + OpenSim::ContactGeometry* geometry = + new ContactSphere(0.005, corner[i], *block, name); + model->addContactGeometry(geometry); + + // HuntCrossleyForce + auto* contactParams = new OpenSim::HuntCrossleyForce:: + ContactParameters(1.0e7, 2e-1, 0.0, 0.0, 0.0); + contactParams->addGeometry(name); + contactParams->addGeometry("floor"); + OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); + model->addForce(force); + } } //_____________________________________________________________________________ void ExponentialSpringTester:: addDecorations() { + // Ground Ground& ground = model->updGround(); SimTK::DecorativeBrick* floor = @@ -377,7 +397,7 @@ addDecorations() { if (blockES) { SimTK::Body& body = blockES->updMobilizedBody().updBody(); SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(halfSide)); + new SimTK::DecorativeBrick(SimTK::Vec3(hs)); brick->setColor(SimTK::Blue); body.addDecoration(SimTK::Transform(), *brick); } @@ -386,7 +406,7 @@ addDecorations() { if (blockHC) { SimTK::Body& body = blockHC->updMobilizedBody().updBody(); SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(halfSide)); + new SimTK::DecorativeBrick(SimTK::Vec3(hs)); brick->setColor(SimTK::Red); body.addDecoration(SimTK::Transform(), *brick); } @@ -404,7 +424,7 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, switch (whichInit) { case Static: pos[0] = 0.0; - pos[1] = halfSide + 0.004; + pos[1] = hs + 0.004; body.setQToFitTranslation(state, pos); break; case Bounce: @@ -414,14 +434,14 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, break; case Slide: pos[0] = 2.0; - pos[1] = 2.0 * halfSide; + pos[1] = 2.0 * hs; vel[0] = -2.0; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); break; case SpinSlide: pos[0] = 2.0; - pos[1] = 2.0 * halfSide; + pos[1] = 2.0 * hs; vel[0] = -2.0; angvel[1] = 2.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); @@ -430,7 +450,7 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, break; case Tumble: pos[0] = 2.0; - pos[1] = 2.0 * halfSide; + pos[1] = 2.0 * hs; vel[0] = -2.0; angvel[2] = 2.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); @@ -448,14 +468,8 @@ ExponentialSpringTester:: simulate() { // Initialize the System + if(showVisuals) model->setUseVisualizer(false); SimTK::State& state = model->initSystem(); - SimTK::MultibodySystem& system = model->updMultibodySystem(); - - // Visuals - if (false) { - addDecorations(); - visuals = new TestExpSprVisuals(system); - } // Set initial conditions double dz = 1.0; @@ -473,7 +487,11 @@ simulate() { manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); + // start timing + std::clock_t startTime = std::clock(); state = manager.integrate(tf); + auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; + cout << "simulation time = " << runTime << " msec" << endl; // Visuals Replay if (visuals) visuals->pollForReplay(); From 30644488120d14f73d83a00c30421bd2db321a7c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 27 May 2022 15:47:54 -0500 Subject: [PATCH 07/55] Constructing based on "BodyName" instead of Body. I also added a default constructor and `extendConnectToModel()`. The springs work in OpenSim! --- .../Model/ExponentialSpringForce.cpp | 86 +++++++++++++------ .../Simulation/Model/ExponentialSpringForce.h | 46 +++++++--- 2 files changed, 95 insertions(+), 37 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp index 4e51c74b58..9f21fc2d05 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -26,7 +26,9 @@ #include "simbody/internal/ExponentialSpringForce.h" -namespace OpenSim { +using namespace OpenSim; +using namespace std; + //============================================================================= // Class ExponentialSpringForce @@ -36,13 +38,25 @@ namespace OpenSim { // Construction //----------------------------------------------------------------------------- //_____________________________________________________________________________ -ExponentialSpringForce:: -ExponentialSpringForce(const SimTK::Transform& XContactPlane, - const PhysicalFrame& body, const SimTK::Vec3& station) +ExponentialSpringForce::ExponentialSpringForce() { - constructProperties(XContactPlane, body, station); + setNull(); + constructProperties(); } +//_____________________________________________________________________________ +ExponentialSpringForce:: +ExponentialSpringForce(const SimTK::Transform& contactPlaneXform, + const std::string& bodyName, const SimTK::Vec3& station) +{ + setNull(); + constructProperties(); + setContactPlaneTransform(contactPlaneXform); + setBodyName(bodyName); + setBodyStation(station); +} +//_____________________________________________________________________________ +void ExponentialSpringForce::setNull() { setAuthors("F. C. Anderson"); } //----------------------------------------------------------------------------- // Properties @@ -55,15 +69,38 @@ ExponentialSpringForce(const SimTK::Transform& XContactPlane, //_____________________________________________________________________________ void ExponentialSpringForce:: -constructProperties(const SimTK::Transform& XContactPlane, - const PhysicalFrame& body, const SimTK::Vec3& station) +constructProperties() { - constructProperty_contact_plane_transform(XContactPlane); - constructProperty_body(body); - constructProperty_body_station(station); + SimTK::Transform contactXForm; + constructProperty_contact_plane_transform(contactXForm); + + constructProperty_body_name(""); + + SimTK::Vec3 origin(0.0); + constructProperty_body_station(origin); } +//----------------------------------------------------------------------------- +// CONNECT TO THE MODEL +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialSpringForce:: +extendConnectToModel(OpenSim::Model& model) +{ + // Allow based class to connect first + Super::extendConnectToModel(model); + + // Find the OpenSim::Body + const string& bodyName = getBodyName(); + if (getModel().hasComponent(bodyName)) + _body = &(getModel().getComponent(bodyName)); + else + _body = &(getModel().getComponent( + "./bodyset/" + bodyName)); +} + //----------------------------------------------------------------------------- // ADD TO THE MULTIBODY SYSTEM //----------------------------------------------------------------------------- @@ -76,20 +113,19 @@ constructProperties(const SimTK::Transform& XContactPlane, // keeps references to it still valid. // //_____________________________________________________________________________ -// Connect to SimTK::ExponentialSpringForce void ExponentialSpringForce:: -extendAddToSystem(SimTK::MultibodySystem& system) const { +extendAddToSystem(SimTK::MultibodySystem& system) const +{ // Extend the OpenSim::Force parent Super::extendAddToSystem(system); // Construct the SimTK::ExponentialSpringForce object SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); const SimTK::Transform& XContactPlane = get_contact_plane_transform(); - const OpenSim::PhysicalFrame& body = get_body(); const SimTK::Vec3& station = get_body_station(); SimTK::ExponentialSpringForce - spr(forces, XContactPlane, body.getMobilizedBody(), station); + spr(forces, XContactPlane, _body->getMobilizedBody(), station); // Get the subsystem index so we can access the SimTK::Force later. ExponentialSpringForce* mutableThis = @@ -102,14 +138,16 @@ extendAddToSystem(SimTK::MultibodySystem& system) const { // ACCESSORS //----------------------------------------------------------------------------- //_____________________________________________________________________________ -const SimTK::Transform& -ExponentialSpringForce::getContactPlaneTransform() const { - return get_contact_plane_transform(); +void ExponentialSpringForce:: +setContactPlaneTransform(const SimTK::Transform& XContactPlane) +{ + set_contact_plane_transform(XContactPlane); } //_____________________________________________________________________________ -void ExponentialSpringForce::setContactPlaneTransform( - const SimTK::Transform& XContactPlane) { - set_contact_plane_transform(XContactPlane); +const SimTK::Transform& +ExponentialSpringForce::getContactPlaneTransform() const +{ + return get_contact_plane_transform(); } @@ -118,13 +156,14 @@ void ExponentialSpringForce::setContactPlaneTransform( //----------------------------------------------------------------------------- // Questions: // 1. Do I need to conform to a certain reporting format since this object -// will be treated as an OpenSim::Force ? +// will be treated as an OpenSim::Force? //_____________________________________________________________________________ OpenSim::Array ExponentialSpringForce:: -getRecordLabels() const { +getRecordLabels() const +{ OpenSim::Array labels(""); - std::string frameName = get_body().getName(); + std::string frameName = getBodyName(); labels.append(getName()+"."+frameName+".force.X"); labels.append(getName()+"."+frameName+".force.Y"); labels.append(getName()+"."+frameName+".force.Z"); @@ -148,4 +187,3 @@ getRecordValues(const SimTK::State& state) const return values; } -}// end of namespace OpenSim diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialSpringForce.h index 9927a67640..a7113cb074 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.h +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.h @@ -44,30 +44,44 @@ class OSIMSIMULATION_API ExponentialSpringForce : public Force { // PROPERTIES //============================================================================== OpenSim_DECLARE_PROPERTY(contact_plane_transform, SimTK::Transform, - "Rotation and translation of the contact plane wrt Ground."); - OpenSim_DECLARE_PROPERTY( - body, PhysicalFrame, "Body to which the force is applied."); + "Rotation and translation of the contact plane wrt Ground."); + OpenSim_DECLARE_PROPERTY(body_name, std::string, + "Name of the Body to which the force is applied."); OpenSim_DECLARE_PROPERTY(body_station, SimTK::Vec3, - "Point on the body at which the force is applied."); + "Point on the body at which the force is applied."); + + //______________________________________________________________________________ + /** Default constructor. */ + ExponentialSpringForce(); //______________________________________________________________________________ /** Construct an ExponentialSpringForce. @param XContactPlane Transform specifying the location and orientation of the contact plane in Ground. - @param body Body to which the force is applied. + @param bodyName Name of the body to which the force is applied. @param station Point on the body at which the force is applied. */ - explicit ExponentialSpringForce(const SimTK::Transform& XContactPlane, - const PhysicalFrame& body, const SimTK::Vec3& station); + explicit ExponentialSpringForce(const SimTK::Transform& contactPlaneXform, + const std::string& bodyName, const SimTK::Vec3& station); //----------------------------------------------------------------------------- // Accessors //----------------------------------------------------------------------------- + /** Set the tranform that specifies the location and orientation of the + contact plane in the Ground frame. */ + void setContactPlaneTransform(const SimTK::Transform& contactPlaneXform); /** Get the tranform that specifies the location and orientation of the contact plane in the Ground frame. */ const SimTK::Transform& getContactPlaneTransform() const; - /** Set the tranform that specifies the location and orientation of the - contact plane in the Ground frame. */ - void setContactPlaneTransform(const SimTK::Transform& XContactPlane); + + /** Set the name of the body to which this force is applied. */ + void setBodyName(const std::string& bodyName) { set_body_name(bodyName); } + /** Get the name of the body to which this force is applied. */ + const std::string& getBodyName() const { return get_body_name(); } + + /** Set the point on the body at which the force is applied. */ + void setBodyStation(SimTK::Vec3 station) { set_body_station(station); } + /** Get the point on the body at which the force is applied. */ + const SimTK::Vec3& getBodyStation() { return get_body_station(); } //----------------------------------------------------------------------------- // Reporting @@ -80,12 +94,18 @@ class OSIMSIMULATION_API ExponentialSpringForce : public Force { const SimTK::State& state) const override; protected: - /** Create a SimTK::Force which implements this Force. */ + /** Connect to the OpenSim Model. */ + void extendConnectToModel(Model& model) override; + + /** Create a SimTK::ExponentialSpringForce that implements this Force. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; private: - void constructProperties(const SimTK::Transform& XContactPlane, - const PhysicalFrame& body, const SimTK::Vec3& station); + void setNull(); + void constructProperties(); + + // Temporary solution until implemented with Sockets + SimTK::ReferencePtr _body; }; // END of class ExponentialSpringForce From ab3aeda24c73ab7087cefa1588c907d56cd8c333 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 27 May 2022 15:50:12 -0500 Subject: [PATCH 08/55] Added Exponential Springs and an ExternalForce. Things are running! I can also run both the ExpSpr and HuntCross block at the same time. Basic computational performance is printed out at the end. --- .../Simulation/Test/testExponentialSpring.cpp | 278 +++++++++++++----- 1 file changed, 202 insertions(+), 76 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index 2b2c2fdbe7..c640b7625d 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -37,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -51,7 +53,8 @@ class TestExpSprVisuals; //============================================================================= /** Record the System state at regular intervals. */ -class VisualsReporter : public PeriodicEventReporter { +class VisualsReporter : public PeriodicEventReporter +{ public: VisualsReporter(TestExpSprVisuals& visuals, Real reportInterval) : PeriodicEventReporter(reportInterval), vis(visuals) {} @@ -66,7 +69,8 @@ class VisualsReporter : public PeriodicEventReporter { /** Class Visuals encapsulates the variables needed for running a SimTK::Visualizer and provides the polling methods for starting a simulation and replaying the simulated motion. */ -class TestExpSprVisuals { +class TestExpSprVisuals +{ public: // Constructor TestExpSprVisuals(MultibodySystem& system) { @@ -130,17 +134,17 @@ class TestExpSprVisuals { // Definition had to follow the declaration for TestExpSprVisuals void VisualsReporter:: -handleEvent(const State& state) const { +handleEvent(const State& state) const +{ vis.appendState(state); } - //============================================================================= /** Class ExponentialSpringTester provides a scope and framework for evaluating and testing the ExponentialSpringForce class. Using a class gets a lot of variables out of the global scope. */ -class ExponentialSpringTester { - +class ExponentialSpringTester +{ public: // Contact choices enum ContactChoice { @@ -173,9 +177,12 @@ class ExponentialSpringTester { // Model Creation void buildModel(); - OpenSim::Body* addBlock(const String& suffix); - //void addExponentialSprings(Model* model, OpenSim::Body* body); + OpenSim::Body* addBlock(const std::string& suffix); + void addExponentialSprings(OpenSim::Body* body); void addHuntCrossleyContact(OpenSim::Body* body); + void setForceData( + double t, const SimTK::Vec3& point, const SimTK::Vec3& force); + void setForceDataHeader(); void addDecorations(); // Simulation @@ -196,29 +203,63 @@ class ExponentialSpringTester { double tf{5.0}; // Command line options and their defaults - ContactChoice whichContact{HuntCross}; + ContactChoice whichContact{ExpSpr}; InitialConditionsChoice whichInit{Slide}; bool noDamp{false}; bool applyFx{false}; - bool showVisuals{true}; + bool showVisuals{false}; // Model and parts Model* model{NULL}; OpenSim::Body* blockES{NULL}; OpenSim::Body* blockHC{NULL}; + Storage fxData; + ExternalForce* fxES{NULL}; + ExternalForce* fxHC{NULL}; // Visualization TestExpSprVisuals* visuals{NULL}; }; // End class ExponentialSpringTester declarations +//_____________________________________________________________________________ +void +ExponentialSpringTester:: +addDecorations() +{ + // Ground + Ground& ground = model->updGround(); + SimTK::DecorativeBrick* floor = + new SimTK::DecorativeBrick(Vec3(2.0, 0.5, 2.0)); + floor->setColor(Green); + floor->setOpacity(0.1); + SimTK::Body& grndBody = ground.updMobilizedBody().updBody(); + grndBody.addDecoration(Transform(Vec3(0, -0.5, 0)), *floor); + // Exponential Springs Block + if (blockES) { + SimTK::Body& body = blockES->updMobilizedBody().updBody(); + SimTK::DecorativeBrick* brick = + new SimTK::DecorativeBrick(SimTK::Vec3(hs)); + brick->setColor(SimTK::Blue); + body.addDecoration(SimTK::Transform(), *brick); + } + + // Hunt-Crossley Block + if (blockHC) { + SimTK::Body& body = blockHC->updMobilizedBody().updBody(); + SimTK::DecorativeBrick* brick = + new SimTK::DecorativeBrick(SimTK::Vec3(hs)); + brick->setColor(SimTK::Red); + body.addDecoration(SimTK::Transform(), *brick); + } +} //_____________________________________________________________________________ int ExponentialSpringTester:: -parseCommandLine(int argc, char** argv) { - - string option; +parseCommandLine(int argc, char** argv) +{ + std::string option; for (int i = 1; i < argc; ++i) { option = argv[i]; @@ -248,7 +289,7 @@ parseCommandLine(int argc, char** argv) { noDamp = true; // Apply a horizontal ramping force - else if (option == "ApplyFx") + else if (option == "Fx") applyFx = true; // Show the visuals @@ -266,7 +307,8 @@ parseCommandLine(int argc, char** argv) { //_____________________________________________________________________________ void ExponentialSpringTester:: -printUsage() { +printUsage() +{ cout << endl << "Usage:" << endl; cout << "$ testExponetialSpring " << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; @@ -293,16 +335,16 @@ printUsage() { //_____________________________________________________________________________ // Build the model void -ExponentialSpringTester:: -buildModel() { - // Create the model(s) +ExponentialSpringTester::buildModel() +{ + // Create the bodies model = new Model(); model->setGravity(gravity); model->setName("TestExponentialSpring"); switch (whichContact) { case ExpSpr: blockES = addBlock("ES"); - //addExponentialSprings(model, blockES); + addExponentialSprings(blockES); break; case HuntCross: blockHC = addBlock("HC"); @@ -310,28 +352,86 @@ buildModel() { break; case Both: blockES = addBlock("ES"); - //addExponentialSprings(model, blockES); + addExponentialSprings(blockES); blockHC = addBlock("HC"); addHuntCrossleyContact(blockHC); } + + // Add the external force + if (applyFx) { + setForceDataHeader(); + SimTK::Vec3 point(0.0, -hs, 0.0); + SimTK::Vec3 zero(0.0); + SimTK::Vec3 force(-0.7*gravity[1]*mass, 0.0, 0.0); + setForceData(0.0, point, zero); + setForceData(tf, point, zero); + tf = tf + 10.0; + setForceData(tf, point, force); + if (blockES) { + cout << "Adding fx for " << blockES->getName() << endl; + fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); + fxES = new ExternalForce(fxData,"force", "point", "", + blockES->getName(), "ground", blockES->getName()); + model->addForce(fxES); + } + if (blockHC) { + cout << "Adding fx for " << blockHC->getName() << endl; + fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); + fxHC = new ExternalForce(fxData, "force", "point", "", + blockHC->getName(), "ground", blockHC->getName()); + model->addForce(fxHC); + } + } } //______________________________________________________________________________ -OpenSim::Body* +void ExponentialSpringTester::setForceDataHeader() +{ + fxData.setName("fx"); + fxData.setDescription("An external force applied to a block."); + Array lab; // labels + lab.append("time"); + lab.append("point.x"); + lab.append("point.y"); + lab.append("point.z"); + lab.append("force.x"); + lab.append("force.y"); + lab.append("force.z"); + fxData.setColumnLabels(lab); +} + +//______________________________________________________________________________ +void ExponentialSpringTester:: -addBlock(const String& suffix) { +setForceData(double t, const SimTK::Vec3& point, const SimTK::Vec3& force) +{ + SimTK::Vector_ data(6); + for (int i = 0; i < 3; ++i) { + data[i] = point[i]; + data[3 + i] = force[i]; + } + StateVector sv(t, data); + fxData.append(sv); +} +//______________________________________________________________________________ +OpenSim::Body* +ExponentialSpringTester:: +addBlock(const std::string& suffix) +{ Ground& ground = model->updGround(); // Body + std::string name = "block" + suffix; OpenSim::Body* block = new OpenSim::Body(); - block->setName("block"); + block->setName(name); block->set_mass(mass); block->set_mass_center(Vec3(0)); block->setInertia(Inertia(1.0)); // Joint + name = "free" + suffix; FreeJoint *free = new - FreeJoint("free", ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + FreeJoint(name, ground, Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); model->addBody(block); model->addJoint(free); @@ -339,9 +439,45 @@ addBlock(const String& suffix) { } //______________________________________________________________________________ void -ExponentialSpringTester:: -addHuntCrossleyContact(OpenSim::Body* block) { +ExponentialSpringTester +::addExponentialSprings(OpenSim::Body* block) +{ + Ground& ground = model->updGround(); + // Corners of the block + const int n = 8; + Vec3 corner[n]; + corner[0] = Vec3(hs, -hs, hs); + corner[1] = Vec3(hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3(hs, hs, hs); + corner[5] = Vec3(hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + + // Contact Plane Transform + Real angle = convertDegreesToRadians(90.0); + Rotation floorRot(-angle, XAxis); + Vec3 floorOrigin(0., -0.004, 0.); + Transform floorXForm(floorRot, floorOrigin); + + // Loop over the corners + std::string name = ""; + for (int i = 0; i < n; ++i) { + name = "ExpSpr" + std::to_string(i); + OpenSim::ExponentialSpringForce* force = + new OpenSim::ExponentialSpringForce(floorXForm, + block->getName(), corner[i]); + force->setName(name); + model->addForce(force); + } +} +//______________________________________________________________________________ +void +ExponentialSpringTester:: +addHuntCrossleyContact(OpenSim::Body* block) +{ Ground& ground = model->updGround(); // Geometry for the floor @@ -365,55 +501,27 @@ addHuntCrossleyContact(OpenSim::Body* block) { std::string name = ""; for (int i = 0; i < n; ++i) { // Geometry - name = "sphere" + std::to_string(i); + name = "sphere_" + std::to_string(i); OpenSim::ContactGeometry* geometry = new ContactSphere(0.005, corner[i], *block, name); model->addContactGeometry(geometry); // HuntCrossleyForce auto* contactParams = new OpenSim::HuntCrossleyForce:: - ContactParameters(1.0e7, 2e-1, 0.0, 0.0, 0.0); + ContactParameters(1.0e7, 4e-1, 0.7, 0.465, 0.0); contactParams->addGeometry(name); contactParams->addGeometry("floor"); - OpenSim::Force* force = new OpenSim::HuntCrossleyForce(contactParams); + OpenSim::HuntCrossleyForce* force = + new OpenSim::HuntCrossleyForce(contactParams); + name = "HuntCrossleyForce_" + std::to_string(i); + force->setName(name); + force->setTransitionVelocity(0.01); model->addForce(force); } } //_____________________________________________________________________________ void ExponentialSpringTester:: -addDecorations() { - - // Ground - Ground& ground = model->updGround(); - SimTK::DecorativeBrick* floor = - new SimTK::DecorativeBrick(Vec3(2.0, 0.5, 2.0)); - floor->setColor(Green); - floor->setOpacity(0.1); - SimTK::Body& grndBody = ground.updMobilizedBody().updBody(); - grndBody.addDecoration(Transform(Vec3(0, -0.5, 0)), *floor); - - // Exponential Springs Block - if (blockES) { - SimTK::Body& body = blockES->updMobilizedBody().updBody(); - SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(hs)); - brick->setColor(SimTK::Blue); - body.addDecoration(SimTK::Transform(), *brick); - } - - // Hunt-Crossley Block - if (blockHC) { - SimTK::Body& body = blockHC->updMobilizedBody().updBody(); - SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(hs)); - brick->setColor(SimTK::Red); - body.addDecoration(SimTK::Transform(), *brick); - } -} -//_____________________________________________________________________________ -void -ExponentialSpringTester:: setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz) { @@ -435,15 +543,15 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, case Slide: pos[0] = 2.0; pos[1] = 2.0 * hs; - vel[0] = -2.0; + vel[0] = -4.0; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); break; case SpinSlide: pos[0] = 2.0; pos[1] = 2.0 * hs; - vel[0] = -2.0; - angvel[1] = 2.0 * SimTK::Pi; + vel[0] = -4.0; + angvel[1] = 4.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); body.setUToFitAngularVelocity(state, angvel); @@ -451,8 +559,8 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, case Tumble: pos[0] = 2.0; pos[1] = 2.0 * hs; - vel[0] = -2.0; - angvel[2] = 2.0 * SimTK::Pi; + vel[0] = -4.0; + angvel[2] = 4.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); body.setUToFitAngularVelocity(state, angvel); @@ -464,11 +572,24 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, } //_____________________________________________________________________________ void -ExponentialSpringTester:: -simulate() { +ExponentialSpringTester::simulate() +{ + // Visuals? + if (showVisuals) { + if (blockES) { + auto blockESGeometry = new Brick(Vec3(hs)); + blockESGeometry->setColor(Vec3(0.1, 0.1, 0.8)); + blockES->attachGeometry(blockESGeometry); + } + if (blockHC) { + auto blockHCGeometry = new Brick(Vec3(hs)); + blockHCGeometry->setColor(Vec3(0.8, 0.1, 0.1)); + blockHC->attachGeometry(blockHCGeometry); + } + model->setUseVisualizer(true); + } // Initialize the System - if(showVisuals) model->setUseVisualizer(false); SimTK::State& state = model->initSystem(); // Set initial conditions @@ -477,27 +598,32 @@ simulate() { setInitialConditions(state, blockES->getMobilizedBody(), dz); if (blockHC != NULL) setInitialConditions(state, blockHC->getMobilizedBody(), -dz); - cout << "state =" << state << std::endl; - - // Visuals Start - if(visuals) visuals->pollForStart(); + // cout << "state =" << state << std::endl; // Integrate Manager manager(*model); + manager.getIntegrator().setMaximumStepSize(0.02); manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); - // start timing std::clock_t startTime = std::clock(); state = manager.integrate(tf); auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; cout << "simulation time = " << runTime << " msec" << endl; + // Trys and Steps + int trys = manager.getIntegrator().getNumStepsAttempted(); + int steps = manager.getIntegrator().getNumStepsTaken(); + cout << "trys = " << trys << "\t\tsteps = " << steps << endl; + // Visuals Replay - if (visuals) visuals->pollForReplay(); + //if (showVisuals) { + // //visuals->pollForReplay(); + // auto vis = model->getVisualizer(); + // vis.show(state); + //} } - //_____________________________________________________________________________ /* Entry Point (i.e., main()) From 94f1b8171c75d1d429ec09a0a77b0866776d7e2d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 27 May 2022 22:15:11 -0500 Subject: [PATCH 09/55] Minor Changes + Bug Fixes - Check for nullptr before disowning and deleting model. - Changed SpinSlide into just Spin. - Tweaked initial conditions. --- .../Simulation/Test/testExponentialSpring.cpp | 42 +++++++++++-------- 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index c640b7625d..436906d12c 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -158,7 +158,7 @@ class ExponentialSpringTester Static = 0, Bounce, Slide, - SpinSlide, + Spin, Tumble }; @@ -167,8 +167,8 @@ class ExponentialSpringTester // Destructor ~ExponentialSpringTester() { - model->disownAllComponents(); - delete model; + if(model) model->disownAllComponents(); + if(model) delete model; } // Command line parsing and usage @@ -279,8 +279,8 @@ parseCommandLine(int argc, char** argv) whichInit = Bounce; else if (option == "Slide") whichInit = Slide; - else if (option == "SpinSlide") - whichInit = SpinSlide; + else if (option == "Spin") + whichInit = Spin; else if (option == "Tumble") whichInit = Tumble; @@ -312,7 +312,7 @@ printUsage() cout << endl << "Usage:" << endl; cout << "$ testExponetialSpring " << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; - cout << "\tInitCond (choose one): Static Bounce Slide SpinSlide Tumble"; + cout << "\tInitCond (choose one): Static Bounce Slide Spin Tumble "; cout << endl; cout << "\t Contact (choose one): ExpSpr HuntCross Both" << endl << endl; @@ -365,13 +365,14 @@ ExponentialSpringTester::buildModel() SimTK::Vec3 force(-0.7*gravity[1]*mass, 0.0, 0.0); setForceData(0.0, point, zero); setForceData(tf, point, zero); - tf = tf + 10.0; + tf = tf + 25.0; setForceData(tf, point, force); if (blockES) { cout << "Adding fx for " << blockES->getName() << endl; fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); fxES = new ExternalForce(fxData,"force", "point", "", blockES->getName(), "ground", blockES->getName()); + fxES->setName("externalforceES"); model->addForce(fxES); } if (blockHC) { @@ -379,6 +380,7 @@ ExponentialSpringTester::buildModel() fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); fxHC = new ExternalForce(fxData, "force", "point", "", blockHC->getName(), "ground", blockHC->getName()); + fxHC->setName("externalforceHC"); model->addForce(fxHC); } } @@ -515,7 +517,7 @@ addHuntCrossleyContact(OpenSim::Body* block) new OpenSim::HuntCrossleyForce(contactParams); name = "HuntCrossleyForce_" + std::to_string(i); force->setName(name); - force->setTransitionVelocity(0.01); + force->setTransitionVelocity(0.001); model->addForce(force); } } @@ -532,7 +534,7 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, switch (whichInit) { case Static: pos[0] = 0.0; - pos[1] = hs + 0.004; + pos[1] = hs; body.setQToFitTranslation(state, pos); break; case Bounce: @@ -547,20 +549,20 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); break; - case SpinSlide: - pos[0] = 2.0; - pos[1] = 2.0 * hs; - vel[0] = -4.0; + case Spin: + pos[0] = 0.0; + pos[1] = hs; + vel[0] = 0.0; angvel[1] = 4.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); body.setUToFitAngularVelocity(state, angvel); break; case Tumble: - pos[0] = 2.0; + pos[0] = -1.5; pos[1] = 2.0 * hs; - vel[0] = -4.0; - angvel[2] = 4.0 * SimTK::Pi; + vel[0] = -1.0; + angvel[2] = 2.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); body.setUToFitAngularVelocity(state, angvel); @@ -572,7 +574,8 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, } //_____________________________________________________________________________ void -ExponentialSpringTester::simulate() +ExponentialSpringTester:: +simulate() { // Visuals? if (showVisuals) { @@ -679,7 +682,10 @@ int main(int argc, char** argv) { try { ExponentialSpringTester tester; int status = tester.parseCommandLine(argc, argv); - if (status<0) return 1; + if (status < 0) { + cout << "Exiting..." << endl; + return 1; + } tester.buildModel(); tester.simulate(); From b5812da25a31c7deec3ceb8bebe5615841f545f4 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 5 Jun 2022 00:40:52 -0500 Subject: [PATCH 10/55] Added Parameters. Changed name. I changed the class name from OpenSim::ExponentialSpringForce, which might have been confused with SimTK::ExponentialSpringForce, to OpenSim::ExponentialContact. I also added the topology-stage parameters. On the SimTK side, they are managed by SimTK::ExponentialSpringParameters. I tried not to reproduce functionality on the OpenSim side, but to make use of SimTK::ExponentialSpringParameters as much as possible. OpenSim::ExponentialContact has its own "Parameters" class that encapsulates a SimTK::ExponentialSpringParameters instance. The main purpose of OpenSim::Parameters is to establish and manage all of the OpenSim Properties that correspond to the member variables in SimTK::ExponentialSpringParameters. --- .../Model/ExponentialSpringForce.cpp | 219 +++++++++++++----- .../Simulation/Model/ExponentialSpringForce.h | 183 +++++++++++---- .../Simulation/Test/testExponentialSpring.cpp | 13 +- 3 files changed, 312 insertions(+), 103 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp index 9f21fc2d05..5d4fb1b03e 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -21,72 +21,174 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include "ExponentialSpringForce.h" #include "Model.h" - -#include "simbody/internal/ExponentialSpringForce.h" +#include "ExponentialSpringForce.h" using namespace OpenSim; using namespace std; //============================================================================= -// Class ExponentialSpringForce +// ExponentialContact::Parameters //============================================================================= +//_____________________________________________________________________________ +ExponentialContact::Parameters:: +Parameters() +{ + constructProperties(); +} +//_____________________________________________________________________________ +ExponentialContact::Parameters:: +Parameters(const SimTK::ExponentialSpringParameters& params) +{ + _stkparams = params; + constructProperties(); +} +//_____________________________________________________________________________ +void +ExponentialContact::Parameters:: +constructProperties() +{ + SimTK::Vec3 shape; + _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); + constructProperty_exponential_shape_parameters(shape); + constructProperty_normal_viscosity(_stkparams.getNormalViscosity()); + constructProperty_max_normal_force(_stkparams.getMaxNormalForce()); + constructProperty_friction_elasticity(_stkparams.getFrictionElasticity()); + constructProperty_friction_viscocity(_stkparams.getFrictionViscosity()); + constructProperty_sliding_time_constant(_stkparams.getSlidingTimeConstant()); + constructProperty_settle_velocity(_stkparams.getSettleVelocity()); + constructProperty_settle_acceleration(_stkparams.getSettleAcceleration()); + constructProperty_initial_mu_static(_stkparams.getInitialMuStatic()); + constructProperty_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); +} +//_____________________________________________________________________________ +// Update the Properties based on the SimTK::ExponentialSpringParameters. +void +ExponentialContact::Parameters:: +updateProperties() +{ + SimTK::Vec3 shape; + _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); + set_exponential_shape_parameters(shape); + set_normal_viscosity(_stkparams.getNormalViscosity()); + set_max_normal_force(_stkparams.getMaxNormalForce()); + set_friction_elasticity(_stkparams.getFrictionElasticity()); + set_friction_viscocity(_stkparams.getFrictionViscosity()); + set_sliding_time_constant(_stkparams.getSlidingTimeConstant()); + set_settle_velocity(_stkparams.getSettleVelocity()); + set_settle_acceleration(_stkparams.getSettleAcceleration()); + set_initial_mu_static(_stkparams.getInitialMuStatic()); + set_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); +} +//_____________________________________________________________________________ +// Update the SimTK::ExponentialSpringParamters based on the Properties. +void +ExponentialContact::Parameters:: +updateParameters() +{ + const SimTK::Vec3 shape = get_exponential_shape_parameters(); + _stkparams.setShapeParameters(shape[0], shape[1], shape[2]); + _stkparams.setNormalViscosity(get_normal_viscosity()); + _stkparams.setMaxNormalForce(get_max_normal_force()); + _stkparams.setFrictionElasticity(get_friction_elasticity()); + _stkparams.setFrictionViscosity(get_friction_viscocity()); + _stkparams.setSlidingTimeConstant(get_sliding_time_constant()); + _stkparams.setSettleVelocity(get_settle_velocity()); + _stkparams.setSettleAcceleration(get_settle_acceleration()); + _stkparams.setInitialMuStatic(get_initial_mu_static()); + _stkparams.setInitialMuKinetic(get_initial_mu_kinetic()); +} +//_____________________________________________________________________________ +void +ExponentialContact::Parameters:: +updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) +{ + Super::updateFromXMLNode(node, versionNumber); + updateParameters(); +} +//_____________________________________________________________________________ +// Note that the OpenSim Properties are updated as well. +void +ExponentialContact::Parameters:: +setSimTKParameters(const SimTK::ExponentialSpringParameters& params) +{ + _stkparams = params; + updateProperties(); +} +//_____________________________________________________________________________ +// Get a read-only reference to the SimTK::ExponentialSpringParamters held +// by this instance. +const SimTK::ExponentialSpringParameters& +ExponentialContact::Parameters:: +getSimTKParameters() +{ + return _stkparams; +} -//----------------------------------------------------------------------------- -// Construction -//----------------------------------------------------------------------------- + + +//============================================================================= +// ExponentialContact +//============================================================================= //_____________________________________________________________________________ -ExponentialSpringForce::ExponentialSpringForce() +ExponentialContact::ExponentialContact() { setNull(); constructProperties(); } //_____________________________________________________________________________ -ExponentialSpringForce:: -ExponentialSpringForce(const SimTK::Transform& contactPlaneXform, - const std::string& bodyName, const SimTK::Vec3& station) +ExponentialContact:: +ExponentialContact(const SimTK::Transform& contactPlaneXform, + const std::string& bodyName, const SimTK::Vec3& station, + SimTK::ExponentialSpringParameters params) { setNull(); constructProperties(); - setContactPlaneTransform(contactPlaneXform); setBodyName(bodyName); setBodyStation(station); } //_____________________________________________________________________________ -void ExponentialSpringForce::setNull() { setAuthors("F. C. Anderson"); } - -//----------------------------------------------------------------------------- -// Properties -//----------------------------------------------------------------------------- -// Questions: -// 1. Is it ok not to have a default constructor? There really isn't a -// situation in which a default constructor makes sense. There needs to -// be a body to which the force is applied. I suppose the default constructor -// could apply a force between Ground and Ground. +ExponentialContact:: +ExponentialContact(const ExponentialContact& source) +{ + setNull(); + constructProperties(); + setContactPlaneTransform(source.getContactPlaneTransform()); + setBodyName(source.getBodyName()); + setBodyStation(source.getBodyStation()); +} +//_____________________________________________________________________________ +void +ExponentialContact:: +setNull() +{ + setAuthors("F. C. Anderson"); + _spr = NULL; +} //_____________________________________________________________________________ void -ExponentialSpringForce:: +ExponentialContact:: constructProperties() { SimTK::Transform contactXForm; + SimTK::Vec3 origin(0.0); + Parameters params; constructProperty_contact_plane_transform(contactXForm); - constructProperty_body_name(""); - - SimTK::Vec3 origin(0.0); constructProperty_body_station(origin); + constructProperty_contact_parameters(params); +} +//_____________________________________________________________________________ +void +ExponentialContact +::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { + Super::updateFromXMLNode(node, versionNumber); } - - -//----------------------------------------------------------------------------- -// CONNECT TO THE MODEL -//----------------------------------------------------------------------------- //_____________________________________________________________________________ void -ExponentialSpringForce:: +ExponentialContact:: extendConnectToModel(OpenSim::Model& model) { // Allow based class to connect first @@ -100,56 +202,61 @@ extendConnectToModel(OpenSim::Model& model) _body = &(getModel().getComponent( "./bodyset/" + bodyName)); } - -//----------------------------------------------------------------------------- -// ADD TO THE MULTIBODY SYSTEM -//----------------------------------------------------------------------------- -// Questions: -// 1. Does "spr" not need to be allocated from the heap? -// 2. What happens to the force subsystem when spr goes out of scope? -// -// Guesses: -// 1. The GeneralForceSubsystem makes a clone of spr in a way that -// keeps references to it still valid. -// //_____________________________________________________________________________ void -ExponentialSpringForce:: +ExponentialContact:: extendAddToSystem(SimTK::MultibodySystem& system) const { // Extend the OpenSim::Force parent Super::extendAddToSystem(system); - // Construct the SimTK::ExponentialSpringForce object + // Construct the SimTK::ExponentialContact object SimTK::GeneralForceSubsystem& forces = _model->updForceSubsystem(); const SimTK::Transform& XContactPlane = get_contact_plane_transform(); const SimTK::Vec3& station = get_body_station(); - SimTK::ExponentialSpringForce - spr(forces, XContactPlane, _body->getMobilizedBody(), station); + SimTK::ExponentialSpringForce* spr = + new SimTK::ExponentialSpringForce(forces, XContactPlane, + _body->getMobilizedBody(), station, getParameters()); // Get the subsystem index so we can access the SimTK::Force later. - ExponentialSpringForce* mutableThis = - const_cast(this); - mutableThis->_index = spr.getForceIndex(); + ExponentialContact* mutableThis = + const_cast(this); + mutableThis->_spr = spr; + mutableThis->_index = spr->getForceIndex(); } - //----------------------------------------------------------------------------- // ACCESSORS //----------------------------------------------------------------------------- //_____________________________________________________________________________ -void ExponentialSpringForce:: +void ExponentialContact:: setContactPlaneTransform(const SimTK::Transform& XContactPlane) { set_contact_plane_transform(XContactPlane); } //_____________________________________________________________________________ const SimTK::Transform& -ExponentialSpringForce::getContactPlaneTransform() const +ExponentialContact::getContactPlaneTransform() const { return get_contact_plane_transform(); } +//_____________________________________________________________________________ +void +ExponentialContact:: +setParameters(const SimTK::ExponentialSpringParameters& params) +{ + ExponentialContact::Parameters& p = upd_contact_parameters(); + p.setSimTKParameters(params); + if (_spr != NULL) _spr->setParameters(params); +} +//_____________________________________________________________________________ +const SimTK::ExponentialSpringParameters& +ExponentialContact:: +getParameters() const +{ + return _spr->getParameters(); +} //----------------------------------------------------------------------------- // Reporting @@ -159,7 +266,7 @@ ExponentialSpringForce::getContactPlaneTransform() const // will be treated as an OpenSim::Force? //_____________________________________________________________________________ OpenSim::Array -ExponentialSpringForce:: +ExponentialContact:: getRecordLabels() const { OpenSim::Array labels(""); @@ -172,7 +279,7 @@ getRecordLabels() const } //_____________________________________________________________________________ OpenSim::Array -ExponentialSpringForce:: +ExponentialContact:: getRecordValues(const SimTK::State& state) const { OpenSim::Array values(1); diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialSpringForce.h index a7113cb074..28382a3753 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.h +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.h @@ -1,7 +1,7 @@ -#ifndef OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ -#define OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ +#ifndef OPENSIM_EXPONENTIAL_CONTACT_H_ +#define OPENSIM_EXPONENTIAL_CONTACT_H_ /* -------------------------------------------------------------------------- * - * OpenSim: HuntCrossleyForce.h * + * OpenSim: ExponentialContactForce.h * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * * See http://opensim.stanford.edu and the NOTICE file for more information. * @@ -9,8 +9,8 @@ * 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): Peter Eastman * + * Copyright (c) 2022 Stanford University and the Authors * + * Author(s): F. C. Anderson * * * * 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 * @@ -23,49 +23,65 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ // INCLUDE +#include "SimTKsimbody.h" #include "Force.h" #include "OpenSim/Common/Set.h" namespace OpenSim { -//============================================================================== -// EXPONENTIAL SPRING FORCE -//============================================================================== -/** This force subclass implements an ExponentialSpringForce to model contact -of a specified point on a body (i.e., a "station" in Simbody vocabulary) with -a contact plane that is anchored to Ground. + +//============================================================================= +// ExponentialContact +//============================================================================= +/** This force subclass implements a SimTK::ExponentialSpringForce to model +contact of a specified point on a body (i.e., a "station" in Simbody vocabulary) with +a contact plane that is fixed to Ground. @author F. C. Anderson **/ -class OSIMSIMULATION_API ExponentialSpringForce : public Force { - OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialSpringForce, Force); +class OSIMSIMULATION_API ExponentialContact : public Force { + OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialContact, Force); public: - //============================================================================== + class Parameters; + + //------------------------------------------------------------------------- // PROPERTIES - //============================================================================== + //------------------------------------------------------------------------- OpenSim_DECLARE_PROPERTY(contact_plane_transform, SimTK::Transform, - "Rotation and translation of the contact plane wrt Ground."); + "Location and orientation of the contact plane wrt Ground. \ + The positive z-axis of the contact plane defines the normal."); OpenSim_DECLARE_PROPERTY(body_name, std::string, - "Name of the Body to which the force is applied."); + "Name of the Body to which the resultant contact force is applied."); OpenSim_DECLARE_PROPERTY(body_station, SimTK::Vec3, - "Point on the body at which the force is applied."); - - //______________________________________________________________________________ + "Point on the body, expressed in the Body frame, at which the \ + resultant contact force is applied."); + OpenSim_DECLARE_PROPERTY(contact_parameters, ExponentialContact::Parameters, + "Customizable topology-stage parameters."); + + //------------------------------------------------------------------------- + // Construction + //------------------------------------------------------------------------- /** Default constructor. */ - ExponentialSpringForce(); + ExponentialContact(); - //______________________________________________________________________________ - /** Construct an ExponentialSpringForce. + /** Construct an ExponentialContact. @param XContactPlane Transform specifying the location and orientation of the contact plane in Ground. @param bodyName Name of the body to which the force is applied. - @param station Point on the body at which the force is applied. */ - explicit ExponentialSpringForce(const SimTK::Transform& contactPlaneXform, - const std::string& bodyName, const SimTK::Vec3& station); - - //----------------------------------------------------------------------------- + @param station Point on the body at which the force is applied. + @param params Optional parameters object used to customize the + topology-stage characteristics of the contact model. */ + explicit ExponentialContact(const SimTK::Transform& contactPlaneXform, + const std::string& bodyName, const SimTK::Vec3& station, + SimTK::ExponentialSpringParameters params = + SimTK::ExponentialSpringParameters()); + + /** Copy constructor. */ + ExponentialContact(const ExponentialContact& other); + + //------------------------------------------------------------------------- // Accessors - //----------------------------------------------------------------------------- + //------------------------------------------------------------------------- /** Set the tranform that specifies the location and orientation of the contact plane in the Ground frame. */ void setContactPlaneTransform(const SimTK::Transform& contactPlaneXform); @@ -79,15 +95,23 @@ class OSIMSIMULATION_API ExponentialSpringForce : public Force { const std::string& getBodyName() const { return get_body_name(); } /** Set the point on the body at which the force is applied. */ - void setBodyStation(SimTK::Vec3 station) { set_body_station(station); } + void setBodyStation(const SimTK::Vec3& station) { set_body_station(station); } /** Get the point on the body at which the force is applied. */ - const SimTK::Vec3& getBodyStation() { return get_body_station(); } + const SimTK::Vec3& getBodyStation() const { return get_body_station(); } - //----------------------------------------------------------------------------- + /** Set the customizable Topology-stage spring parameters. Calling this + method will invalidate the SimTK::System at Stage::Toplogy. The System + must therefore be realized at Stage::Topology, as well subsequent stages + when appropriate, before simulation or analysis can proceed. */ + void setParameters(const SimTK::ExponentialSpringParameters& params); + /** Get the customizable Topology-stage spring parameters. */ + const SimTK::ExponentialSpringParameters& getParameters() const; + + //------------------------------------------------------------------------- // Reporting - //----------------------------------------------------------------------------- - /** Provide name(s) of the quantities (column labels) of the value(s) to be - reported. */ + //------------------------------------------------------------------------- + /** Provide name(s) of the quantities (column labels) of the value(s) + to be reported. */ OpenSim::Array getRecordLabels() const override; /** Provide the value(s) to be reported that correspond to the labels. */ OpenSim::Array getRecordValues( @@ -97,20 +121,97 @@ class OSIMSIMULATION_API ExponentialSpringForce : public Force { /** Connect to the OpenSim Model. */ void extendConnectToModel(Model& model) override; - /** Create a SimTK::ExponentialSpringForce that implements this Force. */ + /** Create a SimTK::ExponentialContact that implements this Force. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; + /** Update this Object base on an XML node. */ + void updateFromXMLNode(SimTK::Xml::Element& node, + int versionNumber) override; + private: void setNull(); void constructProperties(); - - // Temporary solution until implemented with Sockets SimTK::ReferencePtr _body; + SimTK::ExponentialSpringForce* _spr{NULL}; -}; // END of class ExponentialSpringForce +}; // END of class ExponentialContact -} // end of namespace OpenSim -#endif // OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ +//============================================================================= +// ExponentialContact::Parameters +//============================================================================= +/** This subclass manages the topology-stage parameters that are available +for customizing the characteristics of an ExponentialContact object. More +specifically, it has two purposes: + +- Initialization. Before model initialization there is no underlying +SimTK::ExponentialSpringForce object upon which to set non-default parameters. +This class serves as a storage mechanism so that, upon model initialization, +parameters can be pushed to the underlying SimTK::ExponentialSpringForce +instance. + +- Property Maintenance. This class maintains consistence between the +parameters of the underlying SimTK::ExponentialSpringForce inst ance and their +corresponding OpenSim Properties. The local SimTK::ExponentialSpringParameters +member variable (_stkparams) provides a way to ensure the Properties hold +valid values. Before any properties are updated, new parameters are set +on _stkparams. +@author F. C. Anderson **/ +class ExponentialContact::Parameters : public Object { + OpenSim_DECLARE_CONCRETE_OBJECT(ExponentialContact::Parameters, Object); + +public: + OpenSim_DECLARE_PROPERTY(exponential_shape_parameters, SimTK::Vec3, + "Shape parameters for the exponential that is used to model the \ + normal force: d0 (0.0065905 m), d1 (0.5336 N), d2 (1150.0/m)."); + OpenSim_DECLARE_PROPERTY(normal_viscosity, double, + "Viscosity in the normal direction (0.5 s/m)."); + OpenSim_DECLARE_PROPERTY(max_normal_force, double, + "Maximum allowed normal force (100,000.0 N)."); + OpenSim_DECLARE_PROPERTY(friction_elasticity, double, + "Elasticity of the linear friction spring (20,000.0 N/m)."); + OpenSim_DECLARE_PROPERTY(friction_viscocity, double, + "Viscosity of the linear friction spring (282.8427 N*s/m)."); + OpenSim_DECLARE_PROPERTY(sliding_time_constant, double, + "Time constant for rise/decay between static and kinetic friction \ + conditions (0.01 s)."); + OpenSim_DECLARE_PROPERTY(settle_velocity, double, + "Velocity below which static friction conditions are triggered \ + to come into effect (0.01 m/s) ."); + OpenSim_DECLARE_PROPERTY(settle_acceleration, double, + "Acceleration below which static friction conditions are triggered \ + to come into effect (1.0 m/s^2)."); + OpenSim_DECLARE_PROPERTY(initial_mu_static, double, + "Initial value of the static coefficient of friction."); + OpenSim_DECLARE_PROPERTY(initial_mu_kinetic, double, + "Initial value of the kinetic coefficient of friction."); + /** Default constructor. */ + Parameters(); + + /** Construct an instance based on a SimTK::ExponentialSpringParameters + object. */ + Parameters(const SimTK::ExponentialSpringParameters& params); + + /** Set the underlying SimTK::ExponentialSpringParameters. */ + void setSimTKParameters(const SimTK::ExponentialSpringParameters& params); + + /** Get a read-only reference to the underlying + SimTK::ExponentialSpringParameters. To alter the parameters, use + setParameters(). */ + const SimTK::ExponentialSpringParameters& getSimTKParameters(); + +private: + void setNull(); + void constructProperties(); + void updateParameters(); + void updateProperties(); + void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber); + SimTK::ExponentialSpringParameters _stkparams; +}; + + +} // end of namespace OpenSim + +#endif // OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index 436906d12c..56b5fa40d0 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -468,8 +468,8 @@ ::addExponentialSprings(OpenSim::Body* block) std::string name = ""; for (int i = 0; i < n; ++i) { name = "ExpSpr" + std::to_string(i); - OpenSim::ExponentialSpringForce* force = - new OpenSim::ExponentialSpringForce(floorXForm, + OpenSim::ExponentialContact* force = + new OpenSim::ExponentialContact(floorXForm, block->getName(), corner[i]); force->setName(name); model->addForce(force); @@ -517,7 +517,7 @@ addHuntCrossleyContact(OpenSim::Body* block) new OpenSim::HuntCrossleyForce(contactParams); name = "HuntCrossleyForce_" + std::to_string(i); force->setName(name); - force->setTransitionVelocity(0.001); + force->setTransitionVelocity(0.01); model->addForce(force); } } @@ -601,7 +601,6 @@ simulate() setInitialConditions(state, blockES->getMobilizedBody(), dz); if (blockHC != NULL) setInitialConditions(state, blockHC->getMobilizedBody(), -dz); - // cout << "state =" << state << std::endl; // Integrate Manager manager(*model); @@ -612,12 +611,14 @@ simulate() std::clock_t startTime = std::clock(); state = manager.integrate(tf); auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; - cout << "simulation time = " << runTime << " msec" << endl; // Trys and Steps int trys = manager.getIntegrator().getNumStepsAttempted(); int steps = manager.getIntegrator().getNumStepsTaken(); - cout << "trys = " << trys << "\t\tsteps = " << steps << endl; + + // Output + cout << "trys = " << trys << "\t\tsteps = " << steps; + cout << "\t\tcpu time = " << runTime << endl; // Visuals Replay //if (showVisuals) { From 8f339cd86fe122de1d3a4b34f86684f45a96c3d8 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 5 Jun 2022 08:51:28 -0500 Subject: [PATCH 11/55] Bug fix, plus minor tweaks Bug Fix- ExponentialContact::getParameters() can be called before the Model is initialized. The resolution of the call should therefore go through the ExponentialContact::Parameters instance and not the pointer (_spr) to theunderlying SimTK::ExponentialSpringForce instance. Minor tweaks included the addition of a "const" qualifier and some comments. --- OpenSim/Simulation/Model/ExponentialSpringForce.cpp | 6 ++++-- OpenSim/Simulation/Model/ExponentialSpringForce.h | 2 +- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp index 5d4fb1b03e..40ed51e2c0 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -121,7 +121,7 @@ setSimTKParameters(const SimTK::ExponentialSpringParameters& params) // by this instance. const SimTK::ExponentialSpringParameters& ExponentialContact::Parameters:: -getSimTKParameters() +getSimTKParameters() const { return _stkparams; } @@ -148,6 +148,7 @@ ExponentialContact(const SimTK::Transform& contactPlaneXform, setContactPlaneTransform(contactPlaneXform); setBodyName(bodyName); setBodyStation(station); + setParameters(params); } //_____________________________________________________________________________ ExponentialContact:: @@ -248,6 +249,7 @@ setParameters(const SimTK::ExponentialSpringParameters& params) { ExponentialContact::Parameters& p = upd_contact_parameters(); p.setSimTKParameters(params); + // The following call will invalidate the State at Stage::Topology if (_spr != NULL) _spr->setParameters(params); } //_____________________________________________________________________________ @@ -255,7 +257,7 @@ const SimTK::ExponentialSpringParameters& ExponentialContact:: getParameters() const { - return _spr->getParameters(); + return get_contact_parameters().getSimTKParameters(); } //----------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialSpringForce.h index 28382a3753..e8cad2d5e8 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.h +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.h @@ -200,7 +200,7 @@ class ExponentialContact::Parameters : public Object { /** Get a read-only reference to the underlying SimTK::ExponentialSpringParameters. To alter the parameters, use setParameters(). */ - const SimTK::ExponentialSpringParameters& getSimTKParameters(); + const SimTK::ExponentialSpringParameters& getSimTKParameters() const; private: void setNull(); From 6a9563fce3744a86ebd62fcaa37daa180970c859 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 7 Jun 2022 09:43:19 -0500 Subject: [PATCH 12/55] Non-default params working + comments and tweaks. testExponentialSpring.cpp is now working with default and non-default contact parameters. Testing is the exhaustive, but some basic scenarios work. I also added some comments and made some small polishing tweaks to the code. --- .../Model/ExponentialSpringForce.cpp | 27 +++++++-- .../Simulation/Model/ExponentialSpringForce.h | 55 +++++++++++-------- .../Simulation/Test/testExponentialSpring.cpp | 13 ++++- 3 files changed, 66 insertions(+), 29 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp index 40ed51e2c0..ccfc84bd83 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.cpp @@ -35,16 +35,23 @@ using namespace std; ExponentialContact::Parameters:: Parameters() { + setNull(); constructProperties(); } //_____________________________________________________________________________ ExponentialContact::Parameters:: Parameters(const SimTK::ExponentialSpringParameters& params) { + setNull(); _stkparams = params; constructProperties(); } //_____________________________________________________________________________ +void ExponentialContact::Parameters::setNull() +{ + setAuthors("F. C. Anderson"); +} +//_____________________________________________________________________________ void ExponentialContact::Parameters:: constructProperties() @@ -100,12 +107,23 @@ updateParameters() _stkparams.setInitialMuKinetic(get_initial_mu_kinetic()); } //_____________________________________________________________________________ +// This method is needed to ensure that the SimTK::ExponentialSpringParameters +// member variable (_stkparam) is kept consistent with the properties. +// It is necessary to have the _stkparams member variable because there needs +// to be a place to store non-default parameters when the underlying +// SimTK::ExponentialSpringForce hasn't been instantiated. +// Having to do a little extra bookkeeping (i.e., storing properties values +// twice [once in the Properties and once in _stkparams]) is justified +// by not having to rewrite a whole bunch of additional accessor methods. +// All parameter set/gets go through the SimTK::ExponentialSpringParameters +// interface (i.e., through _stkparams). void ExponentialContact::Parameters:: updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { Super::updateFromXMLNode(node, versionNumber); - updateParameters(); + updateParameters(); // catching any invalid property values in the process + updateProperties(); // pushes valid parameters back to the properties. } //_____________________________________________________________________________ // Note that the OpenSim Properties are updated as well. @@ -127,7 +145,6 @@ getSimTKParameters() const } - //============================================================================= // ExponentialContact //============================================================================= @@ -151,6 +168,7 @@ ExponentialContact(const SimTK::Transform& contactPlaneXform, setParameters(params); } //_____________________________________________________________________________ +/* ExponentialContact:: ExponentialContact(const ExponentialContact& source) { @@ -160,6 +178,7 @@ ExponentialContact(const ExponentialContact& source) setBodyName(source.getBodyName()); setBodyStation(source.getBodyStation()); } +*/ //_____________________________________________________________________________ void ExponentialContact:: @@ -249,7 +268,7 @@ setParameters(const SimTK::ExponentialSpringParameters& params) { ExponentialContact::Parameters& p = upd_contact_parameters(); p.setSimTKParameters(params); - // The following call will invalidate the State at Stage::Topology + // The following call will invalidate the System at Stage::Topology if (_spr != NULL) _spr->setParameters(params); } //_____________________________________________________________________________ @@ -264,7 +283,7 @@ getParameters() const // Reporting //----------------------------------------------------------------------------- // Questions: -// 1. Do I need to conform to a certain reporting format since this object +// 1. Do I need to conform to a certain reporting format because this object // will be treated as an OpenSim::Force? //_____________________________________________________________________________ OpenSim::Array diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialSpringForce.h index e8cad2d5e8..91042b0fe7 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.h +++ b/OpenSim/Simulation/Model/ExponentialSpringForce.h @@ -33,9 +33,9 @@ namespace OpenSim { //============================================================================= // ExponentialContact //============================================================================= -/** This force subclass implements a SimTK::ExponentialSpringForce to model -contact of a specified point on a body (i.e., a "station" in Simbody vocabulary) with -a contact plane that is fixed to Ground. +/** This OpenSim::Force subclass implements a SimTK::ExponentialSpringForce +to model contact of a specified point on a body (i.e., a "station" in Simbody +vocabulary) with a contact plane that is fixed to Ground. @author F. C. Anderson **/ class OSIMSIMULATION_API ExponentialContact : public Force { @@ -77,7 +77,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { SimTK::ExponentialSpringParameters()); /** Copy constructor. */ - ExponentialContact(const ExponentialContact& other); + //ExponentialContact(const ExponentialContact& other); //------------------------------------------------------------------------- // Accessors @@ -94,15 +94,15 @@ class OSIMSIMULATION_API ExponentialContact : public Force { /** Get the name of the body to which this force is applied. */ const std::string& getBodyName() const { return get_body_name(); } - /** Set the point on the body at which the force is applied. */ + /** Set the point on the body at which this force is applied. */ void setBodyStation(const SimTK::Vec3& station) { set_body_station(station); } - /** Get the point on the body at which the force is applied. */ + /** Get the point on the body at which this force is applied. */ const SimTK::Vec3& getBodyStation() const { return get_body_station(); } /** Set the customizable Topology-stage spring parameters. Calling this method will invalidate the SimTK::System at Stage::Toplogy. The System - must therefore be realized at Stage::Topology, as well subsequent stages - when appropriate, before simulation or analysis can proceed. */ + must therefore be realized at Stage::Topology before simulation or + analysis can proceed. */ void setParameters(const SimTK::ExponentialSpringParameters& params); /** Get the customizable Topology-stage spring parameters. */ const SimTK::ExponentialSpringParameters& getParameters() const; @@ -140,22 +140,29 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //============================================================================= // ExponentialContact::Parameters //============================================================================= -/** This subclass manages the topology-stage parameters that are available -for customizing the characteristics of an ExponentialContact object. More -specifically, it has two purposes: - -- Initialization. Before model initialization there is no underlying -SimTK::ExponentialSpringForce object upon which to set non-default parameters. -This class serves as a storage mechanism so that, upon model initialization, -parameters can be pushed to the underlying SimTK::ExponentialSpringForce -instance. - -- Property Maintenance. This class maintains consistence between the -parameters of the underlying SimTK::ExponentialSpringForce inst ance and their -corresponding OpenSim Properties. The local SimTK::ExponentialSpringParameters -member variable (_stkparams) provides a way to ensure the Properties hold -valid values. Before any properties are updated, new parameters are set -on _stkparams. +/** This subclass manages topology-stage parameters that are available for +for customizing the characteristics of an ExponentialContact object. It does +not provide the interface for getting and setting contact parameters (directly +anyway) but rather provides the infrastructure for making the underlying +SimTK::ExponentialSpringForce and SimTK::ExponentialSpringParameters classes +available in OpenSim. More specifically, the infrastructure does three things: +- Implements OpenSim Properties for most of the customizable contact +parameters of class OpenSim::ExponentialContact, enabling those parameters +to be serialized to and de-serialized from file. +- Provides a member variable (_stkparams) for storing non-default parameters +prior to creation of an underlying SimTK::ExponentialSpringForce object. During model +initialization, when the SimTK::ExponetialSpringForce object is constructed, +the parameters are pushed to that object. +- Ensures that the values held by the OpenSim properties are consistent +with the values held by a SimTK::ExponentialSpringParameters object. +Depending on the circumstance, sometimes parameters are updated to match +properties; other times properties are update to match parameters. + +To access the values of parameters (and properties) managed by this class, +you should use ExponentialContact::getParameters() and +ExponentialContact::setParameters(). All parameter changes are made via +SimTK::ExpponentialSpringParameters(). + @author F. C. Anderson **/ class ExponentialContact::Parameters : public Object { diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialSpring.cpp index 56b5fa40d0..fc207a444d 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialSpring.cpp @@ -464,13 +464,21 @@ ::addExponentialSprings(OpenSim::Body* block) Vec3 floorOrigin(0., -0.004, 0.); Transform floorXForm(floorRot, floorOrigin); + // Specify non-default contact parameters + SimTK::ExponentialSpringParameters params; + if (noDamp) { + params.setNormalViscosity(0.0); + params.setFrictionViscosity(0.0); + params.setInitialMuStatic(0.0); + } + // Loop over the corners std::string name = ""; for (int i = 0; i < n; ++i) { name = "ExpSpr" + std::to_string(i); OpenSim::ExponentialContact* force = new OpenSim::ExponentialContact(floorXForm, - block->getName(), corner[i]); + block->getName(), corner[i], params); force->setName(name); model->addForce(force); } @@ -626,6 +634,9 @@ simulate() // auto vis = model->getVisualizer(); // vis.show(state); //} + + // Write the model to file + model->print("C:\\Users\\fcand\\Documents\\block.osim"); } //_____________________________________________________________________________ From 110d356a820aad6ce4d7bbaf3ede9c25e7e57c8a Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 7 Jun 2022 10:03:06 -0500 Subject: [PATCH 13/55] File rename: "ExpoentialSpringForce" to "ExponentialContact" This name change made the name shorter and will help distinguish the OpenSim classess (ExponentialContact) from the Simbody classes (ExponentialSpringForce). --- .../{ExponentialSpringForce.cpp => ExponentialContact.cpp} | 4 ++-- .../Model/{ExponentialSpringForce.h => ExponentialContact.h} | 0 .../{testExponentialSpring.cpp => testExponentialContact.cpp} | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename OpenSim/Simulation/Model/{ExponentialSpringForce.cpp => ExponentialContact.cpp} (99%) rename OpenSim/Simulation/Model/{ExponentialSpringForce.h => ExponentialContact.h} (100%) rename OpenSim/Simulation/Test/{testExponentialSpring.cpp => testExponentialContact.cpp} (99%) diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp similarity index 99% rename from OpenSim/Simulation/Model/ExponentialSpringForce.cpp rename to OpenSim/Simulation/Model/ExponentialContact.cpp index ccfc84bd83..dabbbbbc85 100644 --- a/OpenSim/Simulation/Model/ExponentialSpringForce.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -1,5 +1,5 @@ /* -------------------------------------------------------------------------- * - * OpenSim: ExponentialSpringForce.cpp * + * OpenSim: ExponentialContact.cpp * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * * See http://opensim.stanford.edu and the NOTICE file for more information. * @@ -22,7 +22,7 @@ * -------------------------------------------------------------------------- */ #include "Model.h" -#include "ExponentialSpringForce.h" +#include "ExponentialContact.h" using namespace OpenSim; using namespace std; diff --git a/OpenSim/Simulation/Model/ExponentialSpringForce.h b/OpenSim/Simulation/Model/ExponentialContact.h similarity index 100% rename from OpenSim/Simulation/Model/ExponentialSpringForce.h rename to OpenSim/Simulation/Model/ExponentialContact.h diff --git a/OpenSim/Simulation/Test/testExponentialSpring.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp similarity index 99% rename from OpenSim/Simulation/Test/testExponentialSpring.cpp rename to OpenSim/Simulation/Test/testExponentialContact.cpp index fc207a444d..f2bd1f2878 100644 --- a/OpenSim/Simulation/Test/testExponentialSpring.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include From 4a0d4a7549756521b5f3c79d113110ab2b9823c1 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 7 Jun 2022 13:16:01 -0500 Subject: [PATCH 14/55] Improved documentation comments. --- OpenSim/Simulation/Model/ExponentialContact.h | 33 +++++++++---------- 1 file changed, 16 insertions(+), 17 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index 91042b0fe7..02827ec728 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -140,29 +140,29 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //============================================================================= // ExponentialContact::Parameters //============================================================================= -/** This subclass manages topology-stage parameters that are available for -for customizing the characteristics of an ExponentialContact object. It does -not provide the interface for getting and setting contact parameters (directly -anyway) but rather provides the infrastructure for making the underlying -SimTK::ExponentialSpringForce and SimTK::ExponentialSpringParameters classes -available in OpenSim. More specifically, the infrastructure does three things: +/** This subclass helps manage topology-stage parameters of an +OpenSim::ExponentialContact object. It does not provide the interface for +getting and setting contact parameters (directly anyway) but rather provides +the infrastructure for making the underlying SimTK::ExponentialSpringForce and +SimTK::ExponentialSpringParameters classes available in OpenSim. + +More specifically, this class does 3 things: - Implements OpenSim Properties for most of the customizable contact parameters of class OpenSim::ExponentialContact, enabling those parameters to be serialized to and de-serialized from file. - Provides a member variable (_stkparams) for storing non-default parameters -prior to creation of an underlying SimTK::ExponentialSpringForce object. During model -initialization, when the SimTK::ExponetialSpringForce object is constructed, -the parameters are pushed to that object. -- Ensures that the values held by the OpenSim properties are consistent +prior to the creation of an underlying SimTK::ExponentialSpringForce object. +During model initialization, when the SimTK::ExponetialSpringForce object is +constructed, the parameters are pushed to that object. +- Ensures that the values held by the OpenSim properties are kept consistent with the values held by a SimTK::ExponentialSpringParameters object. -Depending on the circumstance, sometimes parameters are updated to match -properties; other times properties are update to match parameters. +Depending on the circumstance, parameters are updated to match properties or +properties are update to match parameters. -To access the values of parameters (and properties) managed by this class, +To access the values of the parameters (and properties) managed by this class, you should use ExponentialContact::getParameters() and -ExponentialContact::setParameters(). All parameter changes are made via -SimTK::ExpponentialSpringParameters(). - +ExponentialContact::setParameters(). Like SimTK::ExponentialSpringForce, +parameter changes are made via a SimTK::ExpponentialSpringParameters() object. @author F. C. Anderson **/ class ExponentialContact::Parameters : public Object { @@ -218,7 +218,6 @@ class ExponentialContact::Parameters : public Object { SimTK::ExponentialSpringParameters _stkparams; }; - } // end of namespace OpenSim #endif // OPENSIM_EXPONENTIAL_SPRING_FORCE_H_ From 1ffa6e3427c78d91f19c5d8cbeb3866b76665b6c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 10:01:42 -0500 Subject: [PATCH 15/55] Cleaned up code, removed memory leaks, polished comments. - Took out a custom visualization class that was not currently being used. - Now deleting the objects that are allocated from the heap. - Corrected some of the comments. --- .../Test/testExponentialContact.cpp | 155 ++++-------------- 1 file changed, 30 insertions(+), 125 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index f2bd1f2878..781ba220b1 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -49,96 +49,6 @@ using namespace SimTK; using namespace std; using namespace OpenSim; -class TestExpSprVisuals; - -//============================================================================= -/** Record the System state at regular intervals. */ -class VisualsReporter : public PeriodicEventReporter -{ -public: - VisualsReporter(TestExpSprVisuals& visuals, Real reportInterval) : - PeriodicEventReporter(reportInterval), vis(visuals) {} - - void handleEvent(const State& state) const override; - -private: - TestExpSprVisuals& vis; -}; - -//============================================================================= -/** Class Visuals encapsulates the variables needed for running a -SimTK::Visualizer and provides the polling methods for starting a -simulation and replaying the simulated motion. */ -class TestExpSprVisuals -{ -public: - // Constructor - TestExpSprVisuals(MultibodySystem& system) { - // Menu Items - runMenuItems.push_back(std::make_pair("Go", goItem)); - runMenuItems.push_back(std::make_pair("Replay", replayItem)); - - // SimTK::Visualizer - vis = new Visualizer(system); - vis->setShowShadows(true); - - // Input Silo - silo = new Visualizer::InputSilo(); - vis->addInputListener(silo); - vis->addMenu("Run", runMenuID, runMenuItems); - - // Reporters - system.addEventReporter(new Visualizer::Reporter(*vis, reportInterval)); - system.addEventReporter(new VisualsReporter(*this, reportInterval)); - - // Reserve memory for the System states to be recorded - states.reserve(50000); - } - - // State storage - void appendState(const State& state) { states.emplace_back(state); } - - // Polling - void pollForStart() { - cout << "\nChoose 'Go' from the Run menu to simulate.\n"; - int menuID, item; - do { - silo->waitForMenuPick(menuID, item); - if (menuID != runMenuID || item != goItem) - cout << "\aDude ... follow instructions!\n"; - } while (menuID != runMenuID || item != goItem); - } - void pollForReplay() { - silo->clear(); - while (true) { - cout << "Choose Replay to see that again ...\n"; - int menuID, item; - silo->waitForMenuPick(menuID, item); - for (double i = 0; i < (int)states.size(); i++) { - vis->report(states[(int)i]); - } - } - } - -private: - SimTK::Visualizer* vis{NULL}; - SimTK::Visualizer::InputSilo* silo{NULL}; - SimTK::Array_> runMenuItems; - const int runMenuID = 3; - const int goItem{1}, replayItem{2}, quitItem{3}; - double reportInterval = 0.01; - SimTK::Array_ states; -}; - -//_____________________________________________________________________________ -// Definition had to follow the declaration for TestExpSprVisuals -void -VisualsReporter:: -handleEvent(const State& state) const -{ - vis.appendState(state); -} - //============================================================================= /** Class ExponentialSpringTester provides a scope and framework for evaluating and testing the ExponentialSpringForce class. Using a class gets @@ -167,8 +77,16 @@ class ExponentialSpringTester // Destructor ~ExponentialSpringTester() { - if(model) model->disownAllComponents(); - if(model) delete model; + if (model) { + model->disownAllComponents(); + delete model; + } + if (blockES) delete blockES; + if (blockHC) delete blockHC; + for (int i = 0; i < n; i++) { + if (sprES[i]) delete sprES[i]; + if (sprHC[i]) delete sprHC[i]; + } } // Command line parsing and usage @@ -210,16 +128,16 @@ class ExponentialSpringTester bool showVisuals{false}; // Model and parts + const static int n{8}; Model* model{NULL}; OpenSim::Body* blockES{NULL}; OpenSim::Body* blockHC{NULL}; + ExponentialContact* sprES[n]{NULL}; + OpenSim::HuntCrossleyForce* sprHC[n]{NULL}; Storage fxData; ExternalForce* fxES{NULL}; ExternalForce* fxHC{NULL}; - // Visualization - TestExpSprVisuals* visuals{NULL}; - }; // End class ExponentialSpringTester declarations //_____________________________________________________________________________ @@ -447,7 +365,6 @@ ::addExponentialSprings(OpenSim::Body* block) Ground& ground = model->updGround(); // Corners of the block - const int n = 8; Vec3 corner[n]; corner[0] = Vec3(hs, -hs, hs); corner[1] = Vec3(hs, -hs, -hs); @@ -476,11 +393,10 @@ ::addExponentialSprings(OpenSim::Body* block) std::string name = ""; for (int i = 0; i < n; ++i) { name = "ExpSpr" + std::to_string(i); - OpenSim::ExponentialContact* force = - new OpenSim::ExponentialContact(floorXForm, - block->getName(), corner[i], params); - force->setName(name); - model->addForce(force); + sprES[i] = new OpenSim::ExponentialContact(floorXForm, + block->getName(), corner[i], params); + sprES[i]->setName(name); + model->addForce(sprES[i]); } } //______________________________________________________________________________ @@ -496,7 +412,6 @@ addHuntCrossleyContact(OpenSim::Body* block) model->addContactGeometry(floor); // Corners of the block - const int n = 8; Vec3 corner[n]; corner[0] = Vec3(hs, -hs, hs); corner[1] = Vec3(hs, -hs, -hs); @@ -521,12 +436,11 @@ addHuntCrossleyContact(OpenSim::Body* block) ContactParameters(1.0e7, 4e-1, 0.7, 0.465, 0.0); contactParams->addGeometry(name); contactParams->addGeometry("floor"); - OpenSim::HuntCrossleyForce* force = - new OpenSim::HuntCrossleyForce(contactParams); + sprHC[i] = new OpenSim::HuntCrossleyForce(contactParams); name = "HuntCrossleyForce_" + std::to_string(i); - force->setName(name); - force->setTransitionVelocity(0.01); - model->addForce(force); + sprHC[i]->setName(name); + sprHC[i]->setTransitionVelocity(0.01); + model->addForce(sprHC[i]); } } //_____________________________________________________________________________ @@ -620,23 +534,14 @@ simulate() state = manager.integrate(tf); auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; - // Trys and Steps + // Output int trys = manager.getIntegrator().getNumStepsAttempted(); int steps = manager.getIntegrator().getNumStepsTaken(); - - // Output cout << "trys = " << trys << "\t\tsteps = " << steps; cout << "\t\tcpu time = " << runTime << endl; - // Visuals Replay - //if (showVisuals) { - // //visuals->pollForReplay(); - // auto vis = model->getVisualizer(); - // vis.show(state); - //} - // Write the model to file - model->print("C:\\Users\\fcand\\Documents\\block.osim"); + //model->print("C:\\Users\\fcand\\Documents\\block.osim"); } //_____________________________________________________________________________ @@ -646,11 +551,11 @@ The motion of a 10 kg, 6 degree-of-freedom block and its force interaction with a laboratory floor are simulated. Contact with the floor is modeled using either - 1) 8 ExponentialSpringForce instances, one at each corner of the block, or + 1) 8 ExponentialContact instances, one at each corner of the block, or 2) 8 HuntCrossleyForce instances, one at each corner of the block. For a side-by-side comparison of simulated motions, two blocks (one using -the ExponentialSpringForce class for contact and the other using the +the ExponentialContact class for contact and the other using the HuntCrossleyForce class) can be created and visualized simultaneously. For an assessment of computational performance, just one block should be @@ -662,7 +567,7 @@ Choice of initial conditions can be made in order to generate the following 1) Static (y = 0.1 m, sitting at rest on the floor) 2) Bouncing (y = 1.0 m, dropped) 3) Sliding (y = 0.2 m, vx = -2.0 m/s) - 4) Spinning & Sliding (y = 0.2 m, vx = -2.0 m/s, wy = 2.0 pi rad/sec) + 4) Spinning (y = 0.2 m, vx = 0.0 m/s, wy = 2.0 pi rad/sec) 5) Tumbling (py = 2.0 m, vx = -2.0 m/s, wz = 2.0 pi rad/sec) Additional options allow the following to be specified: @@ -679,17 +584,17 @@ This ramping profile was done with the "Static" initial condition choice in mind so that friction models could be evaluated more critically. In particular, a static block should not start sliding until fx > μₛ Fₙ. -For ExponentialSpringForce, the following things are tested: +For ExponentialContact, the following things are tested: a) instantiation b) model initialization - c) energy conservation + c) consistency between OpenSim Properties and SimTK parameters d) data cache access e) realization stage invalidation f) reporting g) serialization -The HuntCrossleyForce class is tested elsewhere (e.g., see -testContactGeometry.cpp). */ +The HuntCrossleyForce class is tested elsewhere +(e.g., see testContactGeometry.cpp and testForce.cpp). */ int main(int argc, char** argv) { try { ExponentialSpringTester tester; From 814f974330b441ed8d76d375d54e4e7c2bfe65a6 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 10:02:49 -0500 Subject: [PATCH 16/55] Added detailed Doxygen comments. --- OpenSim/Simulation/Model/ExponentialContact.h | 306 ++++++++++++++++-- 1 file changed, 280 insertions(+), 26 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index 02827ec728..fa4c25c338 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -1,4 +1,4 @@ -#ifndef OPENSIM_EXPONENTIAL_CONTACT_H_ +#ifndef OPENSIM_EXPONENTIAL_CONTACT_H_ #define OPENSIM_EXPONENTIAL_CONTACT_H_ /* -------------------------------------------------------------------------- * * OpenSim: ExponentialContactForce.h * @@ -33,9 +33,250 @@ namespace OpenSim { //============================================================================= // ExponentialContact //============================================================================= -/** This OpenSim::Force subclass implements a SimTK::ExponentialSpringForce -to model contact of a specified point on a body (i.e., a "station" in Simbody -vocabulary) with a contact plane that is fixed to Ground. +/** Class ExponentialContact uses an "exponential spring" as a means +of modeling contact of a specified point on a Body with a contact +plane that is fixed to Ground. In this documentation, this specified point +is referred to as the "body station". Each ExponentialContact instance +acts at only one body station. In practice, you should choose a number of +body stations strategically located across the surface of a Body, +and construct an ExponentialContact instance for each of those body stations. +For example, if the Body were a cube, you would likely choose the body +stations to be the corners of the cube and construct an ExponentialContact +instance for each corner of the cube (so a total of 8 instances). The contact +plane is typically used to model interactions with a floor, but is not +limited to this use case. The contact plane can be rotated and translated +relative to the Ground frame and so can be used to model a wall, ramp, or +some other planar structure. + +Aspects of the exponential contact model are described in the following +publication: + + Anderson F.C. and Pandy M.G. (1999). A dynamics optimization + solution for vertical jumping in three dimensions. Computer Methods + in Biomechanics and Biomedical Engineering 2(3):201-231. + +Under the covers, the OpenSim::ExponentialContact class encapsulates two +SimTK objects: ExponentialSpringForce and ExponentialSpringParameters. +For the details concerning the contact model, see the Simbody API +documentation for SimTK::ExponentialSpringForce. A condensed version of +that documentation is provided here. + +---------------------------------- +Computations and Coordinate Frames +---------------------------------- +The positive z-axis of the contact plane defines its normal. The positive +z-axis is the axis along which the repelling normal force (modeled using an +exponential) is applied. The x-axis and y-axis of the contact plane together +define the tangent plane. Friction forces will always be tangent to the x-y +plane. + +In the equations below, all quantities are expressed in the frame of the +contact plane. A variable with a "z" suffix (e.g., pz, vz, or cz) refers +to a quantity that is normal to the contact plane or that pertains to +calculation of the normal force. A variable with an "xy" suffix +(e.g., pxy, vxy, or cxy) refers to a quantity that lies in or is tangent to +the contact plane or that pertains to calculation of the friction force. + +### Normal Force (positive z-axis) + +The elastic part of the normal force is computed using an exponential +whose shape is a function of three parameters (d₀, d₁, and d₂): + + fzElastic = d₁exp(−d₂(pz−d₀)) + +Note that pz is the displacement of the body station above (pz > 0.0) +or below (pz < 0.0) the contact plane. The default values of the shape +parameters were chosen to maximize integration step size while maintaining a +number of constraints (e.g., the normal force should fall below 0.01 Newtons +when pz > 1.0 cm). The damping part of the normal force is linear in velocity +and scaled by the elastic part: + + fzDamping = −cz vz fzElastic, + +where vz is the normal component of the velocity of the body station and +cz is the damping coefficient for the normal direction. All together, the +spring force in the normal direction is given by + + fz = fzElastic + fzDamping + = d₁exp(d₂(py−d₀)) − cz vz d₁exp(d₂(pz−d₀))) + = d₁exp(d₂(pz−d₀)) (1 − cz vz) + +which has the form of the Hunt & Crossley damping model: + + K. H. Hunt and F. R. E. Crossley (1975). Coefficient of Restitution + Interpreted as Damping in Vibroimpact. ASME Journal of Applied + Mechanics, pp. 440-445. + +### Friction Force (x-y plane) + +The friction force is computed by blending two different friction models. +The blending is performed based on the 'Sliding' State of the +ExponentialSpringForce class. 'Sliding' is a continuous state variable (a Z +in Simbody vocabulary) that characterizes the extent to which either static +or kinetic conditions are present. + +#### Friction Model 1 - Pure Damping (Sliding = 1.0) +When the body station is moving with respect to the contact plane, the +friction force is computed using a simple damping term: + + fricDamp = −cxy vxy + +where cxy is the damping coefficient in the contact plane and vxy is the +velocity of the body station in the contact plane. However, the magnitude +of the total frictional force is not allowed to exceed the frictional limit: + + fricLimit = μ fz + if (|fricDamp| > fricLimit) + fricDamp = −fricLimit vxy / |vxy| = −μ fz vxy / |vxy| + +where μ is the instantaneous coefficient of friction (more below). Note that +fz is always positive and so fricLimit is a positive scalar. Thus, for +velocities in the contact plane above some threshold velocity, which is +typically small (i.e., less than 0.1 m/s), this model is consistent with a +standard Coulomb Friction model. + +#### Friction Model 2 - Damped Linear Spring (Sliding = 0.0) +When the body station is anchored with respect to the contact plane, the +friction force is represented by a damped linear spring. The viscous term is +given by the same damping expression as above: + + fricDampSpr = −cxy vxy + +and the elastic term is given by + + fricElasSpr = −kxy (pxy−p₀) + +where kxy is the friction spring elasticity, pxy is the position of the body +station projected onto the contact plane, and p₀ is the current spring zero +(i.e., the elastic anchor point of the friction spring). Note that p₀ always +resides in the contact plane. + +The total friction spring force is then given by the sum of the elastic and +viscous terms: + + fricSpr = fricElasSpr + fricDampSpr + +If the magnitude of the fricSpr exceeds the magnitude of the friction limit, +the terms are scaled down: + + if(|fricSpr| > fricLimit) + scaleFactor = fricLimit / |fricSpr| + fricDampSpr = scaleFactor * fricDampSpr + fricElasSpr = scaleFactor * fricElasSpr + fricSpr = fricElasSpr + fricDampSpr + +Scaling down the friction spring force does not alter its direction. + +#### Blending the Friction Models +Blending Model 1 and Model 2 is accomplished using linear expressions of the +Sliding State: + + fricElasBlend = fricElasSpr * (1.0 − Sliding) + fricDampBlend = fricDampSpr + (fricDamp − fricDampSpr)*Sliding + fricBlend = fricElasBlend + fricDampBlend + +Thus, Model 1 (Pure Damping) dominates as Sliding → 1.0, and Model 2 +(Damped Linear Spring) dominates as Sliding → 0.0. + +#### Moving the Friction Spring Zero +The friction spring zero (p₀) (the elastic anchor point) is always altered +to be consistent with the final value of the blended elastic force: + + p₀ = pxy + fricElasBlend / kxy; + p₀[2] = 0.0; // ensures that p₀ lies in the contact plane + +#### Coefficients of Friction +Coefficients of kinetic (sliding) and static (fixed) friction can be specified +for the spring, subject to the following constraints: + + 0.0 ≤ μₖ ≤ μₛ + +Note that there is no upper bound on μₛ. The instantaneous coefficient of +friction (μ) is calculated based on the value of the Sliding State: + + μ = μₛ − Sliding*(μₛ − μₖ) + +The time derivative of Sliding, SlidingDot, is used to drive Sliding toward +the extremes of 0.0 or 1.0, depending on the following criteria: + +- If the frictional spring force (fricSpr) exceeded the frictional limit at +any point during its calculation, Sliding is driven toward 1.0 (rise): + + SlidingDot = (1.0 − Sliding)/tau + +- If the frictional spring force (fricSpr) does not exceed the frictional +limit at any point during its calculation and if the kinematics of the body +station are near static equilibrium, Sliding is driven toward 0.0 (decay): + + SlidingDot = −Sliding/tau + +The threshold for being "near" static equilibrium is established by two +parameters, vSettle and aSettle. When the velocity and acceleration of the +body station relative to the contact plane are below vSettle and aSettle, +respectively, static equilibrium is considered effectively reached. + +During a simulation, once a rise or decay is triggered, the rise or decay +continues uninterrupted until Sliding crosses 0.95 or 0.05, respectively. Once +these thresholds have been crossed, the criteria for rise and decay are again +monitored. + +In the above equations for SlidingDot, tau is the characteristic time it takes +for the Sliding State to rise or decay. The default value of tau is 0.01 sec. +The motivation for using a continuous state variable is that, although the +transition between static and kinetic may happen quickly, it does not happen +instantaneously. Evolving Sliding based on a differential equation ensures +that μ is continuous and that the blending of friction models is well behaved. + +----------------------- +CUSTOMIZABLE PARAMETERS +----------------------- +Customizable Topology-stage parameters specifying the characteristics of the +exponential spring are managed using SimTK::ExponentialSpringParameters. +To customize any of the Topology-stage parameters on an ExponentialContact +instance, you should + +1) Create an ExponentialSpringParameters object. For example, + + ExponentialSpringParameters myParams; + +2) Use the available 'set' methods in ExponentialSpringParamters to change +the parameters of that object. For example, + + myParams.setNormalViscosity(0.25); + +3) Use ExponentialContact::setParameters() to alter the parameters of one +(or many) ExponentialContact instances. For example, + + ExponentialContact spr1, spr2; + spr1.setParameters(myParams); + spr2.setParameters(myParams); + +4) Realize the system to Stage::Topology. When a new set of parameters is +set on an ExponentialContact instance, as above in step 3, the System will +be invalidated at Stage::Topology. The System must therefore be realized at +Stage::Topology (and hence Stage::Model) before a simulation can proceed. + + system.realizeTopology(); + +Note that each ExponentialContact instance owns its own private +ExponentialSpringParameters object. The myParams object is just used to set +the desired parameter values of the privately owned parameters object. It is +fine for objects like myParams to go out of scope or for myParams objects +allocated from the heap to be deleted. + +Therefore, also note that the parameter values possessed by an +ExponentialContact instance do not necessarily correspond to the values +held by an instance of ExponentialSpringParameters until a call to +ExponentialContact::setParameters() is made. + +The default values of the parameters are expressed in units of Newtons, +meters, seconds, and kilograms; however, you may use an alternate set of +self-consistent units by re-specifying all parameters. + +The default values of the parameters work well for typical contact +interactions, but clearly may not be appropriate for simulating many contact +interactions. For the full descriptions of the contact parameters see the +Simbody API documentation for SimTK::ExponentialSpringParameters. @author F. C. Anderson **/ class OSIMSIMULATION_API ExponentialContact : public Force { @@ -64,7 +305,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { /** Default constructor. */ ExponentialContact(); - /** Construct an ExponentialContact. + /** Construct an ExponentialContact instance. @param XContactPlane Transform specifying the location and orientation of the contact plane in Ground. @param bodyName Name of the body to which the force is applied. @@ -76,35 +317,41 @@ class OSIMSIMULATION_API ExponentialContact : public Force { SimTK::ExponentialSpringParameters params = SimTK::ExponentialSpringParameters()); - /** Copy constructor. */ - //ExponentialContact(const ExponentialContact& other); + /** Destructor. */ + ~ExponentialContact() { + if (_spr != NULL) delete _spr; + } //------------------------------------------------------------------------- // Accessors //------------------------------------------------------------------------- - /** Set the tranform that specifies the location and orientation of the + /** Set the transform that specifies the location and orientation of the contact plane in the Ground frame. */ void setContactPlaneTransform(const SimTK::Transform& contactPlaneXform); - /** Get the tranform that specifies the location and orientation of the + /** Get the transform that specifies the location and orientation of the contact plane in the Ground frame. */ const SimTK::Transform& getContactPlaneTransform() const; /** Set the name of the body to which this force is applied. */ - void setBodyName(const std::string& bodyName) { set_body_name(bodyName); } + void setBodyName(const std::string& bodyName) { + set_body_name(bodyName); + } /** Get the name of the body to which this force is applied. */ const std::string& getBodyName() const { return get_body_name(); } /** Set the point on the body at which this force is applied. */ - void setBodyStation(const SimTK::Vec3& station) { set_body_station(station); } + void setBodyStation(const SimTK::Vec3& station) { + set_body_station(station); + } /** Get the point on the body at which this force is applied. */ const SimTK::Vec3& getBodyStation() const { return get_body_station(); } /** Set the customizable Topology-stage spring parameters. Calling this - method will invalidate the SimTK::System at Stage::Toplogy. The System - must therefore be realized at Stage::Topology before simulation or - analysis can proceed. */ + method will invalidate the SimTK::System at Stage::Toplogy. */ void setParameters(const SimTK::ExponentialSpringParameters& params); - /** Get the customizable Topology-stage spring parameters. */ + /** Get the customizable Topology-stage spring parameters. Use the copy + constructor on the returned reference to create an object that can + be altered. */ const SimTK::ExponentialSpringParameters& getParameters() const; //------------------------------------------------------------------------- @@ -121,7 +368,8 @@ class OSIMSIMULATION_API ExponentialContact : public Force { /** Connect to the OpenSim Model. */ void extendConnectToModel(Model& model) override; - /** Create a SimTK::ExponentialContact that implements this Force. */ + /** Create a SimTK::ExponentialSpringForce objects that implements + this Force. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; /** Update this Object base on an XML node. */ @@ -140,13 +388,13 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //============================================================================= // ExponentialContact::Parameters //============================================================================= -/** This subclass helps manage topology-stage parameters of an +/** This subclass helps manage most of the topology-stage parameters of an OpenSim::ExponentialContact object. It does not provide the interface for getting and setting contact parameters (directly anyway) but rather provides the infrastructure for making the underlying SimTK::ExponentialSpringForce and SimTK::ExponentialSpringParameters classes available in OpenSim. -More specifically, this class does 3 things: +More specifically, this class mainly does 3 things: - Implements OpenSim Properties for most of the customizable contact parameters of class OpenSim::ExponentialContact, enabling those parameters to be serialized to and de-serialized from file. @@ -159,10 +407,10 @@ with the values held by a SimTK::ExponentialSpringParameters object. Depending on the circumstance, parameters are updated to match properties or properties are update to match parameters. -To access the values of the parameters (and properties) managed by this class, -you should use ExponentialContact::getParameters() and -ExponentialContact::setParameters(). Like SimTK::ExponentialSpringForce, -parameter changes are made via a SimTK::ExpponentialSpringParameters() object. +To get or set values of the parameters managed by this class, you should use +ExponentialContact::getParameters() and ExponentialContact::setParameters(). +Note that the values of the parameters managed by this class are always the +same as the values of their corresponding properties. @author F. C. Anderson **/ class ExponentialContact::Parameters : public Object { @@ -201,12 +449,18 @@ class ExponentialContact::Parameters : public Object { object. */ Parameters(const SimTK::ExponentialSpringParameters& params); - /** Set the underlying SimTK::ExponentialSpringParameters. */ + /** Set the underlying SimTK parameters. This method is used to maintain + consistency between OpenSim Properties and the underlying parameters. + The typical user of OpenSim::ExponentialContact will not have reason to + call this method. For setting contact parameters, the typical user should + call OpenSim::ExponentialContact::setParameters(). */ void setSimTKParameters(const SimTK::ExponentialSpringParameters& params); - /** Get a read-only reference to the underlying - SimTK::ExponentialSpringParameters. To alter the parameters, use - setParameters(). */ + /** Get a read-only reference to the underlying SimTK paramters. This + method is used to maintain consistency between OpenSim Properties and the + underlying parameters. The typical user of OpenSim::ExponentialContact will + not have reason to call this method. For getting contact parameters, the + typical user should call OpenSim::ExponentialContact::getParameters() */ const SimTK::ExponentialSpringParameters& getSimTKParameters() const; private: From fa57ae29ac54637a90ebde575bff8788a0ddb21d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 10:04:23 -0500 Subject: [PATCH 17/55] Enhanced reported information. --- .../Simulation/Model/ExponentialContact.cpp | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index dabbbbbc85..e9ee776234 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -168,18 +168,6 @@ ExponentialContact(const SimTK::Transform& contactPlaneXform, setParameters(params); } //_____________________________________________________________________________ -/* -ExponentialContact:: -ExponentialContact(const ExponentialContact& source) -{ - setNull(); - constructProperties(); - setContactPlaneTransform(source.getContactPlaneTransform()); - setBodyName(source.getBodyName()); - setBodyStation(source.getBodyStation()); -} -*/ -//_____________________________________________________________________________ void ExponentialContact:: setNull() @@ -282,9 +270,6 @@ getParameters() const //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- -// Questions: -// 1. Do I need to conform to a certain reporting format because this object -// will be treated as an OpenSim::Force? //_____________________________________________________________________________ OpenSim::Array ExponentialContact:: @@ -292,6 +277,18 @@ getRecordLabels() const { OpenSim::Array labels(""); std::string frameName = getBodyName(); + labels.append(getName()+"."+frameName+".p0.X"); + labels.append(getName()+"."+frameName+".p0.Y"); + labels.append(getName()+"."+frameName+".p0.Z"); + labels.append(getName()+"."+frameName+".station.X"); + labels.append(getName()+"."+frameName+".station.Y"); + labels.append(getName()+"."+frameName+".station.Z"); + labels.append(getName()+"."+frameName+".forceNormal.X"); + labels.append(getName()+"."+frameName+".forceNormal.Y"); + labels.append(getName()+"."+frameName+".forceNormal.Z"); + labels.append(getName()+"."+frameName+".forceFriction.X"); + labels.append(getName()+"."+frameName+".forceFriction.Y"); + labels.append(getName()+"."+frameName+".forceFriction.Z"); labels.append(getName()+"."+frameName+".force.X"); labels.append(getName()+"."+frameName+".force.Y"); labels.append(getName()+"."+frameName+".force.Z"); @@ -303,13 +300,22 @@ OpenSim::Array ExponentialContact:: getRecordValues(const SimTK::State& state) const { - OpenSim::Array values(1); + OpenSim::Array values(0.0); const auto& forceSubsys = getModel().getForceSubsystem(); const SimTK::Force& abstractForce = forceSubsys.getForce(_index); const auto& spr = (SimTK::ExponentialSpringForce&)(abstractForce); + SimTK::Vec3 p0 = spr.getFrictionSpringZeroPosition(state); + SimTK::Vec3 station = spr.getStationPosition(state); + SimTK::Vec3 normal = spr.getNormalForce(state); + SimTK::Vec3 friction = spr.getFrictionForce(state); SimTK::Vec3 force = spr.getForce(state); + + values.append(3, &p0[0]); + values.append(3, &station[0]); + values.append(3, &normal[0]); + values.append(3, &friction[0]); values.append(3, &force[0]); return values; From 52c95bc0535410920382ad81935a891de0109e53 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 18:35:52 -0500 Subject: [PATCH 18/55] Registered class ExponentialContact. Also needed to register class ExponentialContact::Parameters. --- OpenSim/Simulation/RegisterTypes_osimSimulation.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 2cfa3a5285..867da8c5ac 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -47,6 +47,7 @@ #include "Model/CoordinateLimitForce.h" #include "Model/CoordinateSet.h" #include "Model/ElasticFoundationForce.h" +#include "Model/ExponentialContact.h" #include "Model/HuntCrossleyForce.h" #include "Model/SmoothSphereHalfSpaceForce.h" #include "Model/Ligament.h" @@ -242,6 +243,8 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( ContactSphere() ); Object::registerType( CoordinateLimitForce() ); Object::registerType( SmoothSphereHalfSpaceForce() ); + Object::registerType( ExponentialContact() ); + Object::registerType( ExponentialContact::Parameters() ); Object::registerType( HuntCrossleyForce() ); Object::registerType( ElasticFoundationForce() ); Object::registerType( HuntCrossleyForce::ContactParameters() ); From 9e710a43452d3aff127870e37746627d3ccd42d7 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 18:40:15 -0500 Subject: [PATCH 19/55] Bug fix and some internal testing. Bug Fix: "viscosity" was misspelled in a number property related methods. I added a method that tests if the OpenSim Properties and SimTK Parameters are consistent with each other. --- .../Simulation/Model/ExponentialContact.cpp | 57 ++++++++++++++++++- OpenSim/Simulation/Model/ExponentialContact.h | 8 ++- 2 files changed, 61 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index e9ee776234..59798cfacf 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -62,7 +62,7 @@ constructProperties() constructProperty_normal_viscosity(_stkparams.getNormalViscosity()); constructProperty_max_normal_force(_stkparams.getMaxNormalForce()); constructProperty_friction_elasticity(_stkparams.getFrictionElasticity()); - constructProperty_friction_viscocity(_stkparams.getFrictionViscosity()); + constructProperty_friction_viscosity(_stkparams.getFrictionViscosity()); constructProperty_sliding_time_constant(_stkparams.getSlidingTimeConstant()); constructProperty_settle_velocity(_stkparams.getSettleVelocity()); constructProperty_settle_acceleration(_stkparams.getSettleAcceleration()); @@ -81,7 +81,7 @@ updateProperties() set_normal_viscosity(_stkparams.getNormalViscosity()); set_max_normal_force(_stkparams.getMaxNormalForce()); set_friction_elasticity(_stkparams.getFrictionElasticity()); - set_friction_viscocity(_stkparams.getFrictionViscosity()); + set_friction_viscosity(_stkparams.getFrictionViscosity()); set_sliding_time_constant(_stkparams.getSlidingTimeConstant()); set_settle_velocity(_stkparams.getSettleVelocity()); set_settle_acceleration(_stkparams.getSettleAcceleration()); @@ -99,7 +99,7 @@ updateParameters() _stkparams.setNormalViscosity(get_normal_viscosity()); _stkparams.setMaxNormalForce(get_max_normal_force()); _stkparams.setFrictionElasticity(get_friction_elasticity()); - _stkparams.setFrictionViscosity(get_friction_viscocity()); + _stkparams.setFrictionViscosity(get_friction_viscosity()); _stkparams.setSlidingTimeConstant(get_sliding_time_constant()); _stkparams.setSettleVelocity(get_settle_velocity()); _stkparams.setSettleAcceleration(get_settle_acceleration()); @@ -321,3 +321,54 @@ getRecordValues(const SimTK::State& state) const return values; } +//----------------------------------------------------------------------------- +// Internal Testing +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +assertPropertiesAndParametersEqual() const +{ + const ExponentialContact::Parameters& a = get_contact_parameters(); + const SimTK::ExponentialSpringParameters& b = getParameters(); + + const SimTK::Vec3& vecA = a.get_exponential_shape_parameters(); + SimTK::Vec3 vecB; + b.getShapeParameters(vecB[0], vecB[1], vecB[2]); + SimTK_TEST_EQ(vecA[0], vecB[0]); + SimTK_TEST_EQ(vecA[1], vecB[1]); + SimTK_TEST_EQ(vecA[2], vecB[2]); + + double valA, valB; + valA = a.get_normal_viscosity(); + valB = b.getNormalViscosity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_friction_elasticity(); + valB = b.getFrictionElasticity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_friction_viscosity(); + valB = b.getFrictionViscosity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_sliding_time_constant(); + valB = b.getSlidingTimeConstant(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_settle_velocity(); + valB = b.getSettleVelocity(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_settle_acceleration(); + valB = b.getSettleAcceleration(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_initial_mu_static(); + valB = b.getInitialMuStatic(); + SimTK_TEST_EQ(valA, valB); + + valA = a.get_initial_mu_kinetic(); + valB = b.getInitialMuKinetic(); + SimTK_TEST_EQ(valA, valB); +} diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index fa4c25c338..5c482654d1 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -364,6 +364,12 @@ class OSIMSIMULATION_API ExponentialContact : public Force { OpenSim::Array getRecordValues( const SimTK::State& state) const override; + //------------------------------------------------------------------------- + // Internal Testing + //------------------------------------------------------------------------- + /** Assess consistency between Properties and internal parameters. */ + void assertPropertiesAndParametersEqual() const; + protected: /** Connect to the OpenSim Model. */ void extendConnectToModel(Model& model) override; @@ -426,7 +432,7 @@ class ExponentialContact::Parameters : public Object { "Maximum allowed normal force (100,000.0 N)."); OpenSim_DECLARE_PROPERTY(friction_elasticity, double, "Elasticity of the linear friction spring (20,000.0 N/m)."); - OpenSim_DECLARE_PROPERTY(friction_viscocity, double, + OpenSim_DECLARE_PROPERTY(friction_viscosity, double, "Viscosity of the linear friction spring (282.8427 N*s/m)."); OpenSim_DECLARE_PROPERTY(sliding_time_constant, double, "Time constant for rise/decay between static and kinetic friction \ From a4f91aa67ab2a9b0cd5b1ae68082c674a6d309fa Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 19:25:55 -0500 Subject: [PATCH 20/55] Made naming more consistent. ExponentialSpringForce -> ExponentialContact, and the abbreviation at the end of variable names when from "ES" to "EC". I also re-organized some of the model initialization so that static model testing could precede the simulation. Basically, a model needs to be completely put together before meaningful tests can be conducted, AND the state needs to be initialized (and the SimTK::System needs to be realized at Stage::Topology) just prior to integration. --- .../Test/testExponentialContact.cpp | 261 +++++++++++++----- OpenSim/Simulation/Test/testForces.cpp | 2 +- 2 files changed, 200 insertions(+), 63 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 781ba220b1..931fb92b5a 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -50,10 +50,11 @@ using namespace std; using namespace OpenSim; //============================================================================= -/** Class ExponentialSpringTester provides a scope and framework for +/** Class ExponentialContactTester provides a scope and framework for evaluating and testing the ExponentialSpringForce class. Using a class gets -a lot of variables out of the global scope. */ -class ExponentialSpringTester +a lot of variables out of the global scope and allows for more structured +memory cleanup. */ +class ExponentialContactTester { public: // Contact choices @@ -73,18 +74,18 @@ class ExponentialSpringTester }; // Constructor - ExponentialSpringTester() {}; + ExponentialContactTester() {}; // Destructor - ~ExponentialSpringTester() { + ~ExponentialContactTester() { if (model) { model->disownAllComponents(); delete model; } - if (blockES) delete blockES; + if (blockEC) delete blockEC; if (blockHC) delete blockHC; for (int i = 0; i < n; i++) { - if (sprES[i]) delete sprES[i]; + if (sprEC[i]) delete sprEC[i]; if (sprHC[i]) delete sprHC[i]; } } @@ -96,13 +97,18 @@ class ExponentialSpringTester // Model Creation void buildModel(); OpenSim::Body* addBlock(const std::string& suffix); - void addExponentialSprings(OpenSim::Body* body); + void addExponentialContact(OpenSim::Body* body); void addHuntCrossleyContact(OpenSim::Body* body); void setForceData( double t, const SimTK::Vec3& point, const SimTK::Vec3& force); void setForceDataHeader(); void addDecorations(); + // Test stuff not covered elsewhere. + void test(); + void testParameters(); + void testSerialization(); + // Simulation void setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz); @@ -130,19 +136,19 @@ class ExponentialSpringTester // Model and parts const static int n{8}; Model* model{NULL}; - OpenSim::Body* blockES{NULL}; + OpenSim::Body* blockEC{NULL}; OpenSim::Body* blockHC{NULL}; - ExponentialContact* sprES[n]{NULL}; + OpenSim::ExponentialContact* sprEC[n]{NULL}; OpenSim::HuntCrossleyForce* sprHC[n]{NULL}; Storage fxData; ExternalForce* fxES{NULL}; ExternalForce* fxHC{NULL}; -}; // End class ExponentialSpringTester declarations +}; // End class ExponentialContactTester declarations //_____________________________________________________________________________ void -ExponentialSpringTester:: +ExponentialContactTester:: addDecorations() { // Ground @@ -154,9 +160,9 @@ addDecorations() SimTK::Body& grndBody = ground.updMobilizedBody().updBody(); grndBody.addDecoration(Transform(Vec3(0, -0.5, 0)), *floor); - // Exponential Springs Block - if (blockES) { - SimTK::Body& body = blockES->updMobilizedBody().updBody(); + // Exponential Contact Block + if (blockEC) { + SimTK::Body& body = blockEC->updMobilizedBody().updBody(); SimTK::DecorativeBrick* brick = new SimTK::DecorativeBrick(SimTK::Vec3(hs)); brick->setColor(SimTK::Blue); @@ -174,7 +180,7 @@ addDecorations() } //_____________________________________________________________________________ int -ExponentialSpringTester:: +ExponentialContactTester:: parseCommandLine(int argc, char** argv) { std::string option; @@ -224,11 +230,11 @@ parseCommandLine(int argc, char** argv) } //_____________________________________________________________________________ void -ExponentialSpringTester:: +ExponentialContactTester:: printUsage() { cout << endl << "Usage:" << endl; - cout << "$ testExponetialSpring " + cout << "$ testExponetialContact " << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; cout << "\tInitCond (choose one): Static Bounce Slide Spin Tumble "; cout << endl; @@ -237,40 +243,41 @@ printUsage() cout << "All arguments are optional. If no arguments are specified, "; cout << "a 'Slide' will" << endl; cout << "be simulated with one block that uses "; - cout << "ExponentialSpringForce contact," << endl; + cout << "ExponentialContact instances," << endl; cout << "with typical damping settings, "; cout << "with no extnerally applied force, " << endl; cout << "and with no visuals." << endl << endl; cout << "Example:" << endl; - cout << "To simulated 2 blocks (one with Exponential Springs and one"; + cout << "To simulated 2 blocks (one with ExponentialContact and one"; cout << " with Hunt-Crossley)" << endl; cout << "that bounce without energy dissipation and with Visuals, "; cout << "enter the following: " << endl << endl; - cout << "$ testExponentialSpring Bounce Both NoDamp Vis" << endl << endl; + cout << "$ testExponentialContact Bounce Both NoDamp Vis" << endl << endl; } //_____________________________________________________________________________ // Build the model void -ExponentialSpringTester::buildModel() +ExponentialContactTester:: +buildModel() { // Create the bodies model = new Model(); model->setGravity(gravity); - model->setName("TestExponentialSpring"); + model->setName("TestExponentialContact"); switch (whichContact) { case ExpSpr: - blockES = addBlock("ES"); - addExponentialSprings(blockES); + blockEC = addBlock("EC"); + addExponentialContact(blockEC); break; case HuntCross: blockHC = addBlock("HC"); addHuntCrossleyContact(blockHC); break; case Both: - blockES = addBlock("ES"); - addExponentialSprings(blockES); + blockEC = addBlock("EC"); + addExponentialContact(blockEC); blockHC = addBlock("HC"); addHuntCrossleyContact(blockHC); } @@ -285,11 +292,11 @@ ExponentialSpringTester::buildModel() setForceData(tf, point, zero); tf = tf + 25.0; setForceData(tf, point, force); - if (blockES) { - cout << "Adding fx for " << blockES->getName() << endl; + if (blockEC) { + cout << "Adding fx for " << blockEC->getName() << endl; fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); fxES = new ExternalForce(fxData,"force", "point", "", - blockES->getName(), "ground", blockES->getName()); + blockEC->getName(), "ground", blockEC->getName()); fxES->setName("externalforceES"); model->addForce(fxES); } @@ -302,9 +309,29 @@ ExponentialSpringTester::buildModel() model->addForce(fxHC); } } + + // Visuals? + if (showVisuals) { + if (blockEC) { + auto blockESGeometry = new Brick(Vec3(hs)); + blockESGeometry->setColor(Vec3(0.1, 0.1, 0.8)); + blockEC->attachGeometry(blockESGeometry); + } + if (blockHC) { + auto blockHCGeometry = new Brick(Vec3(hs)); + blockHCGeometry->setColor(Vec3(0.8, 0.1, 0.1)); + blockHC->attachGeometry(blockHCGeometry); + } + model->setUseVisualizer(true); + } + + // Build the System + model->buildSystem(); } //______________________________________________________________________________ -void ExponentialSpringTester::setForceDataHeader() +void +ExponentialContactTester:: +setForceDataHeader() { fxData.setName("fx"); fxData.setDescription("An external force applied to a block."); @@ -318,10 +345,9 @@ void ExponentialSpringTester::setForceDataHeader() lab.append("force.z"); fxData.setColumnLabels(lab); } - //______________________________________________________________________________ void -ExponentialSpringTester:: +ExponentialContactTester:: setForceData(double t, const SimTK::Vec3& point, const SimTK::Vec3& force) { SimTK::Vector_ data(6); @@ -332,10 +358,9 @@ setForceData(double t, const SimTK::Vec3& point, const SimTK::Vec3& force) StateVector sv(t, data); fxData.append(sv); } - //______________________________________________________________________________ OpenSim::Body* -ExponentialSpringTester:: +ExponentialContactTester:: addBlock(const std::string& suffix) { Ground& ground = model->updGround(); @@ -359,8 +384,8 @@ addBlock(const std::string& suffix) } //______________________________________________________________________________ void -ExponentialSpringTester -::addExponentialSprings(OpenSim::Body* block) +ExponentialContactTester:: +addExponentialContact(OpenSim::Body* block) { Ground& ground = model->updGround(); @@ -393,15 +418,15 @@ ::addExponentialSprings(OpenSim::Body* block) std::string name = ""; for (int i = 0; i < n; ++i) { name = "ExpSpr" + std::to_string(i); - sprES[i] = new OpenSim::ExponentialContact(floorXForm, + sprEC[i] = new OpenSim::ExponentialContact(floorXForm, block->getName(), corner[i], params); - sprES[i]->setName(name); - model->addForce(sprES[i]); + sprEC[i]->setName(name); + model->addForce(sprEC[i]); } } //______________________________________________________________________________ void -ExponentialSpringTester:: +ExponentialContactTester:: addHuntCrossleyContact(OpenSim::Body* block) { Ground& ground = model->updGround(); @@ -445,7 +470,7 @@ addHuntCrossleyContact(OpenSim::Body* block) } //_____________________________________________________________________________ void -ExponentialSpringTester:: +ExponentialContactTester:: setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz) { @@ -494,33 +519,144 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, } } + +//----------------------------------------------------------------------------- +// TESTING +//----------------------------------------------------------------------------- //_____________________________________________________________________________ void -ExponentialSpringTester:: -simulate() +ExponentialContactTester:: +test() { - // Visuals? - if (showVisuals) { - if (blockES) { - auto blockESGeometry = new Brick(Vec3(hs)); - blockESGeometry->setColor(Vec3(0.1, 0.1, 0.8)); - blockES->attachGeometry(blockESGeometry); - } - if (blockHC) { - auto blockHCGeometry = new Brick(Vec3(hs)); - blockHCGeometry->setColor(Vec3(0.8, 0.1, 0.1)); - blockHC->attachGeometry(blockHCGeometry); - } - model->setUseVisualizer(true); + testParameters(); + testSerialization(); +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +testParameters() +{ + // Check current properties/parameters for all springs + for (int i = 0; i < n; i++) { + sprEC[i]->assertPropertiesAndParametersEqual(); } - // Initialize the System - SimTK::State& state = model->initSystem(); + // Pick just one contact instance to manipulate. + ExponentialContact& spr = *sprEC[0]; + + // Save the starting parameters + SimTK::ExponentialSpringParameters p0 = spr.getParameters(); + + // Change the properties systematically + SimTK::ExponentialSpringParameters p1 = p0; + + // Shape + double delta = 0.1; + Vec3 d; + p1.getShapeParameters(d[0], d[1], d[2]); + p1.setShapeParameters(d[0] + delta, d[1], d[2]); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setShapeParameters(d[0], d[1] + delta, d[2]); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setShapeParameters(d[0], d[1], d[2] + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Normal Viscosity + double value; + value = p1.getNormalViscosity(); + p1.setNormalViscosity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Friction Elasticity + value = p1.getFrictionElasticity(); + p1.setFrictionElasticity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Friction Viscosity + value = p1.getFrictionViscosity(); + p1.setFrictionViscosity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Sliding Time Constant + value = p1.getSlidingTimeConstant(); + p1.setSlidingTimeConstant(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Settle Velocity + value = p1.getSettleVelocity(); + p1.setSettleVelocity(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Settle Acceleration + value = p1.getSettleAcceleration(); + p1.setSettleAcceleration(value + delta); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Initial Coefficients of Friction + double mus = p1.getInitialMuStatic(); + double muk = p1.getInitialMuKinetic(); + p1.setInitialMuStatic(muk - delta); // Changes muk also + mus = p1.getInitialMuStatic(); + muk = p1.getInitialMuKinetic(); + SimTK_TEST_EQ(mus, muk); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + p1.setInitialMuKinetic(mus + delta); // Changes mus also + SimTK_TEST_EQ(mus, muk); + spr.setParameters(p1); + spr.assertPropertiesAndParametersEqual(); + + // Return to the starting parameters + spr.setParameters(p0); + spr.assertPropertiesAndParametersEqual(); +} +//_____________________________________________________________________________ +void +ExponentialContactTester:: +testSerialization() { + // Serialize the current model + std::string fileName = "testExponentialContact.osim"; + model->print(fileName); + ExponentialSpringParameters p = sprEC[0]->getParameters(); + + // Deserialize the model + Model modelCopy(fileName); + + // Get the 0th contact instance + ExponentialContact* spr = sprEC[0]; + ExponentialContact* sprCopy = dynamic_cast( + &modelCopy.updForceSet().get(spr->getName())); + ExponentialSpringParameters pCopy = sprCopy->getParameters(); + + // Parameters should be equal + SimTK_ASSERT_ALWAYS(pCopy == p, + "Deserialized parameters are not equal to original parameters."); +} + +//_____________________________________________________________________________ +void +ExponentialContactTester:: +simulate() +{ + // Initialize the state + // Note that model components have already been connected and the + // SimTK::System has already been built. + // See TestExponentialContact::buildModel(). + SimTK::State& state = model->initializeState(); // Set initial conditions double dz = 1.0; - if (blockES != NULL) - setInitialConditions(state, blockES->getMobilizedBody(), dz); + if (blockEC != NULL) + setInitialConditions(state, blockEC->getMobilizedBody(), dz); if (blockHC != NULL) setInitialConditions(state, blockHC->getMobilizedBody(), -dz); @@ -597,13 +733,14 @@ The HuntCrossleyForce class is tested elsewhere (e.g., see testContactGeometry.cpp and testForce.cpp). */ int main(int argc, char** argv) { try { - ExponentialSpringTester tester; + ExponentialContactTester tester; int status = tester.parseCommandLine(argc, argv); if (status < 0) { cout << "Exiting..." << endl; return 1; } tester.buildModel(); + tester.test(); tester.simulate(); } catch (const OpenSim::Exception& e) { diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index cdfb53dc0e..45b6d50684 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1316,7 +1316,7 @@ void testHuntCrossleyForce() { // In that case the force generated by contact should be identically body // weight in vertical and zero else where. OpenSim::HuntCrossleyForce& contact = - (OpenSim::HuntCrossleyForce&)osimModel.getForceSet().get("contact"); + (OpenSim::HuntCrossleyForce&)osimModel.getForceSet().get("contact"); Array contact_force = contact.getRecordValues(osim_state); ASSERT_EQUAL( From fd85ee5e3c8507e3094b7d95b57f6f78b4922253 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 19:33:59 -0500 Subject: [PATCH 21/55] Removed unused method. --- .../Test/testExponentialContact.cpp | 40 ++----------------- 1 file changed, 3 insertions(+), 37 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 931fb92b5a..3a12e93bf4 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -51,9 +51,9 @@ using namespace OpenSim; //============================================================================= /** Class ExponentialContactTester provides a scope and framework for -evaluating and testing the ExponentialSpringForce class. Using a class gets -a lot of variables out of the global scope and allows for more structured -memory cleanup. */ +evaluating and testing the ExponentialContact class. Using a testing class, as +opposed to just a main() and C-style procedures, gets a lot of variables out +of the global scope and allows for more structured memory management. */ class ExponentialContactTester { public: @@ -102,7 +102,6 @@ class ExponentialContactTester void setForceData( double t, const SimTK::Vec3& point, const SimTK::Vec3& force); void setForceDataHeader(); - void addDecorations(); // Test stuff not covered elsewhere. void test(); @@ -146,38 +145,6 @@ class ExponentialContactTester }; // End class ExponentialContactTester declarations -//_____________________________________________________________________________ -void -ExponentialContactTester:: -addDecorations() -{ - // Ground - Ground& ground = model->updGround(); - SimTK::DecorativeBrick* floor = - new SimTK::DecorativeBrick(Vec3(2.0, 0.5, 2.0)); - floor->setColor(Green); - floor->setOpacity(0.1); - SimTK::Body& grndBody = ground.updMobilizedBody().updBody(); - grndBody.addDecoration(Transform(Vec3(0, -0.5, 0)), *floor); - - // Exponential Contact Block - if (blockEC) { - SimTK::Body& body = blockEC->updMobilizedBody().updBody(); - SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(hs)); - brick->setColor(SimTK::Blue); - body.addDecoration(SimTK::Transform(), *brick); - } - - // Hunt-Crossley Block - if (blockHC) { - SimTK::Body& body = blockHC->updMobilizedBody().updBody(); - SimTK::DecorativeBrick* brick = - new SimTK::DecorativeBrick(SimTK::Vec3(hs)); - brick->setColor(SimTK::Red); - body.addDecoration(SimTK::Transform(), *brick); - } -} //_____________________________________________________________________________ int ExponentialContactTester:: @@ -517,7 +484,6 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, default: cout << "Unrecognized set of initial conditions!" << endl; } - } //----------------------------------------------------------------------------- From a5d579317fd09b67ceeb2c8c64346d5608254cae Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 19:40:26 -0500 Subject: [PATCH 22/55] Tweaked/shortened command-line options. --- .../Test/testExponentialContact.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 3a12e93bf4..d581e70d99 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -59,8 +59,8 @@ class ExponentialContactTester public: // Contact choices enum ContactChoice { - ExpSpr = 0, - HuntCross, + Exp = 0, + Hunt, Both }; @@ -126,7 +126,7 @@ class ExponentialContactTester double tf{5.0}; // Command line options and their defaults - ContactChoice whichContact{ExpSpr}; + ContactChoice whichContact{Exp}; InitialConditionsChoice whichInit{Slide}; bool noDamp{false}; bool applyFx{false}; @@ -156,10 +156,10 @@ parseCommandLine(int argc, char** argv) option = argv[i]; // Contact choice - if (option == "ExpSpr") - whichContact = ExpSpr; - else if (option == "HuntCross") - whichContact = HuntCross; + if (option == "Exp") + whichContact = Exp; + else if (option == "Hunt") + whichContact = Hunt; else if (option == "Both") whichContact = Both; @@ -205,7 +205,7 @@ printUsage() << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; cout << "\tInitCond (choose one): Static Bounce Slide Spin Tumble "; cout << endl; - cout << "\t Contact (choose one): ExpSpr HuntCross Both" << endl << endl; + cout << "\t Contact (choose one): Exp Hunt Both" << endl << endl; cout << "All arguments are optional. If no arguments are specified, "; cout << "a 'Slide' will" << endl; @@ -234,11 +234,11 @@ buildModel() model->setGravity(gravity); model->setName("TestExponentialContact"); switch (whichContact) { - case ExpSpr: + case Exp: blockEC = addBlock("EC"); addExponentialContact(blockEC); break; - case HuntCross: + case Hunt: blockHC = addBlock("HC"); addHuntCrossleyContact(blockHC); break; @@ -384,7 +384,7 @@ addExponentialContact(OpenSim::Body* block) // Loop over the corners std::string name = ""; for (int i = 0; i < n; ++i) { - name = "ExpSpr" + std::to_string(i); + name = "Exp" + std::to_string(i); sprEC[i] = new OpenSim::ExponentialContact(floorXForm, block->getName(), corner[i], params); sprEC[i]->setName(name); From e5d97e16e1d85d4f2d0268a755a574354f523d95 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 18 Jun 2022 20:03:45 -0500 Subject: [PATCH 23/55] Minor name change: FxES -> FxEC --- OpenSim/Simulation/Test/testExponentialContact.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index d581e70d99..d0fc99b84e 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -140,7 +140,7 @@ class ExponentialContactTester OpenSim::ExponentialContact* sprEC[n]{NULL}; OpenSim::HuntCrossleyForce* sprHC[n]{NULL}; Storage fxData; - ExternalForce* fxES{NULL}; + ExternalForce* fxEC{NULL}; ExternalForce* fxHC{NULL}; }; // End class ExponentialContactTester declarations @@ -261,15 +261,13 @@ buildModel() setForceData(tf, point, force); if (blockEC) { cout << "Adding fx for " << blockEC->getName() << endl; - fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); - fxES = new ExternalForce(fxData,"force", "point", "", + fxEC = new ExternalForce(fxData,"force", "point", "", blockEC->getName(), "ground", blockEC->getName()); - fxES->setName("externalforceES"); - model->addForce(fxES); + fxEC->setName("externalforceES"); + model->addForce(fxEC); } if (blockHC) { cout << "Adding fx for " << blockHC->getName() << endl; - fxData.print("C:\\Users\\fcand\\Documents\\fxData.sto"); fxHC = new ExternalForce(fxData, "force", "point", "", blockHC->getName(), "ground", blockHC->getName()); fxHC->setName("externalforceHC"); From ff0765867bf0a56633d552b9aca7354d711f7077 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 22 Jun 2022 12:09:40 -0500 Subject: [PATCH 24/55] Polished documentation comments. --- OpenSim/Simulation/Model/ExponentialContact.h | 30 ++++++++----------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index 5c482654d1..f0daa1f51b 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -289,13 +289,11 @@ class OSIMSIMULATION_API ExponentialContact : public Force { // PROPERTIES //------------------------------------------------------------------------- OpenSim_DECLARE_PROPERTY(contact_plane_transform, SimTK::Transform, - "Location and orientation of the contact plane wrt Ground. \ - The positive z-axis of the contact plane defines the normal."); + "Orientation and location of the contact plane wrt Ground. The positive z-axis of the contact plane defines the normal."); OpenSim_DECLARE_PROPERTY(body_name, std::string, "Name of the Body to which the resultant contact force is applied."); OpenSim_DECLARE_PROPERTY(body_station, SimTK::Vec3, - "Point on the body, expressed in the Body frame, at which the \ - resultant contact force is applied."); + "Point on the body, expressed in the Body frame, at which the resultant contact force is applied."); OpenSim_DECLARE_PROPERTY(contact_parameters, ExponentialContact::Parameters, "Customizable topology-stage parameters."); @@ -424,29 +422,25 @@ class ExponentialContact::Parameters : public Object { public: OpenSim_DECLARE_PROPERTY(exponential_shape_parameters, SimTK::Vec3, - "Shape parameters for the exponential that is used to model the \ - normal force: d0 (0.0065905 m), d1 (0.5336 N), d2 (1150.0/m)."); + "Shape parameters for the exponential that models the normal force: d0 (0.0065905 m), d1 (0.5336 N), d2 (1150.0/m)."); OpenSim_DECLARE_PROPERTY(normal_viscosity, double, - "Viscosity in the normal direction (0.5 s/m)."); + "Viscosity in the normal direction (0.5 s/m)."); OpenSim_DECLARE_PROPERTY(max_normal_force, double, - "Maximum allowed normal force (100,000.0 N)."); + "Maximum allowed normal force (100,000.0 N)."); OpenSim_DECLARE_PROPERTY(friction_elasticity, double, - "Elasticity of the linear friction spring (20,000.0 N/m)."); + "Elasticity of the friction spring (20,000.0 N/m)."); OpenSim_DECLARE_PROPERTY(friction_viscosity, double, - "Viscosity of the linear friction spring (282.8427 N*s/m)."); + "Viscosity of the friction spring (282.8427 N*s/m)."); OpenSim_DECLARE_PROPERTY(sliding_time_constant, double, - "Time constant for rise/decay between static and kinetic friction \ - conditions (0.01 s)."); + "Time constant for rise/decay between static and kinetic friction conditions (0.01 s)."); OpenSim_DECLARE_PROPERTY(settle_velocity, double, - "Velocity below which static friction conditions are triggered \ - to come into effect (0.01 m/s) ."); + "Velocity below which static friction conditions are triggered (0.01 m/s) ."); OpenSim_DECLARE_PROPERTY(settle_acceleration, double, - "Acceleration below which static friction conditions are triggered \ - to come into effect (1.0 m/s^2)."); + "Acceleration below which static friction conditions are triggered (1.0 m/s²)."); OpenSim_DECLARE_PROPERTY(initial_mu_static, double, - "Initial value of the static coefficient of friction."); + "Initial value of the static coefficient of friction."); OpenSim_DECLARE_PROPERTY(initial_mu_kinetic, double, - "Initial value of the kinetic coefficient of friction."); + "Initial value of the kinetic coefficient of friction."); /** Default constructor. */ Parameters(); From e4b341158031fe0630b338cad274bb696d2324f1 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 22 Jun 2022 12:11:44 -0500 Subject: [PATCH 25/55] Memory management and simplifications. Now deleting contact geometry associated with the Hunt-Crossley springs. Changed corner[n] from a local variable to a member variable so that it only needs to be initialized once. --- .../Test/testExponentialContact.cpp | 67 ++++++++++--------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index d0fc99b84e..4d1319acd4 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -74,7 +74,16 @@ class ExponentialContactTester }; // Constructor - ExponentialContactTester() {}; + ExponentialContactTester() { + corner[0] = Vec3( hs, -hs, hs); + corner[1] = Vec3( hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3( hs, hs, hs); + corner[5] = Vec3( hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + }; // Destructor ~ExponentialContactTester() { @@ -87,6 +96,7 @@ class ExponentialContactTester for (int i = 0; i < n; i++) { if (sprEC[i]) delete sprEC[i]; if (sprHC[i]) delete sprHC[i]; + if (geomHC[i]) delete geomHC[i]; } } @@ -122,8 +132,10 @@ class ExponentialContactTester double integ_accuracy{1.0e-5}; SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; double mass{10.0}; - double hs{0.10}; // half of a side of a cube (kind of like a radius) double tf{5.0}; + const static int n{8}; + const double hs{0.10}; // half of a side of a cube (like a radius) + Vec3 corner[n]; // Command line options and their defaults ContactChoice whichContact{Exp}; @@ -133,12 +145,12 @@ class ExponentialContactTester bool showVisuals{false}; // Model and parts - const static int n{8}; Model* model{NULL}; OpenSim::Body* blockEC{NULL}; OpenSim::Body* blockHC{NULL}; OpenSim::ExponentialContact* sprEC[n]{NULL}; OpenSim::HuntCrossleyForce* sprHC[n]{NULL}; + OpenSim::ContactGeometry* geomHC[n]{NULL}; Storage fxData; ExternalForce* fxEC{NULL}; ExternalForce* fxHC{NULL}; @@ -354,32 +366,21 @@ addExponentialContact(OpenSim::Body* block) { Ground& ground = model->updGround(); - // Corners of the block - Vec3 corner[n]; - corner[0] = Vec3(hs, -hs, hs); - corner[1] = Vec3(hs, -hs, -hs); - corner[2] = Vec3(-hs, -hs, -hs); - corner[3] = Vec3(-hs, -hs, hs); - corner[4] = Vec3(hs, hs, hs); - corner[5] = Vec3(hs, hs, -hs); - corner[6] = Vec3(-hs, hs, -hs); - corner[7] = Vec3(-hs, hs, hs); - // Contact Plane Transform Real angle = convertDegreesToRadians(90.0); Rotation floorRot(-angle, XAxis); Vec3 floorOrigin(0., -0.004, 0.); Transform floorXForm(floorRot, floorOrigin); - // Specify non-default contact parameters - SimTK::ExponentialSpringParameters params; + // Contact Parameters + SimTK::ExponentialSpringParameters params; // yields default params if (noDamp) { params.setNormalViscosity(0.0); params.setFrictionViscosity(0.0); params.setInitialMuStatic(0.0); } - // Loop over the corners + // Place a spring at each of the 8 corners std::string name = ""; for (int i = 0; i < n; ++i) { name = "Exp" + std::to_string(i); @@ -401,29 +402,26 @@ addHuntCrossleyContact(OpenSim::Body* block) Vec3(0), Vec3(0, 0, -0.5 * SimTK_PI), ground, "floor"); model->addContactGeometry(floor); - // Corners of the block - Vec3 corner[n]; - corner[0] = Vec3(hs, -hs, hs); - corner[1] = Vec3(hs, -hs, -hs); - corner[2] = Vec3(-hs, -hs, -hs); - corner[3] = Vec3(-hs, -hs, hs); - corner[4] = Vec3(hs, hs, hs); - corner[5] = Vec3(hs, hs, -hs); - corner[6] = Vec3(-hs, hs, -hs); - corner[7] = Vec3(-hs, hs, hs); - - // Loop over the corners + // Place a contact sphere at each of the 8 corners std::string name = ""; for (int i = 0; i < n; ++i) { // Geometry name = "sphere_" + std::to_string(i); - OpenSim::ContactGeometry* geometry = - new ContactSphere(0.005, corner[i], *block, name); - model->addContactGeometry(geometry); + geomHC[i] = new ContactSphere(0.005, corner[i], *block, name); + model->addContactGeometry(geomHC[i]); // HuntCrossleyForce + double dissipation = 4e-1; + double mus = 0.7; + double muk = 0.465; + if (noDamp) { + dissipation = 0.0; + mus = 0.0; + muk = 0.0; + } auto* contactParams = new OpenSim::HuntCrossleyForce:: - ContactParameters(1.0e7, 4e-1, 0.7, 0.465, 0.0); + ContactParameters(1.0e7, dissipation, mus, muk, 0.0); + contactParams->addGeometry(name); contactParams->addGeometry("floor"); sprHC[i] = new OpenSim::HuntCrossleyForce(contactParams); @@ -492,6 +490,9 @@ void ExponentialContactTester:: test() { + // Don't test if the only contact is Hunt-Crossley + if (whichContact == Hunt) return; + testParameters(); testSerialization(); } From 94ac923021ab323a1096522a9e1868bafa9b60f1 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 23 Jun 2022 19:06:49 -0500 Subject: [PATCH 26/55] Added ExponentialContact.h to include file. --- OpenSim/Simulation/osimSimulation.h | 1 + 1 file changed, 1 insertion(+) diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 173bd34a95..5894e3ca03 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -40,6 +40,7 @@ #include "Model/ContactSphere.h" #include "Model/CoordinateSet.h" #include "Model/ElasticFoundationForce.h" +#include "Model/ExponentialContact.h" #include "Model/HuntCrossleyForce.h" #include "Model/SmoothSphereHalfSpaceForce.h" #include "Model/Ligament.h" From 882e83f49fa1071fd5c8d64e98b7801bbad23838 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 23 Jun 2022 19:07:13 -0500 Subject: [PATCH 27/55] Minor name changes. --- OpenSim/Simulation/Test/testExponentialContact.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 4d1319acd4..4d457f6e0e 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -244,7 +244,7 @@ buildModel() // Create the bodies model = new Model(); model->setGravity(gravity); - model->setName("TestExponentialContact"); + model->setName("BouncingBlock_ExponentialContact"); switch (whichContact) { case Exp: blockEC = addBlock("EC"); @@ -589,7 +589,7 @@ void ExponentialContactTester:: testSerialization() { // Serialize the current model - std::string fileName = "testExponentialContact.osim"; + std::string fileName = "BouncingBlock_ExponentialContact_Serialized.osim"; model->print(fileName); ExponentialSpringParameters p = sprEC[0]->getParameters(); From ccb4258ec8366b7a1a85bc3210ddd7680a911469 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Thu, 23 Jun 2022 19:08:18 -0500 Subject: [PATCH 28/55] Added test routine for class ExponentialContact. I'm not yet finished adding tests, but the code is running. --- OpenSim/Simulation/Test/testForces.cpp | 97 ++++++++++++++++++++++++-- 1 file changed, 91 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 45b6d50684..35a6adc9ad 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -28,12 +28,13 @@ // 2. BushingForce // 3. ElasticFoundationForce // 4. HuntCrossleyForce -// 5. SmoothSphereHalfSpaceForce -// 6. CoordinateLimitForce -// 7. RotationalCoordinateLimitForce -// 8. ExternalForce -// 9. PathSpring -// 10. ExpressionBasedPointToPointForce +// 5. ExponentialContact +// 6. SmoothSphereHalfSpaceForce +// 7. CoordinateLimitForce +// 8. RotationalCoordinateLimitForce +// 9. ExternalForce +// 10. PathSpring +// 11. ExpressionBasedPointToPointForce // 11. Blankevoort1991Ligament // // Add tests here as Forces are added to OpenSim @@ -65,6 +66,7 @@ void testExpressionBasedBushingForceTranslational(); void testExpressionBasedBushingForceRotational(); void testElasticFoundation(); void testHuntCrossleyForce(); +void testExponentialContact(); void testSmoothSphereHalfSpaceForce(); void testCoordinateLimitForce(); void testCoordinateLimitForceRotational(); @@ -127,6 +129,13 @@ int main() { cout << e.what() <getForceStorage().print("bouncing_block_EC_forces.sto"); + +/* + // Bouncing block should have settled on ground due to dissipation. + // The force generated by contact then should be identically body + // weight vertically and zero horizontally. + OpenSim::ExponentialContact& spr = + (OpenSim::ExponentialContact&)osimModel.getForceSet().get("Exp0"); + + Array contact_force = spr.getRecordValues(state); + ASSERT_EQUAL(contact_force[0], 0.0, 1e-4); // no horizontal force + // vertical should be the weight + ASSERT_EQUAL(contact_force[1], -block.getMass()*gravity_vec[1], 1e-3); + ASSERT_EQUAL(contact_force[2], 0.0, 1e-4); // no horizontal force + ASSERT_EQUAL(contact_force[3], 0.0, 1e-4); // no torque on the ball + ASSERT_EQUAL(contact_force[4], 0.0, 1e-4); // no torque on the ball + ASSERT_EQUAL(contact_force[5], 0.0, 1e-4); // no torque on the ball + + // Before exiting lets see if copying the force works + OpenSim::ExponentialContact* sprCopy = spr.clone(); + bool isEqual = (*sprCopy == spr); + if (!isEqual) { + spr.print("originalForce.xml"); + sprCopy->print("copyOfForce.xml"); + } + + ASSERT(isEqual); + + */ +} + + + + // Test our wrapping of Hunt-Crossley force in OpenSim // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. @@ -1342,6 +1426,7 @@ void testHuntCrossleyForce() { ASSERT(isEqual); } + // Test our wrapping of SimTK::SmoothSphereHalfSpaceForce. // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. From 9eca269490bfaf589e81e075615dc7af3a8c6ee2 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 11 Sep 2022 12:41:05 -0500 Subject: [PATCH 29/55] Added two new initial conditions. 1) SpinSlide 2) SpinTop --- .../Test/testExponentialContact.cpp | 38 +++++++++++++++++-- 1 file changed, 34 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 4d457f6e0e..918ec402f1 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -70,6 +70,8 @@ class ExponentialContactTester Bounce, Slide, Spin, + SpinSlide, + SpinTop, Tumble }; @@ -184,6 +186,10 @@ parseCommandLine(int argc, char** argv) whichInit = Slide; else if (option == "Spin") whichInit = Spin; + else if (option == "SpinSlide") + whichInit = SpinSlide; + else if (option == "SpinTop") + whichInit = SpinTop; else if (option == "Tumble") whichInit = Tumble; @@ -214,7 +220,7 @@ printUsage() { cout << endl << "Usage:" << endl; cout << "$ testExponetialContact " - << "[InitCond] [Contact] [NoDamp] [ApplyFx] [Vis]" << endl; + << "[InitCond] [Contact] [NoDamp] [Fx] [Vis]" << endl; cout << "\tInitCond (choose one): Static Bounce Slide Spin Tumble "; cout << endl; cout << "\t Contact (choose one): Exp Hunt Both" << endl << endl; @@ -413,7 +419,7 @@ addHuntCrossleyContact(OpenSim::Body* block) // HuntCrossleyForce double dissipation = 4e-1; double mus = 0.7; - double muk = 0.465; + double muk = 0.5; if (noDamp) { dissipation = 0.0; mus = 0.0; @@ -432,11 +438,14 @@ addHuntCrossleyContact(OpenSim::Body* block) } } //_____________________________________________________________________________ +// dz allows for the body to be shifted along the z axis. This is useful for +// displacing the Exp and Hunt models. void ExponentialContactTester:: setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, double dz) { + SimTK::Rotation R; SimTK::Vec3 pos(0.0, 0.0, dz); SimTK::Vec3 vel(0.0); SimTK::Vec3 angvel(0.0); @@ -463,11 +472,32 @@ setInitialConditions(SimTK::State& state, const SimTK::MobilizedBody& body, pos[0] = 0.0; pos[1] = hs; vel[0] = 0.0; + angvel[1] = 8.0 * SimTK::Pi; + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; + case SpinSlide: + pos[0] = 1.0; + pos[1] = hs; + vel[0] = -3.0; angvel[1] = 4.0 * SimTK::Pi; body.setQToFitTranslation(state, pos); body.setUToFitLinearVelocity(state, vel); body.setUToFitAngularVelocity(state, angvel); break; + case SpinTop: + R.setRotationFromAngleAboutNonUnitVector( + convertDegreesToRadians(54.74), Vec3(1, 0, 1)); + pos[0] = 0.0; + pos[1] = 2.0*hs; + vel[0] = 0.0; + angvel[1] = 1.5 * SimTK::Pi; + body.setQToFitRotation(state, R); + body.setQToFitTranslation(state, pos); + body.setUToFitLinearVelocity(state, vel); + body.setUToFitAngularVelocity(state, angvel); + break; case Tumble: pos[0] = -1.5; pos[1] = 2.0 * hs; @@ -490,7 +520,7 @@ void ExponentialContactTester:: test() { - // Don't test if the only contact is Hunt-Crossley + // Don't run tests if the only contact is Hunt-Crossley if (whichContact == Hunt) return; testParameters(); @@ -627,7 +657,7 @@ simulate() // Integrate Manager manager(*model); - manager.getIntegrator().setMaximumStepSize(0.02); + manager.getIntegrator().setMaximumStepSize(0.03); manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); From fc38ee5165c190c7878c540f31ba7a46582bf60b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 11 Sep 2022 14:13:04 -0500 Subject: [PATCH 30/55] Improved output formatting. --- .../Test/testExponentialContact.cpp | 60 +++++++++++++++++-- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 918ec402f1..e4eabfc2ce 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -105,6 +105,7 @@ class ExponentialContactTester // Command line parsing and usage int parseCommandLine(int argc, char** argv); void printUsage(); + void printConditions(); // Model Creation void buildModel(); @@ -132,6 +133,7 @@ class ExponentialContactTester // Simulation related double integ_accuracy{1.0e-5}; + double dt_max{0.03}; SimTK::Vec3 gravity{SimTK::Vec3(0, -9.8065, 0)}; double mass{10.0}; double tf{5.0}; @@ -221,8 +223,8 @@ printUsage() cout << endl << "Usage:" << endl; cout << "$ testExponetialContact " << "[InitCond] [Contact] [NoDamp] [Fx] [Vis]" << endl; - cout << "\tInitCond (choose one): Static Bounce Slide Spin Tumble "; - cout << endl; + cout << "\tInitCond (choose one): Static Bounce Slide Spin "; + cout << "SpinSlide SpinTop Tumble" << endl; cout << "\t Contact (choose one): Exp Hunt Both" << endl << endl; cout << "All arguments are optional. If no arguments are specified, "; @@ -242,6 +244,52 @@ printUsage() cout << "$ testExponentialContact Bounce Both NoDamp Vis" << endl << endl; } //_____________________________________________________________________________ +void +ExponentialContactTester:: +printConditions() { + std::string modelDes; + std::string contactDes; + switch (whichContact) { + case (Exp): + modelDes = "One Block"; + contactDes = "Exponential"; + break; + case (Hunt): + modelDes = "One Block"; + contactDes = "Hunt Crossley"; + break; + case (Both): + modelDes = "Two Blocks"; + contactDes = "1 Block Exponential, 1 Block Hunt Crossley"; + } + + std::string motion; + switch (whichInit) { + case (Static): motion = "Sitting Still"; break; + case (Bounce): motion = "Bouncing"; break; + case (Slide): motion = "Sliding"; break; + case (Spin): motion = "Spinning"; break; + case (SpinSlide): motion = "Spinning & Sliding"; break; + case (SpinTop): motion = "Spinnning like a Top"; break; + case (Tumble): motion = "Tumbling Horizontally"; + } + + std::string appliedForce; + if (applyFx) + appliedForce = "Ramping Fx after 25 sec."; + else + appliedForce = "None"; + + cout << endl << endl; + cout << " model: " << modelDes << " (10 kg, 6 dof, 10x10x10 cm^3)" << endl; + cout << " contact: " << contactDes << endl; + cout << " motion: " << motion << endl; + cout << " applied force: " << appliedForce << endl; + cout << " integ acc: " << integ_accuracy << endl; + cout << " max step size: " << dt_max << " sec" << endl; + cout << " tf: " << tf << " sec" << endl; +} +//_____________________________________________________________________________ // Build the model void ExponentialContactTester:: @@ -657,7 +705,7 @@ simulate() // Integrate Manager manager(*model); - manager.getIntegrator().setMaximumStepSize(0.03); + manager.getIntegrator().setMaximumStepSize(dt_max); manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); @@ -668,8 +716,10 @@ simulate() // Output int trys = manager.getIntegrator().getNumStepsAttempted(); int steps = manager.getIntegrator().getNumStepsTaken(); - cout << "trys = " << trys << "\t\tsteps = " << steps; - cout << "\t\tcpu time = " << runTime << endl; + printConditions(); + cout << " trys: " << trys << endl; + cout << " steps: " << steps << endl; + cout << " cpu time: " << runTime << " msec" << endl; // Write the model to file //model->print("C:\\Users\\fcand\\Documents\\block.osim"); From 56f4e22382ce39eb8639b779cf6a43f02b439ef9 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 16 Sep 2022 12:21:20 -0500 Subject: [PATCH 31/55] Added resetAnchorPoint(), removed props, polished comments. ExponentialContact::resetAnchorPoint() is used to move the friction spring zero so that it coincides with the projection of the body station on the contact plane. Properties "sliding time constant" and "settle acceleration" are no longer used. Updated / polished documentation comments. --- .../Simulation/Model/ExponentialContact.cpp | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 59798cfacf..57af26bbec 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -63,9 +63,7 @@ constructProperties() constructProperty_max_normal_force(_stkparams.getMaxNormalForce()); constructProperty_friction_elasticity(_stkparams.getFrictionElasticity()); constructProperty_friction_viscosity(_stkparams.getFrictionViscosity()); - constructProperty_sliding_time_constant(_stkparams.getSlidingTimeConstant()); constructProperty_settle_velocity(_stkparams.getSettleVelocity()); - constructProperty_settle_acceleration(_stkparams.getSettleAcceleration()); constructProperty_initial_mu_static(_stkparams.getInitialMuStatic()); constructProperty_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); } @@ -82,9 +80,7 @@ updateProperties() set_max_normal_force(_stkparams.getMaxNormalForce()); set_friction_elasticity(_stkparams.getFrictionElasticity()); set_friction_viscosity(_stkparams.getFrictionViscosity()); - set_sliding_time_constant(_stkparams.getSlidingTimeConstant()); set_settle_velocity(_stkparams.getSettleVelocity()); - set_settle_acceleration(_stkparams.getSettleAcceleration()); set_initial_mu_static(_stkparams.getInitialMuStatic()); set_initial_mu_kinetic(_stkparams.getInitialMuKinetic()); } @@ -100,9 +96,7 @@ updateParameters() _stkparams.setMaxNormalForce(get_max_normal_force()); _stkparams.setFrictionElasticity(get_friction_elasticity()); _stkparams.setFrictionViscosity(get_friction_viscosity()); - _stkparams.setSlidingTimeConstant(get_sliding_time_constant()); _stkparams.setSettleVelocity(get_settle_velocity()); - _stkparams.setSettleAcceleration(get_settle_acceleration()); _stkparams.setInitialMuStatic(get_initial_mu_static()); _stkparams.setInitialMuKinetic(get_initial_mu_kinetic()); } @@ -233,6 +227,25 @@ extendAddToSystem(SimTK::MultibodySystem& system) const mutableThis->_index = spr->getForceIndex(); } +//----------------------------------------------------------------------------- +// Utility +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +resetAnchorPoint(SimTK::State& state) const { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated! The system must be built by calling "; + msg += "Model::buildSystem() and the state must be initialized by "; + msg += "calling Model::initializeState() before calling "; + msg += "ExponentialContact::resetAnchorPoint()."; + SimTK_ASSERT(_spr!=NULL, msg); + } + _spr->resetAnchorPoint(state); +} + + //----------------------------------------------------------------------------- // ACCESSORS //----------------------------------------------------------------------------- @@ -352,18 +365,10 @@ assertPropertiesAndParametersEqual() const valB = b.getFrictionViscosity(); SimTK_TEST_EQ(valA, valB); - valA = a.get_sliding_time_constant(); - valB = b.getSlidingTimeConstant(); - SimTK_TEST_EQ(valA, valB); - valA = a.get_settle_velocity(); valB = b.getSettleVelocity(); SimTK_TEST_EQ(valA, valB); - valA = a.get_settle_acceleration(); - valB = b.getSettleAcceleration(); - SimTK_TEST_EQ(valA, valB); - valA = a.get_initial_mu_static(); valB = b.getInitialMuStatic(); SimTK_TEST_EQ(valA, valB); From 82cf83d0190345f1686087d1c8f1913e9b5f0637 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 16 Sep 2022 12:22:30 -0500 Subject: [PATCH 32/55] Added resetAnchorPoint(), removed props, polished comments. ExponentialContact::resetAnchorPoint() is used to move the friction spring zero so that it coincides with the projection of the body station on the contact plane. Properties "sliding time constant" and "settle acceleration" are no longer used. Updated / polished documentation comments. --- OpenSim/Simulation/Model/ExponentialContact.h | 84 +++++++++---------- .../Test/testExponentialContact.cpp | 46 +++++----- 2 files changed, 61 insertions(+), 69 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index f0daa1f51b..5c288f08f5 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -35,8 +35,8 @@ namespace OpenSim { //============================================================================= /** Class ExponentialContact uses an "exponential spring" as a means of modeling contact of a specified point on a Body with a contact -plane that is fixed to Ground. In this documentation, this specified point -is referred to as the "body station". Each ExponentialContact instance +plane that is fixed to Ground. This specified point is referred to in this +documentation as the "body station". Each ExponentialContact instance acts at only one body station. In practice, you should choose a number of body stations strategically located across the surface of a Body, and construct an ExponentialContact instance for each of those body stations. @@ -57,9 +57,8 @@ Aspects of the exponential contact model are described in the following Under the covers, the OpenSim::ExponentialContact class encapsulates two SimTK objects: ExponentialSpringForce and ExponentialSpringParameters. -For the details concerning the contact model, see the Simbody API -documentation for SimTK::ExponentialSpringForce. A condensed version of -that documentation is provided here. +For the details concerning these classes, see the Simbody API +documentation. A condensed version of that documentation is provided here. ---------------------------------- Computations and Coordinate Frames @@ -111,19 +110,17 @@ which has the form of the Hunt & Crossley damping model: The friction force is computed by blending two different friction models. The blending is performed based on the 'Sliding' State of the -ExponentialSpringForce class. 'Sliding' is a continuous state variable (a Z -in Simbody vocabulary) that characterizes the extent to which either static -or kinetic conditions are present. +ExponentialSpringForce class. #### Friction Model 1 - Pure Damping (Sliding = 1.0) -When the body station is moving with respect to the contact plane, the +When the body station is sliding with respect to the contact plane, the friction force is computed using a simple damping term: fricDamp = −cxy vxy where cxy is the damping coefficient in the contact plane and vxy is the -velocity of the body station in the contact plane. However, the magnitude -of the total frictional force is not allowed to exceed the frictional limit: +velocity of the body station in the contact plane. The magnitude of the +total frictional force is not allowed to exceed the frictional limit: fricLimit = μ fz if (|fricDamp| > fricLimit) @@ -175,7 +172,7 @@ Sliding State: fricDampBlend = fricDampSpr + (fricDamp − fricDampSpr)*Sliding fricBlend = fricElasBlend + fricDampBlend -Thus, Model 1 (Pure Damping) dominates as Sliding → 1.0, and Model 2 +Model 1 (Pure Damping) dominates as Sliding → 1.0, and Model 2 (Damped Linear Spring) dominates as Sliding → 0.0. #### Moving the Friction Spring Zero @@ -196,36 +193,19 @@ friction (μ) is calculated based on the value of the Sliding State: μ = μₛ − Sliding*(μₛ − μₖ) -The time derivative of Sliding, SlidingDot, is used to drive Sliding toward -the extremes of 0.0 or 1.0, depending on the following criteria: +The value of Sliding is calculated using a continuous function of the +ratio of the speed of the elastic anchor point (ṗ₀) to the settle velocity +(vSettle). vSettle is a customizable topology-stage parameter that represents +the speed at which a body station settles into a static state with respect to +the contact plane. See SimTK::ExponentialSpringParameters::setSettleVelocity() +for details. In particular, -- If the frictional spring force (fricSpr) exceeded the frictional limit at -any point during its calculation, Sliding is driven toward 1.0 (rise): + ṗ₀ = |Δp₀| / Δt + Sliding = SimTK::stepUp( SimTK::clamp(0.0, ṗ₀/vSettle, 1.0) ) - SlidingDot = (1.0 − Sliding)/tau - -- If the frictional spring force (fricSpr) does not exceed the frictional -limit at any point during its calculation and if the kinematics of the body -station are near static equilibrium, Sliding is driven toward 0.0 (decay): - - SlidingDot = −Sliding/tau - -The threshold for being "near" static equilibrium is established by two -parameters, vSettle and aSettle. When the velocity and acceleration of the -body station relative to the contact plane are below vSettle and aSettle, -respectively, static equilibrium is considered effectively reached. - -During a simulation, once a rise or decay is triggered, the rise or decay -continues uninterrupted until Sliding crosses 0.95 or 0.05, respectively. Once -these thresholds have been crossed, the criteria for rise and decay are again -monitored. - -In the above equations for SlidingDot, tau is the characteristic time it takes -for the Sliding State to rise or decay. The default value of tau is 0.01 sec. -The motivation for using a continuous state variable is that, although the -transition between static and kinetic may happen quickly, it does not happen -instantaneously. Evolving Sliding based on a differential equation ensures -that μ is continuous and that the blending of friction models is well behaved. +where Δp₀ is the change in p₀ and Δt is the change in time since the last +successful integration step. When ṗ₀ ≥ vSettle, Sliding = 1.0, and as +ṗ₀ → 0.0, Sliding → 0.0. ----------------------- CUSTOMIZABLE PARAMETERS @@ -266,7 +246,7 @@ allocated from the heap to be deleted. Therefore, also note that the parameter values possessed by an ExponentialContact instance do not necessarily correspond to the values -held by an instance of ExponentialSpringParameters until a call to +held by a local instance of ExponentialSpringParameters until a call to ExponentialContact::setParameters() is made. The default values of the parameters are expressed in units of Newtons, @@ -321,7 +301,19 @@ class OSIMSIMULATION_API ExponentialContact : public Force { } //------------------------------------------------------------------------- - // Accessors + // Initialization + //------------------------------------------------------------------------- + /** Reset the elastic anchor point (friction spring zero) so that it + coincides with the projection of the body station onto the contact + plane. This step is often needed at the beginning of a simulation to + ensure that a simulation does not begin with large friction forces. + After this call, the elastic portion of the friction force should be 0.0 + Calling this method will invalidate the System at Stage::Dynamics. + @param state State object on which to base the reset. */ + void resetAnchorPoint(SimTK::State& state) const; + + //------------------------------------------------------------------------- + // Accessors for properties //------------------------------------------------------------------------- /** Set the transform that specifies the location and orientation of the contact plane in the Ground frame. */ @@ -352,6 +344,10 @@ class OSIMSIMULATION_API ExponentialContact : public Force { be altered. */ const SimTK::ExponentialSpringParameters& getParameters() const; + //------------------------------------------------------------------------- + // Accessors for data cache entries + //------------------------------------------------------------------------- + //------------------------------------------------------------------------- // Reporting //------------------------------------------------------------------------- @@ -398,10 +394,10 @@ getting and setting contact parameters (directly anyway) but rather provides the infrastructure for making the underlying SimTK::ExponentialSpringForce and SimTK::ExponentialSpringParameters classes available in OpenSim. -More specifically, this class mainly does 3 things: +More specifically, this class chiefly does 3 things: - Implements OpenSim Properties for most of the customizable contact parameters of class OpenSim::ExponentialContact, enabling those parameters -to be serialized to and de-serialized from file. +to be serialized and de-serialized from file. - Provides a member variable (_stkparams) for storing non-default parameters prior to the creation of an underlying SimTK::ExponentialSpringForce object. During model initialization, when the SimTK::ExponetialSpringForce object is diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index e4eabfc2ce..72c1257396 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -626,24 +626,12 @@ testParameters() spr.setParameters(p1); spr.assertPropertiesAndParametersEqual(); - // Sliding Time Constant - value = p1.getSlidingTimeConstant(); - p1.setSlidingTimeConstant(value + delta); - spr.setParameters(p1); - spr.assertPropertiesAndParametersEqual(); - // Settle Velocity value = p1.getSettleVelocity(); p1.setSettleVelocity(value + delta); spr.setParameters(p1); spr.assertPropertiesAndParametersEqual(); - // Settle Acceleration - value = p1.getSettleAcceleration(); - p1.setSettleAcceleration(value + delta); - spr.setParameters(p1); - spr.assertPropertiesAndParametersEqual(); - // Initial Coefficients of Friction double mus = p1.getInitialMuStatic(); double muk = p1.getInitialMuKinetic(); @@ -703,6 +691,11 @@ simulate() if (blockHC != NULL) setInitialConditions(state, blockHC->getMobilizedBody(), -dz); + // Reset the elastic anchor point for each ExponentialContact instance + for (int i = 0; i < n; ++i) { + if (sprEC[i] != NULL) sprEC[i]->resetAnchorPoint(state); + } + // Integrate Manager manager(*model); manager.getIntegrator().setMaximumStepSize(dt_max); @@ -740,27 +733,30 @@ the ExponentialContact class for contact and the other using the HuntCrossleyForce class) can be created and visualized simultaneously. For an assessment of computational performance, just one block should be -simulated at a time. Number of integration trys and steps, as well as cpu time, -are reported. +simulated at a time. Number of integration trys and steps, as well as cpu +time, are reported. -Choice of initial conditions can be made in order to generate the following +Choice of initial conditions can be made in order to simulate the following motions: 1) Static (y = 0.1 m, sitting at rest on the floor) 2) Bouncing (y = 1.0 m, dropped) - 3) Sliding (y = 0.2 m, vx = -2.0 m/s) - 4) Spinning (y = 0.2 m, vx = 0.0 m/s, wy = 2.0 pi rad/sec) - 5) Tumbling (py = 2.0 m, vx = -2.0 m/s, wz = 2.0 pi rad/sec) + 3) Sliding (y = 0.2 m, vx = -4.0 m/s) + 4) Spinning (y = 0.1 m, vx = 0.0 m/s, wy = 8.0 pi rad/sec) + 5) Spining and Sliding (y = 0.1 m, vx = -3.0 m/s, wy = 4.0 pi rad/sec) + 6) Spinning like a Top. (y = 0.2 m, vx = 0.0 m/s, wy = 1.5 pi rad/sec) + 7) Tumbling (py = 0.2 m, vx = -1.0 m/s, wz = 2.0 pi rad/sec) Additional options allow the following to be specified: NoDamp Parameters are chosen to eliminate all energy dissipation. - ApplyFx A ramping horizontal force (Fx) is applied after 5.0 sec. + Fx A ramping horizontal force (Fx) is applied. + Vis Open a visualization window. If no external force is applied, tf = 5.0 s. -If an external force is applied, tf = 10.0 s and this force ramps up -linearly from a value of fx = 0.0 at t = 5.0 s to a value of -fx = |mass*g| at t = 10.0 s. The force is not ramped up prior to t = 5.0 s -in order to allow the block an opportunity to come more fully to rest. +If a horizontal force is applied, tf = 30.0 s and the force ramps up +linearly from a value of fx = 0.0 at t = 25.0 s to a value of +fx = |mass*g| at t = 30.0 s. The force is not ramped up prior to t = 25.0 s +in order to allow the block an opportunity to come fully to rest. This ramping profile was done with the "Static" initial condition choice in mind so that friction models could be evaluated more critically. In particular, a static block should not start sliding until fx > μₛ Fₙ. @@ -774,8 +770,8 @@ For ExponentialContact, the following things are tested: f) reporting g) serialization -The HuntCrossleyForce class is tested elsewhere -(e.g., see testContactGeometry.cpp and testForce.cpp). */ +The HuntCrossleyForce class is tested elsewhere (e.g., see +testContactGeometry.cpp and testForce.cpp). */ int main(int argc, char** argv) { try { ExponentialContactTester tester; From 11c8f366e6282e8a41ac8b9008d5c6633ae669bf Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 16 Sep 2022 21:07:28 -0500 Subject: [PATCH 33/55] Added accessors for states. --- .../Simulation/Model/ExponentialContact.cpp | 69 +++++++++++++++++-- OpenSim/Simulation/Model/ExponentialContact.h | 40 ++++++++++- 2 files changed, 104 insertions(+), 5 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 57af26bbec..849a357d9a 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -1,4 +1,4 @@ -/* -------------------------------------------------------------------------- * +/* -------------------------------------------------------------------------- * * OpenSim: ExponentialContact.cpp * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * @@ -245,9 +245,8 @@ resetAnchorPoint(SimTK::State& state) const { _spr->resetAnchorPoint(state); } - //----------------------------------------------------------------------------- -// ACCESSORS +// ACCESSORS for properties //----------------------------------------------------------------------------- //_____________________________________________________________________________ void ExponentialContact:: @@ -280,6 +279,68 @@ getParameters() const return get_contact_parameters().getSimTKParameters(); } + +//----------------------------------------------------------------------------- +// ACCESSORS for states +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +void +ExponentialContact:: +setMuStatic(SimTK::State& state, SimTK::Real mus) { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated!."; + SimTK_ASSERT(_spr != NULL, msg); + } + _spr->setMuStatic(state, mus); +} +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getMuStatic(const SimTK::State& state) const { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated!."; + SimTK_ASSERT(_spr != NULL, msg); + } + return _spr->getMuStatic(state); +} + +//_____________________________________________________________________________ +void +ExponentialContact:: +setMuKinetic(SimTK::State& state, SimTK::Real mus) { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated!."; + SimTK_ASSERT(_spr != NULL, msg); + } + _spr->setMuKinetic(state, mus); +} +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getMuKinetic(const SimTK::State& state) const { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated!."; + SimTK_ASSERT(_spr != NULL, msg); + } + return _spr->getMuKinetic(state); +} + +//_____________________________________________________________________________ +SimTK::Real +ExponentialContact:: +getSliding(const SimTK::State& state) const { + if (_spr == NULL) { + std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; + msg += " not yet allocated!."; + SimTK_ASSERT(_spr != NULL, msg); + } + return _spr->getSliding(state); +} + //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- @@ -319,7 +380,7 @@ getRecordValues(const SimTK::State& state) const const SimTK::Force& abstractForce = forceSubsys.getForce(_index); const auto& spr = (SimTK::ExponentialSpringForce&)(abstractForce); - SimTK::Vec3 p0 = spr.getFrictionSpringZeroPosition(state); + SimTK::Vec3 p0 = spr.getAnchorPointPosition(state); SimTK::Vec3 station = spr.getStationPosition(state); SimTK::Vec3 normal = spr.getNormalForce(state); SimTK::Vec3 friction = spr.getFrictionForce(state); diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index 5c288f08f5..c295e79dd9 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -344,10 +344,48 @@ class OSIMSIMULATION_API ExponentialContact : public Force { be altered. */ const SimTK::ExponentialSpringParameters& getParameters() const; + //------------------------------------------------------------------------- + // Accessors for states + //------------------------------------------------------------------------- + /** Set the static coefficient of friction (μₛ) for this exponential + spring. The value of μₛ is held in the System's State object. Unlike the + parameters managed by ExponentialSpringParameters, μₛ can be set at any + time during a simulation. A change to μₛ will invalidate the System at + Stage::Dynamics, but not at Stage::Topology. + @param state State object that will be modified. + @param mus %Value of the static coefficient of friction. No upper bound. + 0.0 ≤ μₛ. If μₛ < μₖ, μₖ is set equal to μₛ. */ + void setMuStatic(SimTK::State& state, SimTK::Real mus); + + /** Get the static coefficient of friction (μₛ) held by the specified + state for this exponential spring. + @param state State object from which to retrieve μₛ. */ + SimTK::Real getMuStatic(const SimTK::State& state) const; + + /** Set the kinetic coefficient of friction (μₖ) for this exponential + spring. The value of μₖ is held in the System's State object. Unlike the + parameters managed by ExponentialSpringParameters, μₖ can be set at any + time during a simulation. A change to μₖ will invalidate the System at + Stage::Dynamics. + @param state State object that will be modified. + @param muk %Value of the kinetic coefficient of friction. No upper bound. + 0.0 ≤ μₖ. If μₖ > μₛ, μₛ is set equal to μₖ. */ + void setMuKinetic(SimTK::State& state, SimTK::Real muk); + + /** Get the kinetic coefficient of friction (μₖ) held by the specified + state for this exponential spring. + @param state State object from which to retrieve μₖ. */ + SimTK::Real getMuKinetic(const SimTK::State& state) const; + + /** Get the Sliding state of the spring. + @param state State object from which to retrieve Sliding. */ + SimTK::Real getSliding(const SimTK::State& state) const; + //------------------------------------------------------------------------- // Accessors for data cache entries //------------------------------------------------------------------------- + //------------------------------------------------------------------------- // Reporting //------------------------------------------------------------------------- @@ -452,7 +490,7 @@ class ExponentialContact::Parameters : public Object { call OpenSim::ExponentialContact::setParameters(). */ void setSimTKParameters(const SimTK::ExponentialSpringParameters& params); - /** Get a read-only reference to the underlying SimTK paramters. This + /** Get a read-only reference to the underlying SimTK parameters. This method is used to maintain consistency between OpenSim Properties and the underlying parameters. The typical user of OpenSim::ExponentialContact will not have reason to call this method. For getting contact parameters, the From fd0227417fdf850ad37dbaa519fce5000ca14864 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 18 Sep 2022 01:38:16 -0500 Subject: [PATCH 34/55] Added accessors for data cache entries. --- .../Simulation/Model/ExponentialContact.cpp | 180 ++++++++++-------- OpenSim/Simulation/Model/ExponentialContact.h | 137 ++++++++++++- 2 files changed, 234 insertions(+), 83 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 849a357d9a..f073d646d4 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -26,6 +26,9 @@ using namespace OpenSim; using namespace std; +using SimTK::Real; +using SimTK::Vec3; +using SimTK::State; //============================================================================= @@ -33,29 +36,25 @@ using namespace std; //============================================================================= //_____________________________________________________________________________ ExponentialContact::Parameters:: -Parameters() -{ +Parameters() { setNull(); constructProperties(); } //_____________________________________________________________________________ ExponentialContact::Parameters:: -Parameters(const SimTK::ExponentialSpringParameters& params) -{ +Parameters(const SimTK::ExponentialSpringParameters& params) { setNull(); _stkparams = params; constructProperties(); } //_____________________________________________________________________________ -void ExponentialContact::Parameters::setNull() -{ +void ExponentialContact::Parameters::setNull() { setAuthors("F. C. Anderson"); } //_____________________________________________________________________________ void ExponentialContact::Parameters:: -constructProperties() -{ +constructProperties() { SimTK::Vec3 shape; _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); constructProperty_exponential_shape_parameters(shape); @@ -71,8 +70,7 @@ constructProperties() // Update the Properties based on the SimTK::ExponentialSpringParameters. void ExponentialContact::Parameters:: -updateProperties() -{ +updateProperties() { SimTK::Vec3 shape; _stkparams.getShapeParameters(shape[0], shape[1], shape[2]); set_exponential_shape_parameters(shape); @@ -88,8 +86,7 @@ updateProperties() // Update the SimTK::ExponentialSpringParamters based on the Properties. void ExponentialContact::Parameters:: -updateParameters() -{ +updateParameters() { const SimTK::Vec3 shape = get_exponential_shape_parameters(); _stkparams.setShapeParameters(shape[0], shape[1], shape[2]); _stkparams.setNormalViscosity(get_normal_viscosity()); @@ -113,8 +110,7 @@ updateParameters() // interface (i.e., through _stkparams). void ExponentialContact::Parameters:: -updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) -{ +updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { Super::updateFromXMLNode(node, versionNumber); updateParameters(); // catching any invalid property values in the process updateProperties(); // pushes valid parameters back to the properties. @@ -123,8 +119,7 @@ updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) // Note that the OpenSim Properties are updated as well. void ExponentialContact::Parameters:: -setSimTKParameters(const SimTK::ExponentialSpringParameters& params) -{ +setSimTKParameters(const SimTK::ExponentialSpringParameters& params) { _stkparams = params; updateProperties(); } @@ -133,8 +128,7 @@ setSimTKParameters(const SimTK::ExponentialSpringParameters& params) // by this instance. const SimTK::ExponentialSpringParameters& ExponentialContact::Parameters:: -getSimTKParameters() const -{ +getSimTKParameters() const { return _stkparams; } @@ -143,8 +137,7 @@ getSimTKParameters() const // ExponentialContact //============================================================================= //_____________________________________________________________________________ -ExponentialContact::ExponentialContact() -{ +ExponentialContact::ExponentialContact() { setNull(); constructProperties(); } @@ -164,16 +157,14 @@ ExponentialContact(const SimTK::Transform& contactPlaneXform, //_____________________________________________________________________________ void ExponentialContact:: -setNull() -{ +setNull() { setAuthors("F. C. Anderson"); _spr = NULL; } //_____________________________________________________________________________ void ExponentialContact:: -constructProperties() -{ +constructProperties() { SimTK::Transform contactXForm; SimTK::Vec3 origin(0.0); Parameters params; @@ -184,15 +175,14 @@ constructProperties() } //_____________________________________________________________________________ void -ExponentialContact -::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { +ExponentialContact:: +updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { Super::updateFromXMLNode(node, versionNumber); } //_____________________________________________________________________________ void ExponentialContact:: -extendConnectToModel(OpenSim::Model& model) -{ +extendConnectToModel(OpenSim::Model& model) { // Allow based class to connect first Super::extendConnectToModel(model); @@ -207,8 +197,7 @@ extendConnectToModel(OpenSim::Model& model) //_____________________________________________________________________________ void ExponentialContact:: -extendAddToSystem(SimTK::MultibodySystem& system) const -{ +extendAddToSystem(SimTK::MultibodySystem& system) const { // Extend the OpenSim::Force parent Super::extendAddToSystem(system); @@ -234,14 +223,6 @@ extendAddToSystem(SimTK::MultibodySystem& system) const void ExponentialContact:: resetAnchorPoint(SimTK::State& state) const { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated! The system must be built by calling "; - msg += "Model::buildSystem() and the state must be initialized by "; - msg += "calling Model::initializeState() before calling "; - msg += "ExponentialContact::resetAnchorPoint()."; - SimTK_ASSERT(_spr!=NULL, msg); - } _spr->resetAnchorPoint(state); } @@ -250,22 +231,19 @@ resetAnchorPoint(SimTK::State& state) const { //----------------------------------------------------------------------------- //_____________________________________________________________________________ void ExponentialContact:: -setContactPlaneTransform(const SimTK::Transform& XContactPlane) -{ +setContactPlaneTransform(const SimTK::Transform& XContactPlane) { set_contact_plane_transform(XContactPlane); } //_____________________________________________________________________________ const SimTK::Transform& -ExponentialContact::getContactPlaneTransform() const -{ +ExponentialContact::getContactPlaneTransform() const { return get_contact_plane_transform(); } //_____________________________________________________________________________ void ExponentialContact:: -setParameters(const SimTK::ExponentialSpringParameters& params) -{ +setParameters(const SimTK::ExponentialSpringParameters& params) { ExponentialContact::Parameters& p = upd_contact_parameters(); p.setSimTKParameters(params); // The following call will invalidate the System at Stage::Topology @@ -274,12 +252,10 @@ setParameters(const SimTK::ExponentialSpringParameters& params) //_____________________________________________________________________________ const SimTK::ExponentialSpringParameters& ExponentialContact:: -getParameters() const -{ +getParameters() const { return get_contact_parameters().getSimTKParameters(); } - //----------------------------------------------------------------------------- // ACCESSORS for states //----------------------------------------------------------------------------- @@ -287,22 +263,12 @@ getParameters() const void ExponentialContact:: setMuStatic(SimTK::State& state, SimTK::Real mus) { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated!."; - SimTK_ASSERT(_spr != NULL, msg); - } _spr->setMuStatic(state, mus); } //_____________________________________________________________________________ SimTK::Real ExponentialContact:: getMuStatic(const SimTK::State& state) const { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated!."; - SimTK_ASSERT(_spr != NULL, msg); - } return _spr->getMuStatic(state); } @@ -310,22 +276,12 @@ getMuStatic(const SimTK::State& state) const { void ExponentialContact:: setMuKinetic(SimTK::State& state, SimTK::Real mus) { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated!."; - SimTK_ASSERT(_spr != NULL, msg); - } _spr->setMuKinetic(state, mus); } //_____________________________________________________________________________ SimTK::Real ExponentialContact:: getMuKinetic(const SimTK::State& state) const { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated!."; - SimTK_ASSERT(_spr != NULL, msg); - } return _spr->getMuKinetic(state); } @@ -333,22 +289,92 @@ getMuKinetic(const SimTK::State& state) const { SimTK::Real ExponentialContact:: getSliding(const SimTK::State& state) const { - if (_spr == NULL) { - std::string msg = "Underlying SimTK::ExponentialSpringForce instance"; - msg += " not yet allocated!."; - SimTK_ASSERT(_spr != NULL, msg); - } return _spr->getSliding(state); } +//----------------------------------------------------------------------------- +// ACCESSORS for data cache entries +//----------------------------------------------------------------------------- +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForceElasticPart(const State& state, bool inGround) const { + return _spr->getNormalForceElasticPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForceDampingPart(const State& state, bool inGround) const { + return _spr->getNormalForceDampingPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getNormalForce(const State& state, bool inGround) const { + return _spr->getNormalForce(state, inGround); +} +//_____________________________________________________________________________ +Real +ExponentialContact:: +getMu(const State& state) const { + return _spr->getMu(state); +} +//_____________________________________________________________________________ +Real +ExponentialContact:: +getFrictionForceLimit(const SimTK::State& state) const { + return _spr->getFrictionForceLimit(state); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForceElasticPart(const State& state, bool inGround) const { + return _spr->getFrictionForceElasticPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForceDampingPart(const State& state, bool inGround) const { + return _spr->getFrictionForceDampingPart(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getFrictionForce(const State& state, bool inGround) const { + return _spr->getFrictionForce(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getForce(const State& state, bool inGround) const { + return _spr->getForce(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getStationPosition(const State& state, bool inGround) const { + return _spr->getStationPosition(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getStationVelocity(const State& state, bool inGround) const { + return _spr->getStationVelocity(state, inGround); +} +//_____________________________________________________________________________ +Vec3 +ExponentialContact:: +getAnchorPointPosition(const State& state, bool inGround) const { + return _spr->getAnchorPointPosition(state, inGround); +} + //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- //_____________________________________________________________________________ OpenSim::Array ExponentialContact:: -getRecordLabels() const -{ +getRecordLabels() const { OpenSim::Array labels(""); std::string frameName = getBodyName(); labels.append(getName()+"."+frameName+".p0.X"); @@ -372,8 +398,7 @@ getRecordLabels() const //_____________________________________________________________________________ OpenSim::Array ExponentialContact:: -getRecordValues(const SimTK::State& state) const -{ +getRecordValues(const SimTK::State& state) const { OpenSim::Array values(0.0); const auto& forceSubsys = getModel().getForceSubsystem(); @@ -401,8 +426,7 @@ getRecordValues(const SimTK::State& state) const //_____________________________________________________________________________ void ExponentialContact:: -assertPropertiesAndParametersEqual() const -{ +assertPropertiesAndParametersEqual() const { const ExponentialContact::Parameters& a = get_contact_parameters(); const SimTK::ExponentialSpringParameters& b = getParameters(); diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index c295e79dd9..20eaad0067 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -384,7 +384,134 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //------------------------------------------------------------------------- // Accessors for data cache entries //------------------------------------------------------------------------- - + /** Get the elastic part of the normal force. The system must be realized + to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForceElasticPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the damping part of the normal force. The system must be realized + to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForceDampingPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the normal force. The system must be realized to Stage::Dynamics + to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getNormalForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the instantaneous coefficient of friction (μ). The system must be + realized to Stage::Dynamics to access this data. μ is obtained by using + the Sliding state to transition between μₖ and μₛ: + + μ = μₛ - Sliding*(μₛ - μₖ) + + Because 0.0 ≤ Sliding ≤ 1.0, μₖ ≤ μ ≤ μₛ. + @param state State object on which to base the calculations. */ + SimTK::Real getMu(const SimTK::State& state) const; + + /** Get the friction limit. The system must be realized to Stage::Dynamics + to access this data. + @param state State object on which to base the calculations. */ + SimTK::Real getFrictionForceLimit(const SimTK::State& state) const; + + /** Get the elastic part of the friction force. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForceElasticPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the damping part of the friction force. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForceDampingPart( + const SimTK::State& state, bool inGround = true) const; + + /** Get the total friction force. The total frictional force is always + just the sum of the elastic part and the damping part of the frictional + force, which may be obtained separately by calling + getFrictionalForceElasticPart() and getFrictionalForceDampingPart(). + The system must be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getFrictionForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the total force applied to the body by this + ExponentialSpringForce instance. The total force is the vector sum of the + friction force, which may be obtained by a call to getFrictionForce(), and + the normal force, which may be obtained by a call to getNormalForce(). + The system must be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getForce( + const SimTK::State& state, bool inGround = true) const; + + /** Get the position of the body station (i.e., the point on the body at + which the force generated by this ExponentialSpringForce is applied). This + method differs from getStation() in terms of the frame in which the + station is expressed. getStation() expresses the point in the frame of + the MobilizedBody. getStationPosition() expresses the point either in the + Ground frame or in the frame of the Contact Plane. The system must be + realized to Stage::Position to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getStationPosition( + const SimTK::State& state, bool inGround = true) const; + + /** Get the velocity of the body station (i.e., the point on the body at + which the force generated by this ExponentialSpringForce is applied). + The system must be realized to Stage::Velocity to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getStationVelocity( + const SimTK::State& state, bool inGround = true) const; + + /** Get the position of the elastic anchor point (p₀), which will always + lie in the Contact Plane. p₀ is the spring zero of the damped linear + spring used in Friction Model 2. The system must be realized to + Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getAnchorPointPosition( + const SimTK::State& state, bool inGround = true) const; //------------------------------------------------------------------------- // Reporting @@ -436,10 +563,10 @@ More specifically, this class chiefly does 3 things: - Implements OpenSim Properties for most of the customizable contact parameters of class OpenSim::ExponentialContact, enabling those parameters to be serialized and de-serialized from file. -- Provides a member variable (_stkparams) for storing non-default parameters +- Provides a member variable (_stkparams) for storing user-set parameters prior to the creation of an underlying SimTK::ExponentialSpringForce object. During model initialization, when the SimTK::ExponetialSpringForce object is -constructed, the parameters are pushed to that object. +constructed, the user-set parameters are then pushed to that object. - Ensures that the values held by the OpenSim properties are kept consistent with the values held by a SimTK::ExponentialSpringParameters object. Depending on the circumstance, parameters are updated to match properties or @@ -447,8 +574,8 @@ properties are update to match parameters. To get or set values of the parameters managed by this class, you should use ExponentialContact::getParameters() and ExponentialContact::setParameters(). -Note that the values of the parameters managed by this class are always the -same as the values of their corresponding properties. +Note that the values of the parameters managed by this class should always be +the same as the values of their corresponding properties. @author F. C. Anderson **/ class ExponentialContact::Parameters : public Object { From e7a006b90753c430dd73a022728bac361e30c1d8 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 18 Sep 2022 17:02:28 -0500 Subject: [PATCH 35/55] Added static method for resetting anchor points. --- OpenSim/Simulation/Model/ExponentialContact.cpp | 17 +++++++++++++++++ OpenSim/Simulation/Model/ExponentialContact.h | 13 +++++++++++-- .../Simulation/Test/testExponentialContact.cpp | 5 ++--- OpenSim/Simulation/Test/testForces.cpp | 6 +++++- 4 files changed, 35 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index f073d646d4..6e5067143a 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -225,6 +225,23 @@ ExponentialContact:: resetAnchorPoint(SimTK::State& state) const { _spr->resetAnchorPoint(state); } +//_____________________________________________________________________________ +void +ExponentialContact:: +resetAnchorPoints(OpenSim::ForceSet& fSet, SimTK::State& state) { + int i; + int n = fSet.getSize(); + for (i = 0; i < n; ++i) { + try { + ExponentialContact& ec = + dynamic_cast(fSet.get(i)); + ec.resetAnchorPoint(state); + } catch (const std::bad_cast) { + // Nothing should happen here. Execution is just skipping any + // OpenSim::Force that is not an ExponentialContact. + } + } +} //----------------------------------------------------------------------------- // ACCESSORS for properties diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index 20eaad0067..b1d6d31adf 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -26,10 +26,10 @@ #include "SimTKsimbody.h" #include "Force.h" #include "OpenSim/Common/Set.h" +#include "OpenSim/Simulation/Model/ForceSet.h" namespace OpenSim { - //============================================================================= // ExponentialContact //============================================================================= @@ -301,7 +301,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { } //------------------------------------------------------------------------- - // Initialization + // Utility //------------------------------------------------------------------------- /** Reset the elastic anchor point (friction spring zero) so that it coincides with the projection of the body station onto the contact @@ -312,6 +312,15 @@ class OSIMSIMULATION_API ExponentialContact : public Force { @param state State object on which to base the reset. */ void resetAnchorPoint(SimTK::State& state) const; + /** Reset the elastic anchor points (friction spring zeros) of all + ExponentialContact instances in an OpenSim::ForceSet. This step is often + needed at the beginning of a simulation to ensure that a simulation does + not begin with large friction forces. Calling this method will invalidate + the System at Stage::Dynamics. + @param fSet Force set. + @param state State object on which to base the reset. */ + static void resetAnchorPoints(OpenSim::ForceSet& fSet, SimTK::State& state); + //------------------------------------------------------------------------- // Accessors for properties //------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 72c1257396..25a9e922c1 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -692,9 +692,8 @@ simulate() setInitialConditions(state, blockHC->getMobilizedBody(), -dz); // Reset the elastic anchor point for each ExponentialContact instance - for (int i = 0; i < n; ++i) { - if (sprEC[i] != NULL) sprEC[i]->resetAnchorPoint(state); - } + ForceSet& fSet = model->updForceSet(); + ExponentialContact::resetAnchorPoints(fSet, state); // Integrate Manager manager(*model); diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 35a6adc9ad..1e996da95f 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1239,7 +1239,7 @@ void testElasticFoundation() { manager.getStateStorage().print("bouncing_ball_states.sto"); // Save the forces - reporter->getForceStorage().print("elastic_contact_forces.mot"); + reporter->getForceStorage().print("elastic_contact_forces.sto"); // Bouncing ball should have settled to rest on ground due to dissipation // In that case the force generated by contact should be identically body @@ -1298,6 +1298,10 @@ void testExponentialContact() { pos[1] = 0.5; // starts from 0.5 above the ground body.setQToFitTranslation(state, pos); model.getMultibodySystem().realize(state, Stage::Position); + + // Reset the anchor points of any ExponentialContact instances. + ForceSet &fSet = model.updForceSet(); + ExponentialContact::resetAnchorPoints(fSet, state); // Simulate double final_t = 5.0; From e2a172bce645c1f0cc5ba211bdb359ecf2d1dde7 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 28 Oct 2022 16:32:57 -0500 Subject: [PATCH 36/55] ExponentialContact: Using monograph notation The variable name for the contact plane transform, which is one of the arguments needed in the constructor of ExponentialContact, is now X_GP. This monogram notation is both more compact and informative. The same change was made on the Simbody side. --- .../Simulation/Model/ExponentialContact.cpp | 27 +++++++++++++++++-- OpenSim/Simulation/Model/ExponentialContact.h | 19 +++++++------ 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 6e5067143a..4851cdb3ce 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -165,10 +165,10 @@ setNull() { void ExponentialContact:: constructProperties() { - SimTK::Transform contactXForm; + SimTK::Transform X_GP; SimTK::Vec3 origin(0.0); Parameters params; - constructProperty_contact_plane_transform(contactXForm); + constructProperty_contact_plane_transform(X_GP); constructProperty_body_name(""); constructProperty_body_station(origin); constructProperty_contact_parameters(params); @@ -214,6 +214,29 @@ extendAddToSystem(SimTK::MultibodySystem& system) const { const_cast(this); mutableThis->_spr = spr; mutableThis->_index = spr->getForceIndex(); + + // Should I connect states to the Component interface here by + // instantiating concrete OpenSim::StateVariable objects for which + // I have implemented the required virtual methods? + // + // And then also do something similar for the data cache entries? + // + // On the Simbody side, there are 4 states: + // 1. mus (Real, discrete state) + // 2. muk (Real, discrete state) + // 3. po (Vec3, auto update discrete state) + // 4. K (Real, auto update discrete state) + // + // Also on the Simbody side, are 24 data cache entries: + // 1. p_G (Vec3, Stage::Position) + // 2. p_P (Vec3, Stage::Position) + // 3. pz (Real, Stage::Position) + // 4. pxy (Vec3, Stage::Position) + // 5. v_G (Vec3, Stage::Velocity) + // ... + // 9. fzElas (Real, Stage::Dynamics) + // ... + // 24. f_G (Vec3, State::Dynamics) } //----------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index b1d6d31adf..a406f82066 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -284,13 +284,18 @@ class OSIMSIMULATION_API ExponentialContact : public Force { ExponentialContact(); /** Construct an ExponentialContact instance. - @param XContactPlane Transform specifying the location and orientation of - the contact plane in Ground. + @param X_GP Transform specifying the location and orientation of the + contact plane frame (P) with respect to the Ground frame (G). The positive + z-axis of P defines the normal direction; the x-axis and y-axis of P + together define the tangent (or friction) plane. Note that X_GP is the + operator that transforms a point of P (point_P) to that same point in + space but measured from the Ground origin (G₀) and expressed in G + (i.e., point_G = X_GP * point_P). @param bodyName Name of the body to which the force is applied. @param station Point on the body at which the force is applied. @param params Optional parameters object used to customize the topology-stage characteristics of the contact model. */ - explicit ExponentialContact(const SimTK::Transform& contactPlaneXform, + explicit ExponentialContact(const SimTK::Transform& X_GP, const std::string& bodyName, const SimTK::Vec3& station, SimTK::ExponentialSpringParameters params = SimTK::ExponentialSpringParameters()); @@ -326,7 +331,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //------------------------------------------------------------------------- /** Set the transform that specifies the location and orientation of the contact plane in the Ground frame. */ - void setContactPlaneTransform(const SimTK::Transform& contactPlaneXform); + void setContactPlaneTransform(const SimTK::Transform& X_GP); /** Get the transform that specifies the location and orientation of the contact plane in the Ground frame. */ const SimTK::Transform& getContactPlaneTransform() const; @@ -542,7 +547,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { /** Connect to the OpenSim Model. */ void extendConnectToModel(Model& model) override; - /** Create a SimTK::ExponentialSpringForce objects that implements + /** Create a SimTK::ExponentialSpringForce object that implements this Force. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; @@ -562,7 +567,7 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //============================================================================= // ExponentialContact::Parameters //============================================================================= -/** This subclass helps manage most of the topology-stage parameters of an +/** This subclass helps manage the topology-stage parameters of an OpenSim::ExponentialContact object. It does not provide the interface for getting and setting contact parameters (directly anyway) but rather provides the infrastructure for making the underlying SimTK::ExponentialSpringForce and @@ -583,8 +588,6 @@ properties are update to match parameters. To get or set values of the parameters managed by this class, you should use ExponentialContact::getParameters() and ExponentialContact::setParameters(). -Note that the values of the parameters managed by this class should always be -the same as the values of their corresponding properties. @author F. C. Anderson **/ class ExponentialContact::Parameters : public Object { From 36145fc0e670ba8bee5c7aa67d1e00dc5ca7288c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 31 Oct 2022 21:17:00 -0500 Subject: [PATCH 37/55] Typo Fix: corrected volume of block. --- OpenSim/Simulation/Test/testExponentialContact.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 25a9e922c1..7de1a0eb0a 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -281,7 +281,7 @@ printConditions() { appliedForce = "None"; cout << endl << endl; - cout << " model: " << modelDes << " (10 kg, 6 dof, 10x10x10 cm^3)" << endl; + cout << " model: " << modelDes << " (10 kg, 6 dof, 20x20x20 cm^3)" << endl; cout << " contact: " << contactDes << endl; cout << " motion: " << motion << endl; cout << " applied force: " << appliedForce << endl; From 783c16f500a715ac44c8f26b385c8764873f49d5 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Mon, 31 Oct 2022 21:17:53 -0500 Subject: [PATCH 38/55] testForce: enhanced test for ExponentialContact --- OpenSim/Simulation/Test/testForces.cpp | 137 +++++++++++++++++++------ 1 file changed, 105 insertions(+), 32 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 1e996da95f..d4c0cda4da 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1279,10 +1279,75 @@ void testElasticFoundation() { void testExponentialContact() { using namespace SimTK; - double start_h = 0.5; + // Construct a model and serialize it ----------------------------- + // This is done so that a model file does not need to be installed. + // Subsequent checks are done with the deserialized model. + string fileName = "BouncingBlock_ExponentialContact.osim"; + // Start temporary model scope + { + Model* model = new Model(); + model->setGravity(gravity_vec); + model->setName("BouncingBlock_ExponentialContact"); + // Body + OpenSim::Body* block = new OpenSim::Body(); + block->setName("block"); + block->set_mass(10.0); + block->set_mass_center(Vec3(0)); + block->setInertia(Inertia(1.0)); + // Joint + FreeJoint* freeJoint = new FreeJoint("free", model->updGround(), + Vec3(0), Vec3(0), *block, Vec3(0), Vec3(0)); + model->addBody(block); + model->addJoint(freeJoint); + + // ExponentialContact Instances---------------------------------- + // Customizable Parameters + // Default parameters work well, but may be altered. + // Default values are used here. + SimTK::ExponentialSpringParameters params; + // Contact plane transform. + // Need to make the normal of the contact plane, which is along + // along the z-axis, point along the y-axis of the Ground frame, + // which is up in OpenSim. + Real angle = convertDegreesToRadians(90.0); + Rotation floorRot(-angle, XAxis); + Vec3 floorOrigin(0., -0.004, 0.); // Shift to account for penetration. + Transform X_GP(floorRot, floorOrigin); + // Stations + const int nEC{8}; + double hs = 0.01; // half side = 10 cm. + Vec3 corner[nEC]; + corner[0] = Vec3(hs, -hs, hs); + corner[1] = Vec3(hs, -hs, -hs); + corner[2] = Vec3(-hs, -hs, -hs); + corner[3] = Vec3(-hs, -hs, hs); + corner[4] = Vec3(hs, hs, hs); + corner[5] = Vec3(hs, hs, -hs); + corner[6] = Vec3(-hs, hs, -hs); + corner[7] = Vec3(-hs, hs, hs); + // Place an exponential contact at each of the 8 stations + ExponentialContact* ec[nEC]{NULL}; + for (int i = 0; i < nEC; ++i) { + string nameEC = "Exp" + std::to_string(i); + ec[i] = new OpenSim::ExponentialContact( + X_GP, block->getName(), corner[i], params); + ec[i]->setName(nameEC); + model->addForce(ec[i]); + } + + // Build the Sytem + model->buildSystem(); + + // Serialize the model + model->print(fileName); - // Load the model - Model model{"BouncingBlock_ExponentialContact.osim"}; + // Delete the model + delete model; // TODO(fcanderson): Crashing right now. Why? + + } // End temporary model scope. + + // Deserialize the model + Model model{fileName}; // Create the force reporter ForceReporter* reporter = new ForceReporter(&model); @@ -1291,8 +1356,8 @@ void testExponentialContact() { // Initialize the system SimTK::State& state = model.initSystem(); - // Set the initial state - const OpenSim::Body& block = model.getBodySet().get("blockEC"); + // Set the starting position + const OpenSim::Body& block = model.getBodySet().get("block"); const SimTK::MobilizedBody& body = block.getMobilizedBody(); SimTK::Vec3 pos(0.0); pos[1] = 0.5; // starts from 0.5 above the ground @@ -1304,53 +1369,61 @@ void testExponentialContact() { ExponentialContact::resetAnchorPoints(fSet, state); // Simulate - double final_t = 5.0; Manager manager(model); manager.setIntegratorAccuracy(1e-6); + manager.setIntegratorMaximumStepSize(0.02); state.setTime(0.0); manager.initialize(state); clock_t startTime = clock(); - state = manager.integrate(final_t); + state = manager.integrate(10.0); clock_t stopTime = clock(); - cout << "Exponential Contact simulation time = " + cout << "Exponential Contact simulation execution time = " << 1.e3 * (stopTime - startTime) / CLOCKS_PER_SEC << "ms" << endl; // Print out the states and contact information - manager.getStateStorage().print("bouncing_block_EC_states.sto"); - reporter->getForceStorage().print("bouncing_block_EC_forces.sto"); + manager.getStateStorage().print("BouncingBlock_EC_states.sto"); + reporter->getForceStorage().print("BouncingBlock_EC_forces.sto"); -/* - // Bouncing block should have settled on ground due to dissipation. - // The force generated by contact then should be identically body - // weight vertically and zero horizontally. - OpenSim::ExponentialContact& spr = - (OpenSim::ExponentialContact&)osimModel.getForceSet().get("Exp0"); - - Array contact_force = spr.getRecordValues(state); - ASSERT_EQUAL(contact_force[0], 0.0, 1e-4); // no horizontal force - // vertical should be the weight - ASSERT_EQUAL(contact_force[1], -block.getMass()*gravity_vec[1], 1e-3); - ASSERT_EQUAL(contact_force[2], 0.0, 1e-4); // no horizontal force - ASSERT_EQUAL(contact_force[3], 0.0, 1e-4); // no torque on the ball - ASSERT_EQUAL(contact_force[4], 0.0, 1e-4); // no torque on the ball - ASSERT_EQUAL(contact_force[5], 0.0, 1e-4); // no torque on the ball + // Realize to Stage::Dynamics + // This is necessary to compute elastic anchor points and + // all of the contact forces. + model.getMultibodySystem().realize(state, Stage::Dynamics); - // Before exiting lets see if copying the force works + // The block should have settled on ground due to dissipation. + // The force generated by contact then should sum to body weight + // vertically and zero horizontally when the forces are totaled + // across all contact instances. + Vec3 gfrc = block.getMass() * gravity_vec; + Vec3 tfrc(0.0); + for (int i = 0; i < 8; ++i) { + string nameEC = "Exp" + to_string(i); + const OpenSim::ExponentialContact& spr = + (OpenSim::ExponentialContact&)model.getForceSet().get(nameEC); + Array spr_values = spr.getRecordValues(state); + tfrc[0] += spr_values[12]; // x + tfrc[1] += spr_values[13]; // y + tfrc[2] += spr_values[14]; // z + } + //cout << "gfrc = " << gfrc << endl; + //cout << "tfrc = " << tfrc << endl; + ASSERT_EQUAL(tfrc[0], -gfrc[0], 1.0e-4); // x + ASSERT_EQUAL(tfrc[1], -gfrc[1], 1.0e-3); // y + ASSERT_EQUAL(tfrc[2], -gfrc[2], 1.0e-4); // z + + // Before exiting lets see if cloning works + const OpenSim::ExponentialContact& spr = + (OpenSim::ExponentialContact&)model.getForceSet().get("Exp0"); OpenSim::ExponentialContact* sprCopy = spr.clone(); bool isEqual = (*sprCopy == spr); if (!isEqual) { - spr.print("originalForce.xml"); - sprCopy->print("copyOfForce.xml"); + spr.print("origElasticContact.xml"); + sprCopy->print("cloneElasticContact.xml"); } ASSERT(isEqual); - - */ } - - // Test our wrapping of Hunt-Crossley force in OpenSim // Simple simulation of bouncing ball with dissipation should generate contact // forces that settle to ball weight. From d2fbce235e16762488a3e9b6f6b6f4c98371f914 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 6 Jan 2023 12:19:07 -0600 Subject: [PATCH 39/55] Modified ExponentialContact::getRecordValues() Initially, I included a variety of quantities in the record values related to the force exerted by an ExponentialContact instance, including quantities like the position of the elastic anchor point, total force, normal force, and friction force. The quantities were expressed in the Ground frame, not the local Body frame. In addition, I did not include the loads applied to Ground. In the interest of compatibility between ExponentialContact and other Force components in OpenSim, I removed some quantities returned by getRecordValues() and also added the loads applied to Ground. Also, all quantities are now BodyForces (i.e., SpatialVec's returned by a call to calcForceContribution()). The information returned by a call to getRecordValues() now has the following format: EC.Body.force EC.Body.torque EC.ground.force EC.ground.torque There were other minor changes too, like copyright dates and getting rid of trailing whitespace. --- .../Simulation/Model/ExponentialContact.cpp | 80 ++++++++++++------- .../Test/testExponentialContact.cpp | 6 +- 2 files changed, 53 insertions(+), 33 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 4851cdb3ce..717b79e4f0 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -7,7 +7,7 @@ * 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 * + * Copyright (c) 2022-20232 Stanford University and the Authors * * Author(s): F. C. Anderson * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -22,6 +22,7 @@ * -------------------------------------------------------------------------- */ #include "Model.h" +#include "simbody/internal/SimbodyMatterSubsystem.h" #include "ExponentialContact.h" using namespace OpenSim; @@ -102,7 +103,7 @@ updateParameters() { // member variable (_stkparam) is kept consistent with the properties. // It is necessary to have the _stkparams member variable because there needs // to be a place to store non-default parameters when the underlying -// SimTK::ExponentialSpringForce hasn't been instantiated. +// SimTK::ExponentialSpringForce hasn't yet been instantiated. // Having to do a little extra bookkeeping (i.e., storing properties values // twice [once in the Properties and once in _stkparams]) is justified // by not having to rewrite a whole bunch of additional accessor methods. @@ -124,7 +125,7 @@ setSimTKParameters(const SimTK::ExponentialSpringParameters& params) { updateProperties(); } //_____________________________________________________________________________ -// Get a read-only reference to the SimTK::ExponentialSpringParamters held +// Get a read-only reference to the SimTK::ExponentialSpringParameters held // by this instance. const SimTK::ExponentialSpringParameters& ExponentialContact::Parameters:: @@ -218,15 +219,15 @@ extendAddToSystem(SimTK::MultibodySystem& system) const { // Should I connect states to the Component interface here by // instantiating concrete OpenSim::StateVariable objects for which // I have implemented the required virtual methods? - // + // // And then also do something similar for the data cache entries? - // + // // On the Simbody side, there are 4 states: // 1. mus (Real, discrete state) // 2. muk (Real, discrete state) // 3. po (Vec3, auto update discrete state) // 4. K (Real, auto update discrete state) - // + // // Also on the Simbody side, are 24 data cache entries: // 1. p_G (Vec3, Stage::Position) // 2. p_P (Vec3, Stage::Position) @@ -416,22 +417,25 @@ OpenSim::Array ExponentialContact:: getRecordLabels() const { OpenSim::Array labels(""); + string name = getName(); // Name of this contact instance. std::string frameName = getBodyName(); - labels.append(getName()+"."+frameName+".p0.X"); - labels.append(getName()+"."+frameName+".p0.Y"); - labels.append(getName()+"."+frameName+".p0.Z"); - labels.append(getName()+"."+frameName+".station.X"); - labels.append(getName()+"."+frameName+".station.Y"); - labels.append(getName()+"."+frameName+".station.Z"); - labels.append(getName()+"."+frameName+".forceNormal.X"); - labels.append(getName()+"."+frameName+".forceNormal.Y"); - labels.append(getName()+"."+frameName+".forceNormal.Z"); - labels.append(getName()+"."+frameName+".forceFriction.X"); - labels.append(getName()+"."+frameName+".forceFriction.Y"); - labels.append(getName()+"."+frameName+".forceFriction.Z"); - labels.append(getName()+"."+frameName+".force.X"); - labels.append(getName()+"."+frameName+".force.Y"); - labels.append(getName()+"."+frameName+".force.Z"); + std::string groundName = getModel().getGround().getName(); + + // Record format consistent with HuntCrossleyForce. + // Body + labels.append(name + "." + frameName + ".force.X"); + labels.append(name + "." + frameName + ".force.Y"); + labels.append(name + "." + frameName + ".force.Z"); + labels.append(name + "." + frameName + ".torque.X"); + labels.append(name + "." + frameName + ".torque.Y"); + labels.append(name + "." + frameName + ".torque.Z"); + // Ground + labels.append(name + "." + groundName + ".force.X"); + labels.append(name + "." + groundName + ".force.Y"); + labels.append(name + "." + groundName + ".force.Z"); + labels.append(name + "." + groundName + ".torque.X"); + labels.append(name + "." + groundName + ".torque.Y"); + labels.append(name + "." + groundName + ".torque.Z"); return labels; } @@ -445,17 +449,33 @@ getRecordValues(const SimTK::State& state) const { const SimTK::Force& abstractForce = forceSubsys.getForce(_index); const auto& spr = (SimTK::ExponentialSpringForce&)(abstractForce); - SimTK::Vec3 p0 = spr.getAnchorPointPosition(state); - SimTK::Vec3 station = spr.getStationPosition(state); - SimTK::Vec3 normal = spr.getNormalForce(state); - SimTK::Vec3 friction = spr.getFrictionForce(state); - SimTK::Vec3 force = spr.getForce(state); + // Get the loads + SimTK::Vector_ bForces(0); // body + SimTK::Vector_ pForces(0); // particle + SimTK::Vector mForces(0); // mobility + spr.calcForceContribution(state, bForces, pForces, mForces); + + // Body + SimTK::Vec3 force; + SimTK::Vec3 torque; + const auto& bodyIndex = _body->getMobilizedBodyIndex(); + SimTK::SpatialVec& bodyForce = bForces(bodyIndex); + force = bodyForce[1]; + double fy = force[1]; + torque = bodyForce[0]; + values.append(3, &force[0]); + values.append(3, &torque[0]); - values.append(3, &p0[0]); - values.append(3, &station[0]); - values.append(3, &normal[0]); - values.append(3, &friction[0]); + // Ground + const SimTK::MultibodySystem& system = _model->getSystem(); + const SimTK::SimbodyMatterSubsystem& matter = system.getMatterSubsystem(); + const SimTK::MobilizedBody& ground = matter.getGround(); + const auto& groundIndex = ground.getMobilizedBodyIndex(); + SimTK::SpatialVec& groundForce = bForces(groundIndex); + force = groundForce[1]; + torque = groundForce[0]; values.append(3, &force[0]); + values.append(3, &torque[0]); return values; } diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 7de1a0eb0a..b5aa1c3f68 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -7,7 +7,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2022 Stanford University and the Authors * + * Copyright (c) 2022-2023 Stanford University and the Authors * * Author(s): F. C. Anderson * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -84,7 +84,7 @@ class ExponentialContactTester corner[4] = Vec3( hs, hs, hs); corner[5] = Vec3( hs, hs, -hs); corner[6] = Vec3(-hs, hs, -hs); - corner[7] = Vec3(-hs, hs, hs); + corner[7] = Vec3(-hs, hs, hs); }; // Destructor @@ -460,7 +460,7 @@ addHuntCrossleyContact(OpenSim::Body* block) std::string name = ""; for (int i = 0; i < n; ++i) { // Geometry - name = "sphere_" + std::to_string(i); + name = "sphere_" + std::to_string(i); geomHC[i] = new ContactSphere(0.005, corner[i], *block, name); model->addContactGeometry(geomHC[i]); From 7f8bdf1ea90f651e37665679fa7a54f0a5f40eb5 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Fri, 6 Jan 2023 12:25:55 -0600 Subject: [PATCH 40/55] Modified testExponentialContact() in testForces.cpp I trimmed some unnecessary code, made a few things more general, and adjusted the asserts to account for the changes made to ExponentialContact::getRecordValues(). --- OpenSim/Simulation/Test/testForces.cpp | 27 ++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index d4c0cda4da..806372d27d 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1230,7 +1230,6 @@ void testElasticFoundation() { // end timing cout << "Elastic Foundation simulation time = " << 1.e3 * (clock() - startTime) / CLOCKS_PER_SEC << "ms" << endl; - ; // make sure we can access dynamic variables osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration); @@ -1280,9 +1279,11 @@ void testExponentialContact() { using namespace SimTK; // Construct a model and serialize it ----------------------------- - // This is done so that a model file does not need to be installed. + // This is done so that a model file does not have to be installed. // Subsequent checks are done with the deserialized model. string fileName = "BouncingBlock_ExponentialContact.osim"; + string baseName = "EC"; + const int nEC{8}; // Start temporary model scope { Model* model = new Model(); @@ -1314,7 +1315,6 @@ void testExponentialContact() { Vec3 floorOrigin(0., -0.004, 0.); // Shift to account for penetration. Transform X_GP(floorRot, floorOrigin); // Stations - const int nEC{8}; double hs = 0.01; // half side = 10 cm. Vec3 corner[nEC]; corner[0] = Vec3(hs, -hs, hs); @@ -1328,7 +1328,7 @@ void testExponentialContact() { // Place an exponential contact at each of the 8 stations ExponentialContact* ec[nEC]{NULL}; for (int i = 0; i < nEC; ++i) { - string nameEC = "Exp" + std::to_string(i); + string nameEC = baseName + std::to_string(i); ec[i] = new OpenSim::ExponentialContact( X_GP, block->getName(), corner[i], params); ec[i]->setName(nameEC); @@ -1342,7 +1342,7 @@ void testExponentialContact() { model->print(fileName); // Delete the model - delete model; // TODO(fcanderson): Crashing right now. Why? + delete model; } // End temporary model scope. @@ -1364,10 +1364,10 @@ void testExponentialContact() { body.setQToFitTranslation(state, pos); model.getMultibodySystem().realize(state, Stage::Position); - // Reset the anchor points of any ExponentialContact instances. + // Reset the anchor points of all ExponentialContact instances. ForceSet &fSet = model.updForceSet(); ExponentialContact::resetAnchorPoints(fSet, state); - + // Simulate Manager manager(model); manager.setIntegratorAccuracy(1e-6); @@ -1395,14 +1395,17 @@ void testExponentialContact() { // across all contact instances. Vec3 gfrc = block.getMass() * gravity_vec; Vec3 tfrc(0.0); - for (int i = 0; i < 8; ++i) { - string nameEC = "Exp" + to_string(i); + for (int i = 0; i < nEC; ++i) { + string nameEC = baseName + to_string(i); const OpenSim::ExponentialContact& spr = (OpenSim::ExponentialContact&)model.getForceSet().get(nameEC); + OpenSim::Array labels = spr.getRecordLabels(); + cout << endl << labels << endl; Array spr_values = spr.getRecordValues(state); - tfrc[0] += spr_values[12]; // x - tfrc[1] += spr_values[13]; // y - tfrc[2] += spr_values[14]; // z + cout << endl << spr_values << endl; + tfrc[0] += spr_values[0]; // x + tfrc[1] += spr_values[1]; // y + tfrc[2] += spr_values[2]; // z } //cout << "gfrc = " << gfrc << endl; //cout << "tfrc = " << tfrc << endl; From 540da3f16f73f5f9d5af543df99ca8739b6d700b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 17 Jan 2023 17:15:05 -0600 Subject: [PATCH 41/55] Dependencies updated to build against latest Simbody build. OpenSim::ExponentialContact relies on Simbody d685ed2 (PR #746) or later. There were several performance improvements to SimTK::ExponentialSpringForce, as well as some intuitive name changes to the API. This build change should clear up several errors in the continuous integration build. --- dependencies/CMakeLists.txt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 312e55e2b6..9a25db1535 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -17,14 +17,14 @@ function(SetDefaultCMakeInstallPrefix) get_filename_component(BASE_DIR ${BASE_DIR} DIRECTORY) # Default install prefix for OpenSim dependencies. If user changes # CMAKE_INSTALL_PREFIX, this directory will be removed. - set(DEFAULT_CMAKE_INSTALL_PREFIX + set(DEFAULT_CMAKE_INSTALL_PREFIX ${BASE_DIR}/opensim_dependencies_install CACHE INTERNAL "Default CMAKE_INSTALL_PREFIX for OpenSim dependencies.") if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) - set(CMAKE_INSTALL_PREFIX + set(CMAKE_INSTALL_PREFIX ${DEFAULT_CMAKE_INSTALL_PREFIX} CACHE PATH @@ -33,7 +33,7 @@ function(SetDefaultCMakeInstallPrefix) endif() endfunction() -# CMake doesn't clear prefix directories when user changes it. +# CMake doesn't clear prefix directories when user changes it. # Remove it to avoid confusion. function(RemoveDefaultInstallDirIfEmpty DIR) file(GLOB CONTENTS ${DIR}/*) @@ -66,7 +66,7 @@ endfunction() # GIT_URL -- (Required) git repository to download the sources from. # GIT_TAG -- (Required) git tag to checkout before commencing build. # DEPENDS -- (Optional) Other projects this project depends on. -# CMAKE_ARGS -- (Optional) A CMake list of arguments to be passed to CMake +# CMAKE_ARGS -- (Optional) A CMake list of arguments to be passed to CMake # while building the project. # You must provide either URL or GIT_URL and GIT_TAG, but not all 3. function(AddDependency) @@ -176,7 +176,7 @@ AddDependency(NAME ezc3d AddDependency(NAME simbody DEFAULT ON GIT_URL https://github.com/simbody/simbody.git - GIT_TAG e855d954a786128c3271a3406d7bda782e7c4c4f + GIT_TAG 462b2a6dbb8794db2922d72f52b29b488a178ebc CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF -DBUILD_TESTING:BOOL=OFF) @@ -251,7 +251,7 @@ if (WIN32) if(SUPERBUILD_ipopt) - + # Ipopt: Download pre-built binaries built by chrisdembia. # This binary distribution comes with MUMPS and OpenBLAS. # Compiled with clang-cl 5.0 and flang, using conda. From e7b17b191f8a6acc5c7d9686527f64bb78044e01 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 17 Jan 2023 18:32:12 -0600 Subject: [PATCH 42/55] Commented out some cout statements. These cout statements should be removed entirely once the output format has stabilized. --- OpenSim/Simulation/Test/testForces.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 806372d27d..ea3243e05c 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1400,9 +1400,9 @@ void testExponentialContact() { const OpenSim::ExponentialContact& spr = (OpenSim::ExponentialContact&)model.getForceSet().get(nameEC); OpenSim::Array labels = spr.getRecordLabels(); - cout << endl << labels << endl; + //cout << endl << labels << endl; Array spr_values = spr.getRecordValues(state); - cout << endl << spr_values << endl; + //cout << endl << spr_values << endl; tfrc[0] += spr_values[0]; // x tfrc[1] += spr_values[1]; // y tfrc[2] += spr_values[2]; // z @@ -1414,8 +1414,9 @@ void testExponentialContact() { ASSERT_EQUAL(tfrc[2], -gfrc[2], 1.0e-4); // z // Before exiting lets see if cloning works + string ecName = baseName + "0"; const OpenSim::ExponentialContact& spr = - (OpenSim::ExponentialContact&)model.getForceSet().get("Exp0"); + (OpenSim::ExponentialContact&)model.getForceSet().get(ecName); OpenSim::ExponentialContact* sprCopy = spr.clone(); bool isEqual = (*sprCopy == spr); if (!isEqual) { From eb6752fb7c7d8a435817c1794186d498fc21464b Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 17 Jan 2023 19:01:01 -0600 Subject: [PATCH 43/55] updateFromXMLNode() now declared as override ExponentialContact::Parameters::updateFromXMLNode() without the override declaration was causing and error on the Mac build. --- OpenSim/Simulation/Model/ExponentialContact.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index a406f82066..bd0e53ba61 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -110,7 +110,7 @@ which has the form of the Hunt & Crossley damping model: The friction force is computed by blending two different friction models. The blending is performed based on the 'Sliding' State of the -ExponentialSpringForce class. +ExponentialSpringForce class. #### Friction Model 1 - Pure Damping (Sliding = 1.0) When the body station is sliding with respect to the contact plane, the @@ -641,7 +641,8 @@ class ExponentialContact::Parameters : public Object { void constructProperties(); void updateParameters(); void updateProperties(); - void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber); + void updateFromXMLNode(SimTK::Xml::Element& node, + int versionNumber) override; SimTK::ExponentialSpringParameters _stkparams; }; From 75e87c91952b311234517a51217985dd1f5affc6 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 18 Jan 2023 02:41:43 -0600 Subject: [PATCH 44/55] Ubuntu build fix?: Catching exception std::bad_cast The Ubuntu2004 build did not like catching std::bad_cast by value, so I am trying catching std::bad_cast by reference. I suspect that Ubuntu2004 may not fully comply with C++11. --- .../Simulation/Model/ExponentialContact.cpp | 2 +- OpenSim/Simulation/Model/HuntCrossleyForce.h | 31 ++++++++----------- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 717b79e4f0..61699fef1a 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -260,7 +260,7 @@ resetAnchorPoints(OpenSim::ForceSet& fSet, SimTK::State& state) { ExponentialContact& ec = dynamic_cast(fSet.get(i)); ec.resetAnchorPoint(state); - } catch (const std::bad_cast) { + } catch (const std::bad_cast&) { // Nothing should happen here. Execution is just skipping any // OpenSim::Force that is not an ExponentialContact. } diff --git a/OpenSim/Simulation/Model/HuntCrossleyForce.h b/OpenSim/Simulation/Model/HuntCrossleyForce.h index 28142908de..a31a86c512 100644 --- a/OpenSim/Simulation/Model/HuntCrossleyForce.h +++ b/OpenSim/Simulation/Model/HuntCrossleyForce.h @@ -32,7 +32,7 @@ namespace OpenSim { // HUNT CROSSLEY FORCE //============================================================================== /** This force subclass implements a Hunt-Crossley contact model. It uses Hertz -contact theory to model the interactions between a set of ContactSpheres and +contact theory to model the interactions between a set of ContactSpheres and ContactHalfSpaces. @author Peter Eastman **/ @@ -45,7 +45,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); //============================================================================== // PROPERTIES //============================================================================== - OpenSim_DECLARE_PROPERTY(contact_parameters, + OpenSim_DECLARE_PROPERTY(contact_parameters, HuntCrossleyForce::ContactParametersSet, "Material properties."); OpenSim_DECLARE_PROPERTY(transition_velocity, double, @@ -72,7 +72,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); * %Set the transition velocity for switching between static and dynamic friction. */ void setTransitionVelocity(double velocity); - + /** * Access to ContactParameters. Methods assume size 1 of ContactParametersSet and add one ContactParameter if needed */ @@ -92,7 +92,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce, Force); //----------------------------------------------------------------------------- // Reporting //----------------------------------------------------------------------------- - /** + /** * Provide name(s) of the quantities (column labels) of the force value(s) to be reported */ OpenSim::Array getRecordLabels() const override ; @@ -129,23 +129,18 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParameters, Object); //============================================================================== OpenSim_DECLARE_LIST_PROPERTY(geometry, std::string, "Names of geometry objects affected by these parameters."); - OpenSim_DECLARE_PROPERTY(stiffness, double, - ""); - OpenSim_DECLARE_PROPERTY(dissipation, double, - ""); - OpenSim_DECLARE_PROPERTY(static_friction, double, - ""); - OpenSim_DECLARE_PROPERTY(dynamic_friction, double, - ""); - OpenSim_DECLARE_PROPERTY(viscous_friction, double, - ""); + OpenSim_DECLARE_PROPERTY(stiffness, double, ""); + OpenSim_DECLARE_PROPERTY(dissipation, double, ""); + OpenSim_DECLARE_PROPERTY(static_friction, double, ""); + OpenSim_DECLARE_PROPERTY(dynamic_friction, double, ""); + OpenSim_DECLARE_PROPERTY(viscous_friction, double, ""); //============================================================================== // PUBLIC METHODS //============================================================================== ContactParameters(); - ContactParameters(double stiffness, double dissipation, - double staticFriction, double dynamicFriction, + ContactParameters(double stiffness, double dissipation, + double staticFriction, double dynamicFriction, double viscousFriction); const Property& getGeometry() const; @@ -170,9 +165,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParameters, Object); //============================================================================== // HUNT CROSSLEY FORCE :: CONTACT PARAMETERS SET //============================================================================== -class OSIMSIMULATION_API HuntCrossleyForce::ContactParametersSet +class OSIMSIMULATION_API HuntCrossleyForce::ContactParametersSet : public Set { -OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParametersSet, +OpenSim_DECLARE_CONCRETE_OBJECT(HuntCrossleyForce::ContactParametersSet, Set); public: From d1a605cb03f10081a33e941b8eb4c9907dc6853f Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 28 Jan 2023 12:21:26 -0600 Subject: [PATCH 45/55] Minor changes to comments. --- OpenSim/Simulation/Test/testForces.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index ea3243e05c..7c9e93a300 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1273,14 +1273,14 @@ void testElasticFoundation() { // Test the wrapping of ExponentialSpringForce in OpenSim -// Simple simulation of bouncing ball with dissipation should generate contact +// Simple simulation of bouncing block with dissipation should generate contact // forces that settle to block weight (mg = 10.0 * g). void testExponentialContact() { using namespace SimTK; // Construct a model and serialize it ----------------------------- // This is done so that a model file does not have to be installed. - // Subsequent checks are done with the deserialized model. + // Below the temporary scope, checks are done with the deserialized model. string fileName = "BouncingBlock_ExponentialContact.osim"; string baseName = "EC"; const int nEC{8}; From 855b73ad01bf7eaa2761034dd3dd49385843a322 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 28 Jan 2023 12:22:14 -0600 Subject: [PATCH 46/55] Added tests for Discrete Variable accessors. --- .../Test/testExponentialContact.cpp | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index b5aa1c3f68..af1510cd0f 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -26,6 +26,8 @@ #include #include +#include + #include #include #include @@ -120,6 +122,9 @@ class ExponentialContactTester void test(); void testParameters(); void testSerialization(); + void testDiscreteVariable(State& state, const ExponentialContact& ec, + const string& name, double valOrig, + double delta = 0.1, double tol = 1.0e-6); // Simulation void setInitialConditions(SimTK::State& state, @@ -672,6 +677,27 @@ testSerialization() { SimTK_ASSERT_ALWAYS(pCopy == p, "Deserialized parameters are not equal to original parameters."); } +//_____________________________________________________________________________ +void +ExponentialContactTester:: +testDiscreteVariable(State& state, const ExponentialContact& ec, + const string& name, double val, double delta, double tol) +{ + // Get the starting value, which should be the same as val. + double valDV = ec.getDiscreteVariableValue(state, name); + ASSERT_EQUAL(val, valDV, tol); + + // Set a new value. + double valNew = val + delta; + ec.setDiscreteVariableValue(state, name, valNew); + double valNewDV = ec.getDiscreteVariableValue(state, name); + ASSERT_EQUAL(valNew, valNewDV, tol); + + // Restore the starting value. + ec.setDiscreteVariableValue(state, name, val); + valDV = ec.getDiscreteVariableValue(state, name); + ASSERT_EQUAL(val, valDV, tol); +} //_____________________________________________________________________________ void @@ -695,6 +721,44 @@ simulate() ForceSet& fSet = model->updForceSet(); ExponentialContact::resetAnchorPoints(fSet, state); + // Check the Component API for discrete states. + int i; + int n = fSet.getSize(); + for (i = 0; i < n; ++i) { + try { + string name; + double delta = 0.1; + double tol = 1.0e-6; + double val{0.0}, valAfter{0.0}; + + // Get the ExponentialContact Component + ExponentialContact& ec = + dynamic_cast(fSet.get(i)); + string path = ec.getAbsolutePathString(); + cout << "path = " << path << endl; + + // Static Coefficient of Friction + name = ec.getMuStaticDiscreteStateName(); + val = ec.getMuStatic(state); + testDiscreteVariable(state, ec, name, val, delta, tol); + valAfter = ec.getMuStatic(state); + ASSERT_EQUAL(val, valAfter, tol); + + // Kinetic Coefficient of Friction + name = ec.getMuKineticDiscreteStateName(); + val = ec.getMuKinetic(state); + testDiscreteVariable(state, ec, name, val, delta, tol); + valAfter = ec.getMuKinetic(state); + ASSERT_EQUAL(val, valAfter, tol); + + } catch (const std::exception& e) { + // Nothing should happen here. Execution is just skipping any + // OpenSim::Force that is not an ExponentialContact. + cout << e.what() << endl; + } + + } + // Integrate Manager manager(*model); manager.getIntegrator().setMaximumStepSize(dt_max); From cf84b902caca25e1abdc99772124109142a58bea Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 28 Jan 2023 12:42:18 -0600 Subject: [PATCH 47/55] ExponentialContact:: added methods related to Discrete States. I added several methods to support exposing the discrete states of class ExponentialContact to the OpenSim::Component API. I added the following methods to establish the names that class Component will use for the Discrete Variable names. These methods allow one to avoid placing quoted strings a bunch of places in the code. They are mainly intended for construction / initialization related methods. getMuStaticDiscreteStateName() getMuKineticDiscreteStateName() getSlidingDiscreteStateName(). I added ExponentialContact::extendRealizeTopology(). This method initializes the index and Subsystem for each Discrete Variable. This initialization needs to occur in ExponentialContact because the Discrete Variables are no longer allocated in Component::extendRealizeTopology() (see comment below). Note that the Subsystem needs to be initialized because the Discrete Variables that belong to ExponentialContact are allocated from a different SimTK::Subsystem than is normally used by class Component. Finally, in ExponentialContact::extendAddToSystem(), note that the calls to addDiscreteVariable() include the 'allocate' flag with a value of 'false'. This prevents the double allocation of the discrete variables in Component::extendRealizeTopology(). --- .../Simulation/Model/ExponentialContact.cpp | 79 ++++++++++++++----- OpenSim/Simulation/Model/ExponentialContact.h | 25 +++++- 2 files changed, 83 insertions(+), 21 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 61699fef1a..5f2e80c31d 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -216,30 +216,71 @@ extendAddToSystem(SimTK::MultibodySystem& system) const { mutableThis->_spr = spr; mutableThis->_index = spr->getForceIndex(); - // Should I connect states to the Component interface here by - // instantiating concrete OpenSim::StateVariable objects for which - // I have implemented the required virtual methods? + // Expose the discrete states of ExponentialSpringForce in OpenSim + bool allocate = false; + std::string name = getMuStaticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getMuKineticDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + name = getSlidingDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); + //addDiscreteVariable("anchor", SimTK::Stage::Dynamics, allocate); +} + + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method is needed because class OpenSim::ExponentialContact has 4 +// discrete states that are allocated in +// SimTK::ExponentialSpringForce::realizeTopology(), not in +// OpenSim::Component::extendRealizeTopology(). +// These states are added to the OpenSim map of discrete states +// (i.e., Component::_namedDiscreteVariableInfo) so that they are accessible +// in OpenSim. See ExponentialContact::extendAddToSystem(). However, because +// these discrete states are not allocated by OpenSim::Component, OpenSim has +// no knowledge of their indices into the SimTK::State. The purpose of this +// method is to properly initialize those indices. +void +ExponentialContact:: +extendRealizeTopology(SimTK::State& state) const { + + Super::extendRealizeTopology(state); + + // A critical question... + // Can we guarrantee that ExponentialSpringForce::realizeTopology() will + // always be called before this method !?!? // - // And then also do something similar for the data cache entries? + // Maybe. Perhaps the ExponentialSpringForce object will always + // be listed in the SimTK::System before the ExponentialContact component? + // If not, then it is possible that getMuStaticStateIndex() will + // return a bad index. // - // On the Simbody side, there are 4 states: - // 1. mus (Real, discrete state) - // 2. muk (Real, discrete state) - // 3. po (Vec3, auto update discrete state) - // 4. K (Real, auto update discrete state) + // What I can confirm is that, so far, the indices in multiple tests have + // been assigned correctly. // - // Also on the Simbody side, are 24 data cache entries: - // 1. p_G (Vec3, Stage::Position) - // 2. p_P (Vec3, Stage::Position) - // 3. pz (Real, Stage::Position) - // 4. pxy (Vec3, Stage::Position) - // 5. v_G (Vec3, Stage::Velocity) - // ... - // 9. fzElas (Real, Stage::Dynamics) - // ... - // 24. f_G (Vec3, State::Dynamics) + // If there are mysterious failures because of a bad State index, the + // source of the issue may be that ExponentialSpringForce::realizeTopology + // was not called prior to ExponentialContact::extendRealizeTopology. + + const SimTK::Subsystem* subsys = &_spr->getForceSubsystem(); + SimTK::DiscreteVariableIndex index; + std::string name; + + name = getMuStaticDiscreteStateName(); + index = _spr->getMuStaticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getMuKineticDiscreteStateName(); + index = _spr->getMuKineticStateIndex(); + updDiscreteVariableIndex(name, index, subsys); + + name = getSlidingDiscreteStateName(); + index = _spr->getSlidingStateIndex(); + updDiscreteVariableIndex(name, index, subsys); } + + //----------------------------------------------------------------------------- // Utility //----------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index bd0e53ba61..d9a610e817 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -359,10 +359,16 @@ class OSIMSIMULATION_API ExponentialContact : public Force { const SimTK::ExponentialSpringParameters& getParameters() const; //------------------------------------------------------------------------- - // Accessors for states + // Accessors for Discrete States //------------------------------------------------------------------------- + /** Get the name used for the discrete state representing the static + coefficient of friction (μₛ). This is the name used to access informattion + related to μₛ via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getMuStaticDiscreteStateName() const { return "mu_static"; } + /** Set the static coefficient of friction (μₛ) for this exponential - spring. The value of μₛ is held in the System's State object. Unlike the + spring. μₛ is a discrete state. The value of μₛ is held in the System's State object. Unlike the parameters managed by ExponentialSpringParameters, μₛ can be set at any time during a simulation. A change to μₛ will invalidate the System at Stage::Dynamics, but not at Stage::Topology. @@ -376,6 +382,12 @@ class OSIMSIMULATION_API ExponentialContact : public Force { @param state State object from which to retrieve μₛ. */ SimTK::Real getMuStatic(const SimTK::State& state) const; + /** Get the name used for the discrete state representing the kinetic + coefficient of friction (μₖ). This is the name used to access informattion + related to μₖ via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getMuKineticDiscreteStateName() const { return "mu_kinetic"; } + /** Set the kinetic coefficient of friction (μₖ) for this exponential spring. The value of μₖ is held in the System's State object. Unlike the parameters managed by ExponentialSpringParameters, μₖ can be set at any @@ -391,6 +403,12 @@ class OSIMSIMULATION_API ExponentialContact : public Force { @param state State object from which to retrieve μₖ. */ SimTK::Real getMuKinetic(const SimTK::State& state) const; + /** Get the name used for the discrete state representing the 'sliding' + state (K) of the elastic anchor point. This is the name used to access + informattion related to K via the OpenSim::Component API. For example, see + Component::getDiscreteVariableValue(). */ + std::string getSlidingDiscreteStateName() const { return "sliding"; } + /** Get the Sliding state of the spring. @param state State object from which to retrieve Sliding. */ SimTK::Real getSliding(const SimTK::State& state) const; @@ -551,6 +569,9 @@ class OSIMSIMULATION_API ExponentialContact : public Force { this Force. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; + /** Initialize discrete variable indices. */ + virtual void extendRealizeTopology(SimTK::State& state) const override; + /** Update this Object base on an XML node. */ void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) override; From e114e54e1a605c2d8e1d5faa92db5695767c5703 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 28 Jan 2023 13:07:02 -0600 Subject: [PATCH 48/55] Component: modifications to accommodate externally allocated Discrete Variables. In the code, search on "F. C. Anderson" to locate the substantive changes. These changes are accompanied by detailed comments. 1. Added data members ('subsystem' and 'allocate') to struct DiscreteVariableInfo. 2. Added the variable 'bool allocate' to the argument list of addDiscreteVariable(), which allows prevention of double allocation of a Discrete Variable in Component::extendRealizeTopology(). 3. Added a new method-- updDiscreteVariableIndex(). This method allows a derived (concrete) Component class to initialize the indices and specify the Subsystem for Discrete Variables that the derived Component owns. 4. In getDiscreteVariableValue() and setDiscreteVariableValue(), it is no longer assumed that the Subsystem to be used is the SimTK::DefaultSystemSubsystem. Instead, struct DiscreteVariableInfo is consulted. 5. Finally, in extendRealizeTopology() the 'allocate' data member of the DiscreteVariableInfo struct is consulted. If 'allocate' is 'true', the Discrete Variable is allocated normally. If 'false', allocation is left to the derived (concrete) class. --- OpenSim/Common/Component.cpp | 279 ++++++++++++++++++++++------------- OpenSim/Common/Component.h | 51 ++++++- 2 files changed, 225 insertions(+), 105 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index e66b75c75c..241acbc8ea 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -47,9 +47,9 @@ class ComponentMeasure : public SimTK::Measure_ { public: SimTK_MEASURE_HANDLE_PREAMBLE(ComponentMeasure, SimTK::Measure_); - ComponentMeasure(SimTK::Subsystem& sub, + ComponentMeasure(SimTK::Subsystem& sub, const OpenSim::Component& mc) - : SimTK::Measure_(sub, new Implementation(mc), + : SimTK::Measure_(sub, new Implementation(mc), SimTK::AbstractMeasure::SetHandle()) {} SimTK_MEASURE_HANDLE_POSTSCRIPT(ComponentMeasure, SimTK::Measure_); @@ -57,7 +57,7 @@ class ComponentMeasure : public SimTK::Measure_ { template -class ComponentMeasure::Implementation +class ComponentMeasure::Implementation : public SimTK::Measure_::Implementation { public: // Don't allocate a value cache entry since this measure's value is @@ -73,7 +73,7 @@ class ComponentMeasure::Implementation int getNumTimeDerivativesVirtual() const override final {return 0;} SimTK::Stage getDependsOnStageVirtual(int order) const override final { return SimTK::Stage::Empty; } - + const T& getUncachedValueVirtual (const SimTK::State& s, int derivOrder) const override final { return this->getValueZero(); } @@ -186,7 +186,7 @@ void Component::finalizeFromProperties() } // TODO use a flag to set whether we are lenient on having nameless - // Components. For backward compatibility we need to be able to + // Components. For backward compatibility we need to be able to // handle nameless components so assign them their class name // - aseth if (getName().empty()) { @@ -206,7 +206,7 @@ void Component::finalizeFromProperties() for (auto& comp : _adoptedSubcomponents) { comp->setOwner(*this); } - + // Provide sockets, inputs, and outputs with a pointer to its component // (this) so that they can invoke the component's methods. for (auto& it : _socketsTable) { @@ -226,7 +226,7 @@ void Component::finalizeFromProperties() markPropertiesAsSubcomponents(); componentsFinalizeFromProperties(); - // The following block is used to ensure that deserialized names of + // The following block is used to ensure that deserialized names of // Components are unique so they can be used to unambiguously locate // and connect all loaded Components. If a duplicate is encountered, // it is assigned a unique name. @@ -247,9 +247,9 @@ void Component::finalizeFromProperties() // while the name is still not unique keep incrementing the count while (names.find(uniqueName) != names.cend()) { - // In the future this should become an Exception + // In the future this should become an Exception //OPENSIM_THROW(SubcomponentsWithDuplicateName, getName(), searchName); - // for now, rename the duplicately named subcomponent + // for now, rename the duplicately named subcomponent // but first test the uniqueness of the name (the while condition) uniqueName = name + "_" + std::to_string(count++); } @@ -326,8 +326,8 @@ void Component::finalizeConnections(Component& root) } } - // Allow derived Components to handle/check their connections and also - // override the order in which its subcomponents are ordered when + // Allow derived Components to handle/check their connections and also + // override the order in which its subcomponents are ordered when // adding subcomponents to the System extendFinalizeConnections(root); @@ -371,7 +371,7 @@ void Component::clearConnections() for (auto& it : _socketsTable) { it.second->disconnect(); } - + // Must also clear the input's connections. for (auto& it : _inputsTable) { it.second->disconnect(); @@ -395,7 +395,7 @@ void Component::addToSystem(SimTK::MultibodySystem& system) const } // Base class implementation of virtual method. -// Every Component owns an underlying SimTK::Measure +// Every Component owns an underlying SimTK::Measure // which is a ComponentMeasure and is added to the System's default // subsystem. That measure is used only for the side effect of its realize() // methods being called; its value is not used. @@ -417,8 +417,8 @@ void Component::baseAddToSystem(SimTK::MultibodySystem& system) const Component* mutableThis = const_cast(this); mutableThis->_system = system; - // Allocate the ComponentMeasure, point it to this Component for - // making realize() calls, and add it to the system's default subsystem. + // Allocate the ComponentMeasure, point it to this Component for + // making realize() calls, and add it to the system's default subsystem. ComponentMeasure mcMeasure(system.updDefaultSubsystem(), *this); mutableThis->_simTKcomponentIndex = mcMeasure.getSubsystemMeasureIndex(); } @@ -441,7 +441,7 @@ void Component::componentsAddToSystem(SimTK::MultibodySystem& system) const } } else { - OPENSIM_THROW_FRMOBJ(Exception, + OPENSIM_THROW_FRMOBJ(Exception, "_orderedSubcomponents specified, but its size does not reflect the " "the number of immediate subcomponents. Verify that you have included " "all immediate subcomponents in the ordered list." @@ -490,10 +490,10 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const if(nsv > 0){ int nasv = 0; std::map::const_iterator it; - for(it = _namedStateVariableInfo.begin(); + for(it = _namedStateVariableInfo.begin(); it != _namedStateVariableInfo.end(); ++it){ const StateVariable& sv = *it->second.stateVariable; - const AddedStateVariable *asv = + const AddedStateVariable *asv = dynamic_cast(&sv); if(asv) nasv++; } @@ -501,7 +501,7 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const std::stringstream msg; msg << "Component " + getConcreteClassName()+"::"+getName(); msg << " added " << nasv << " state variables and "; - msg << " must specify their derivatives." << std::endl; + msg << " must specify their derivatives." << std::endl; throw Exception(msg.str()); } @@ -510,9 +510,9 @@ void Component::computeStateVariableDerivatives(const SimTK::State& s) const void Component:: -addModelingOption(const std::string& optionName, int maxFlagValue) const +addModelingOption(const std::string& optionName, int maxFlagValue) const { - // don't add modeling option if there is another state with the same + // don't add modeling option if there is another state with the same // name for this component std::map::const_iterator it; it = _namedModelingOptionInfo.find(optionName); @@ -545,17 +545,17 @@ void Component::addStateVariable(const std::string& stateVariableName, void Component::addStateVariable(Component::StateVariable* stateVariable) const { const std::string& stateVariableName = stateVariable->getName(); - // don't add state if there is another state variable with the same name + // don't add state if there is another state variable with the same name // for this component std::map::const_iterator it; it = _namedStateVariableInfo.find(stateVariableName); if(it != _namedStateVariableInfo.end()){ - throw Exception("Component::addStateVariable: State variable '" + + throw Exception("Component::addStateVariable: State variable '" + stateVariableName + "' already exists."); } int order = (int)_namedStateVariableInfo.size(); - + // assign a "slot" for a state variable by name // state variable index will be invalid by default // upon allocation during realizeTopology the index will be set @@ -573,23 +573,42 @@ void Component::addStateVariable(Component::StateVariable* stateVariable) const } - -void Component::addDiscreteVariable(const std::string& discreteVariableName, - SimTK::Stage invalidatesStage) const -{ - // don't add discrete var if there is another discrete variable with the - // same name for this component +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// A variaible 'allocate' was added to the argument list of +// addDiscreteVariable(). This was done to prevent double allocation of a +// discrete variable that is allocated outside of class Component. Such an +// allocation can occur when a native Simbody class, wrapped as an OpenSim +// Component, allocates its own discrete variables. +// +// When 'allocate' is true (default), the discrete state is allocated normally +// in Component::extendRealizeTopology(). +// +// When 'allocate' is false, the discrete variable is assumed to be allocated +// elswhere. In this case, the derived Component is responsible for +// initializing the index of the discrete state, as well its Subsystem. +// This should be done by implementing an overriding extendRealizeTopology() +// method and, in that method, calling Component::updDiscreteVariableIndex(); +// +// See ExponentialContact::extendRealizeTopology() for an example. +void +Component:: +addDiscreteVariable(const std::string& discreteVariableName, + SimTK::Stage invalidatesStage, bool allocate) const +{ + // Don't add discrete var if there is another discrete variable with the + // same name for this component. std::map::const_iterator it; it = _namedDiscreteVariableInfo.find(discreteVariableName); - if(it != _namedDiscreteVariableInfo.end()){ - throw Exception("Component::addDiscreteVariable: discrete variable '" + + if(it != _namedDiscreteVariableInfo.end()) { + throw Exception("Component::addDiscreteVariable: discrete variable '" + discreteVariableName + "' already exists."); } - // assign "slots" for the discrete variables by name - // discrete variable indices will be invalid by default - // upon allocation during realizeTopology the indices will be set - _namedDiscreteVariableInfo[discreteVariableName] = - DiscreteVariableInfo(invalidatesStage); + // Assign "slots" for the discrete variables by name. + // Discrete variable indices will be invalid by default. + // Upon allocation during realizeTopology, the indices will be set. + _namedDiscreteVariableInfo[discreteVariableName] = + DiscreteVariableInfo(invalidatesStage, allocate); } // Get the value of a ModelingOption flag for this Component. @@ -605,9 +624,9 @@ getModelingOption(const SimTK::State& s, const std::string& name) const getDefaultSubsystem().getDiscreteVariable(s, dvIndex)).get(); } else { std::stringstream msg; - msg << "Component::getModelingOption: ERR- name '" << name - << "' not found.\n " - << "for component '"<< getName() << "' of type " + msg << "Component::getModelingOption: ERR- name '" << name + << "' not found.\n " + << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); return -1; @@ -625,7 +644,7 @@ setModelingOption(SimTK::State& s, const std::string& name, int flag) const SimTK::DiscreteVariableIndex dvIndex = it->second.index; if(flag > it->second.maxOptionValue){ std::stringstream msg; - msg << "Component::setModelingOption: "<< name + msg << "Component::setModelingOption: "<< name << " flag cannot exceed "<< it->second.maxOptionValue <<".\n "; throw Exception(msg.str(),__FILE__,__LINE__); } @@ -634,7 +653,7 @@ setModelingOption(SimTK::State& s, const std::string& name, int flag) const getDefaultSubsystem().updDiscreteVariable(s, dvIndex)).upd() = flag; } else { std::stringstream msg; - msg << "Component::setModelingOption: modeling option " << name + msg << "Component::setModelingOption: modeling option " << name << " not found.\n "; throw Exception(msg.str(),__FILE__,__LINE__); } @@ -658,7 +677,7 @@ int Component::getNumStateVariables() const OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); //Get the number of state variables added (or exposed) by this Component - int ns = getNumStateVariablesAddedByComponent(); + int ns = getNumStateVariablesAddedByComponent(); // And then include the states of its subcomponents for (unsigned int i = 0; i<_memberSubcomponents.size(); i++) ns += _memberSubcomponents[i]->getNumStateVariables(); @@ -673,7 +692,7 @@ int Component::getNumStateVariables() const } -const Component& Component::getOwner() const +const Component& Component::getOwner() const { if (!hasOwner()) { std::string msg = "Component '" + getName() + "'::getOwner(). " + @@ -807,7 +826,7 @@ Array Component::getStateVariableNames() const for (auto& comp : getComponentList()) { const std::string& pathName = comp.getAbsolutePathString();// *this); - Array subStateNames = + Array subStateNames = comp.getStateVariableNamesAddedByComponent(); for (int i = 0; i < subStateNames.size(); ++i) { stateNames.append(pathName + "/" + subStateNames[i]); @@ -847,20 +866,20 @@ double Component:: // Get the value of a state variable derivative computed by this Component. double Component:: - getStateVariableDerivativeValue(const SimTK::State& state, + getStateVariableDerivativeValue(const SimTK::State& state, const std::string& name) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); computeStateVariableDerivatives(state); - + std::map::const_iterator it; it = _namedStateVariableInfo.find(name); if(it != _namedStateVariableInfo.end()) { return it->second.stateVariable->getDerivative(state); - } + } else{ // otherwise find the component that variable belongs to const StateVariable* rsv = traverseToStateVariable(name); @@ -870,9 +889,9 @@ double Component:: } std::stringstream msg; - msg << "Component::getStateVariableDerivative: ERR- variable name '" << name - << "' not found.\n " - << getName() << " of type " << getConcreteClassName() + msg << "Component::getStateVariableDerivative: ERR- variable name '" << name + << "' not found.\n " + << getName() << " of type " << getConcreteClassName() << " has " << getNumStateVariables() << " states."; throw Exception(msg.str(),__FILE__,__LINE__); return SimTK::NaN; @@ -892,10 +911,10 @@ void Component:: if(rsv){ // find required rummaging through the state variable names return rsv->setValue(s, value); } - + std::stringstream msg; - msg << "Component::setStateVariable: ERR- state named '" << name - << "' not found in " << getName() << " of type " + msg << "Component::setStateVariable: ERR- state named '" << name + << "' not found in " << getName() << " of type " << getConcreteClassName() << ".\n"; throw Exception(msg.str(),__FILE__,__LINE__); } @@ -903,7 +922,7 @@ void Component:: bool Component::isAllStatesVariablesListValid() const { int nsv = getNumStateVariables(); - // Consider the list of all StateVariables to be valid if all of + // Consider the list of all StateVariables to be valid if all of // the following conditions are true: // 1. Component is up-to-date with its Properties // 2. a System has been associated with the list of StateVariables @@ -912,7 +931,7 @@ bool Component::isAllStatesVariablesListValid() const // TODO: Enable the isObjectUpToDateWithProperties() check when computing // the path of the GeomtryPath does not involve updating its PathPointSet. // This change dirties the GeometryPath which is a property of a Muscle which - // is property of the Model. Therefore, during integration the Model is not + // is property of the Model. Therefore, during integration the Model is not // up-to-date and this causes a rebuilding of the cached StateVariables list. // See GeometryPath::computePath() for the corresponding TODO that must be // addressed before we can re-enable the isObjectUpToDateWithProperties @@ -970,7 +989,7 @@ void Component:: "Component::setStateVariableValues() number values does not match the " "number of state variables."); - // if the StateVariables are invalid (see above) rebuild the list + // if the StateVariables are invalid (see above) rebuild the list if (!isAllStatesVariablesListValid()) { _statesAssociatedSystem.reset(&getSystem()); _allStateVariables.clear(); @@ -987,7 +1006,7 @@ void Component:: // Set the derivative of a state variable computed by this Component by name. void Component:: - setStateVariableDerivativeValue(const State& state, + setStateVariableDerivativeValue(const State& state, const std::string& name, double value) const { std::map::const_iterator it; @@ -996,12 +1015,12 @@ void Component:: if(it != _namedStateVariableInfo.end()) { const StateVariable& sv = *it->second.stateVariable; sv.setDerivative(state, value); - } + } else{ std::stringstream msg; - msg << "Component::setStateVariableDerivative: ERR- name '" << name - << "' not found.\n " - << getName() << " of type " << getConcreteClassName() + msg << "Component::setStateVariableDerivative: ERR- name '" << name + << "' not found.\n " + << getName() << " of type " << getConcreteClassName() << " has " << getNumStateVariables() << " states."; throw Exception(msg.str(),__FILE__,__LINE__); } @@ -1019,13 +1038,28 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const if(it != _namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // F. C. Anderson + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); return SimTK::Value::downcast( - getDefaultSubsystem().getDiscreteVariable(s, dvIndex)).get(); + subsystem->getDiscreteVariable(s, dvIndex)).get(); + } else { std::stringstream msg; - msg << "Component::getDiscreteVariable: ERR- name '" << name - << "' not found.\n " - << "for component '"<< getName() << "' of type " + msg << "Component::getDiscreteVariable: ERR- name '" << name + << "' not found.\n " + << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); return SimTK::NaN; @@ -1044,13 +1078,28 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) if(it != _namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // F. C. Anderson + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); SimTK::Value::downcast( - getDefaultSubsystem().updDiscreteVariable(s, dvIndex)).upd() = value; + subsystem->updDiscreteVariable(s, dvIndex)).upd() = value; + } else { std::stringstream msg; - msg << "Component::setDiscreteVariable: ERR- name '" << name - << "' not found.\n " - << "for component '"<< getName() << "' of type " + msg << "Component::setDiscreteVariable: ERR- name '" << name + << "' not found.\n " + << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); } @@ -1139,7 +1188,7 @@ void Component::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) // Note: in finalizeFromProperties(), the Component will ensure // that names are unique by travesing its list of subcomponents // and renaming any duplicates. - node.setAttributeValue("name", + node.setAttributeValue("name", IO::Lowercase(getConcreteClassName())); } } @@ -1171,7 +1220,7 @@ void Component::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) iter->setElementTag(tagname); } } - + } if (versionNumber <= 30516) { // Rename xml tags for socket_*_connectee_name to socket_* @@ -1228,8 +1277,8 @@ void Component::markPropertiesAsSubcomponents() // Instead we can see if the object has a property called // "objects" which is a PropertyObjArray used by Set. // knowing the object Type is useful for debugging - // and it could be used to strengthen the test (e.g. scan - // for "Set" in the type name). + // and it could be used to strengthen the test (e.g. scan + // for "Set" in the type name). std::string objType = obj.getConcreteClassName(); if (obj.hasProperty("objects")) { // get the PropertyObjArray if the object has one @@ -1258,7 +1307,7 @@ void Component::markAsPropertySubcomponent(const Component* component) std::find(_propertySubcomponents.begin(), _propertySubcomponents.end(), compRef); if ( it == _propertySubcomponents.end() ){ // Must reconstruct the reference pointer in place in order - // to invoke move constructor from SimTK::Array::push_back + // to invoke move constructor from SimTK::Array::push_back // otherwise it will copy and reset the Component pointer to null. _propertySubcomponents.push_back( SimTK::ReferencePtr(const_cast(component))); @@ -1298,7 +1347,7 @@ void Component::adoptSubcomponent(Component* subcomponent) _adoptedSubcomponents.push_back(SimTK::ClonePtr(subcomponent)); } -std::vector> +std::vector> Component::getImmediateSubcomponents() const { std::vector> mySubcomponents; @@ -1344,9 +1393,9 @@ int Component::getStateIndex(const std::string& name) const return it->second.stateVariable->getVarIndex(); } else { std::stringstream msg; - msg << "Component::getStateVariableSystemIndex: ERR- name '" - << name << "' not found.\n " - << "for component '"<< getName() << "' of type " + msg << "Component::getStateVariableSystemIndex: ERR- name '" + << name << "' not found.\n " + << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); return SimTK::InvalidIndex; @@ -1356,17 +1405,17 @@ int Component::getStateIndex(const std::string& name) const SimTK::SystemYIndex Component:: getStateVariableSystemIndex(const std::string& stateVariableName) const { - //const SimTK::State& s = getSystem().getDefaultState(); - + //const SimTK::State& s = getSystem().getDefaultState(); + std::map::const_iterator it; it = _namedStateVariableInfo.find(stateVariableName); - + if(it != _namedStateVariableInfo.end()){ return it->second.stateVariable->getSystemYIndex(); } // Otherwise we have to search through subcomponents - SimTK::SystemYIndex yix; + SimTK::SystemYIndex yix; for(unsigned int i = 0; i < _propertySubcomponents.size(); ++i) { yix = _propertySubcomponents[i]->getStateVariableSystemIndex(stateVariableName); @@ -1374,7 +1423,7 @@ getStateVariableSystemIndex(const std::string& stateVariableName) const return yix; } } - + if(!(yix.isValid())){ throw Exception(getConcreteClassName() + "::getStateVariableSystemIndex : state variable " @@ -1393,12 +1442,33 @@ getDiscreteVariableIndex(const std::string& name) const return it->second.index; } +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method was added so that a derived Component can properly initialize +// the index and Subsystem of a discrete variable that is allocated outside +// of class Component. +void +Component:: +updDiscreteVariableIndex(const std::string& name, + const SimTK::DiscreteVariableIndex& index, + const SimTK::Subsystem* subsystem) const +{ + std::map::iterator it; + it = _namedDiscreteVariableInfo.find(name); + if (it == _namedDiscreteVariableInfo.end()) { + throw Exception("Component::updDiscreteVariableIndex: " + " discrete variable '" + name + "' not found."); + } + it->second.index = index; + it->second.subsystem = subsystem; +} + Array Component:: getStateVariableNamesAddedByComponent() const { std::map::const_iterator it; it = _namedStateVariableInfo.begin(); - + Array names("",(int)_namedStateVariableInfo.size()); while(it != _namedStateVariableInfo.end()){ @@ -1449,8 +1519,19 @@ void Component::extendRealizeTopology(SimTK::State& s) const // Allocate Discrete State Variables for (auto& kv : _namedDiscreteVariableInfo) { DiscreteVariableInfo& dvi = kv.second; - dvi.index = subSys.allocateDiscreteVariable( - s, dvi.invalidatesStage, new SimTK::Value(0.0)); + + // F. C. Anderson (Jan 2023) + // Do not allocate if the discrete state is allocated outside of class + // Component. This case is encountered when a native Simbody object, + // wrapped as an OpenSim Component, posseses discrete states of its + // own. In such a case, the derived Component is responsible for + // initializing the discrete state index, as well as its Subsystem. + // See ExponentialContact::extendAddToSystem() and + // ExponentialContact::extendRealizeTopology for an example. + if (!dvi.allocate) continue; + + dvi.index = subSys.allocateDiscreteVariable(s, + dvi.invalidatesStage, new SimTK::Value(0.0)); } // allocate cache entry in the state @@ -1516,16 +1597,16 @@ void Component::extendRealizeAcceleration(const SimTK::State& s) const if(getNumStateVariablesAddedByComponent() > 0) { const SimTK::Subsystem& subSys = getDefaultSubsystem(); - // evaluate and set component state derivative values (in cache) + // evaluate and set component state derivative values (in cache) computeStateVariableDerivatives(s); - + std::map::const_iterator it; - for (it = _namedStateVariableInfo.begin(); + for (it = _namedStateVariableInfo.begin(); it != _namedStateVariableInfo.end(); ++it) { const StateVariable& sv = *it->second.stateVariable; - const AddedStateVariable* asv = + const AddedStateVariable* asv = dynamic_cast(&sv); if(asv) // set corresponding system derivative value from @@ -1567,8 +1648,8 @@ double Component::AddedStateVariable::getValue(const SimTK::State& state) const } std::stringstream msg; - msg << "Component::AddedStateVariable::getValue: ERR- variable '" - << getName() << "' is invalid for component " << getOwner().getName() + msg << "Component::AddedStateVariable::getValue: ERR- variable '" + << getName() << "' is invalid for component " << getOwner().getName() << " of type " << getOwner().getConcreteClassName() <<"."; throw Exception(msg.str(),__FILE__,__LINE__); return SimTK::NaN; @@ -1584,8 +1665,8 @@ void Component::AddedStateVariable::setValue(SimTK::State& state, double value) } std::stringstream msg; - msg << "Component::AddedStateVariable::setValue: ERR- variable '" - << getName() << "' is invalid for component " << getOwner().getName() + msg << "Component::AddedStateVariable::setValue: ERR- variable '" + << getName() << "' is invalid for component " << getOwner().getName() << " of type " << getOwner().getConcreteClassName() <<"."; throw Exception(msg.str(),__FILE__,__LINE__); } @@ -1636,7 +1717,7 @@ void Component::printSocketInfo() const { } maxlenTypeName += 6; maxlenSockName += 1; - + for (const auto& it : _socketsTable) { const auto& socket = it.second; // Right-justify the connectee type names and socket names. @@ -1683,7 +1764,7 @@ void Component::printInputInfo() const { fmt::format("[{}]", input->getConnecteeTypeName()), maxlenTypeName, input->getName(), maxlenInputName); - if (input->getNumConnectees() == 0 || + if (input->getNumConnectees() == 0 || (input->getNumConnectees() == 1 && input->getConnecteePath().empty())) { str += "no connectees"; } else { @@ -1719,7 +1800,7 @@ void Component::printOutputInfo(const bool includeDescendants) const { for(const auto& output : outputs) maxlen = std::max(maxlen, output.second->getTypeName().length()); maxlen += 6; - + for(const auto& output : outputs) { const auto& name = output.second->getTypeName(); log_cout("{:>{}} {}", @@ -1749,7 +1830,7 @@ void Component::initComponentTreeTraversal(const Component &root) const { if (!hasOwner()) { // If this isn't the root component and it has no owner, then - // this is an orphan component and we likely failed to call + // this is an orphan component and we likely failed to call // finalizeFromProperties() on the root OR this is a clone that // has not been added to the root (in which case would have an owner). if (this != &root) { diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 3da4c2cd0b..b0bb31664e 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -2422,10 +2422,12 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void addStateVariable(Component::StateVariable* stateVariable) const; /** Add a system discrete variable belonging to this Component, give - it a name by which it can be referenced, and declare the lowest Stage that - should be invalidated if this variable's value is changed. **/ + it a name by which it can be referenced, declare the lowest Stage that + should be invalidated if this variable's value is changed, and specify + whether the discrete state should be allocated by class Component. + */ void addDiscreteVariable(const std::string& discreteVariableName, - SimTK::Stage invalidatesStage) const; + SimTK::Stage invalidatesStage, bool allocate = true) const; /** * Get writable reference to the MultibodySystem that this component is @@ -2455,6 +2457,17 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const SimTK::DiscreteVariableIndex getDiscreteVariableIndex(const std::string& name) const; + /** + * Update the index of a Component's discrete variable. This method is + * intended for derived Components that may need to initialize the index + * held by OpenSim. Such a situation occurs when an underlying Simbody + * subsystem is responsible for the allocation of a discrete variable + * (not OpenSim), and OpenSim needs to be told the value of that index. + */ + void updDiscreteVariableIndex(const std::string& name, + const SimTK::DiscreteVariableIndex& index, + const SimTK::Subsystem* subsystem = nullptr) const; + // End of System Creation and Access Methods. //@} @@ -3176,15 +3189,41 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); int order; }; - // Structure to hold related info about discrete variables + // Structure to hold related info about discrete variables. struct DiscreteVariableInfo { DiscreteVariableInfo() {} - explicit DiscreteVariableInfo(SimTK::Stage invalidates) - : invalidatesStage(invalidates) {} + explicit DiscreteVariableInfo(SimTK::Stage invalidates, + bool allocate = true) : + invalidatesStage(invalidates), allocate(allocate) {} + // Model SimTK::Stage invalidatesStage; + // System SimTK::DiscreteVariableIndex index; + + // F. C. Anderson (Jan 2023) + // Introduced two data members: 'subsystem' and 'allocate'. + // These two data members allow OpenSim::Component to expose a + // discrete state that was allocated by a class other than + // OpenSim::Component and as a member of Subsystem other than the + // default SimTK::Subsystem. + + // Subsystem to which the discrete state belongs. + // If 'nullptr' (default), class Component assumes that the discrete + // state was allocated as a member of the default SimTK::Subsystem. + const SimTK::Subsystem* subsystem{nullptr}; + + // Flag to prevent a double allocation. + // If 'true' (default), the discrete variable is allocated normally + // in Component::extendRealizeTopology(). + // If 'false', allocation in Component::extendRealizeTopology() is + // skipped and is assumed to occur elsewhere. In this case, the + // derived Component is responsible for initializing the index of the + // discrete state, as well its Subsystem. This should be done by + // implementing an overriding extendRealizeTopology() method. + // See ExponentialContact::extendRealizeTopology() for an example. + bool allocate{true}; }; /** From 0ea90bc8367b9f9bb69ca0b198021e24ae4e6a9d Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 12 Feb 2023 15:09:07 -0600 Subject: [PATCH 49/55] ExponentialContact: Exported elastic anchor point as a discrete variable. I also - refined a few documentation comments. - added getSubsystem() as a more general method for getting the non-default subsystem. --- .../Simulation/Model/ExponentialContact.cpp | 9 ++- OpenSim/Simulation/Model/ExponentialContact.h | 66 +++++++++++-------- 2 files changed, 45 insertions(+), 30 deletions(-) diff --git a/OpenSim/Simulation/Model/ExponentialContact.cpp b/OpenSim/Simulation/Model/ExponentialContact.cpp index 5f2e80c31d..c665aeae5f 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.cpp +++ b/OpenSim/Simulation/Model/ExponentialContact.cpp @@ -224,7 +224,8 @@ extendAddToSystem(SimTK::MultibodySystem& system) const { addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); name = getSlidingDiscreteStateName(); addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); - //addDiscreteVariable("anchor", SimTK::Stage::Dynamics, allocate); + name = getAnchorPointDiscreteStateName(); + addDiscreteVariable(name, SimTK::Stage::Dynamics, allocate); } @@ -262,7 +263,7 @@ extendRealizeTopology(SimTK::State& state) const { // source of the issue may be that ExponentialSpringForce::realizeTopology // was not called prior to ExponentialContact::extendRealizeTopology. - const SimTK::Subsystem* subsys = &_spr->getForceSubsystem(); + const SimTK::Subsystem* subsys = getSubsystem(); SimTK::DiscreteVariableIndex index; std::string name; @@ -277,6 +278,10 @@ extendRealizeTopology(SimTK::State& state) const { name = getSlidingDiscreteStateName(); index = _spr->getSlidingStateIndex(); updDiscreteVariableIndex(name, index, subsys); + + name = getAnchorPointDiscreteStateName(); + index = _spr->getAnchorPointStateIndex(); + updDiscreteVariableIndex(name, index, subsys); } diff --git a/OpenSim/Simulation/Model/ExponentialContact.h b/OpenSim/Simulation/Model/ExponentialContact.h index d9a610e817..4d88c947ac 100644 --- a/OpenSim/Simulation/Model/ExponentialContact.h +++ b/OpenSim/Simulation/Model/ExponentialContact.h @@ -361,29 +361,35 @@ class OSIMSIMULATION_API ExponentialContact : public Force { //------------------------------------------------------------------------- // Accessors for Discrete States //------------------------------------------------------------------------- + /** Get a pointer to the SimTK::Subsystem from which this + ExponentialContact instance allocates its discrete states. */ + const SimTK::Subsystem* getSubsystem() const { + return &_spr->getForceSubsystem(); + } + /** Get the name used for the discrete state representing the static - coefficient of friction (μₛ). This is the name used to access informattion + coefficient of friction (μₛ). This name is used to access informattion related to μₛ via the OpenSim::Component API. For example, see Component::getDiscreteVariableValue(). */ std::string getMuStaticDiscreteStateName() const { return "mu_static"; } /** Set the static coefficient of friction (μₛ) for this exponential - spring. μₛ is a discrete state. The value of μₛ is held in the System's State object. Unlike the - parameters managed by ExponentialSpringParameters, μₛ can be set at any - time during a simulation. A change to μₛ will invalidate the System at - Stage::Dynamics, but not at Stage::Topology. + spring. μₛ is a discrete state. The value of μₛ is held in the System's + State object. Unlike the parameters managed by ExponentialSpringParameters, + μₛ can be set at any time during a simulation. A change to μₛ will + invalidate the System at Stage::Dynamics, but not at Stage::Topology. @param state State object that will be modified. @param mus %Value of the static coefficient of friction. No upper bound. 0.0 ≤ μₛ. If μₛ < μₖ, μₖ is set equal to μₛ. */ void setMuStatic(SimTK::State& state, SimTK::Real mus); /** Get the static coefficient of friction (μₛ) held by the specified - state for this exponential spring. + state for this exponential contact instance. @param state State object from which to retrieve μₛ. */ SimTK::Real getMuStatic(const SimTK::State& state) const; /** Get the name used for the discrete state representing the kinetic - coefficient of friction (μₖ). This is the name used to access informattion + coefficient of friction (μₖ). This name is the used to access informattion related to μₖ via the OpenSim::Component API. For example, see Component::getDiscreteVariableValue(). */ std::string getMuKineticDiscreteStateName() const { return "mu_kinetic"; } @@ -399,20 +405,40 @@ class OSIMSIMULATION_API ExponentialContact : public Force { void setMuKinetic(SimTK::State& state, SimTK::Real muk); /** Get the kinetic coefficient of friction (μₖ) held by the specified - state for this exponential spring. + state for this exponential contact instance. @param state State object from which to retrieve μₖ. */ SimTK::Real getMuKinetic(const SimTK::State& state) const; /** Get the name used for the discrete state representing the 'sliding' - state (K) of the elastic anchor point. This is the name used to access + state (K) of the elastic anchor point. This name is used to access informattion related to K via the OpenSim::Component API. For example, see Component::getDiscreteVariableValue(). */ std::string getSlidingDiscreteStateName() const { return "sliding"; } - /** Get the Sliding state of the spring. - @param state State object from which to retrieve Sliding. */ + /** Get the Sliding state of this exponential contact instance after it + has been updated to be consistent with the System State. The System must + be realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. */ SimTK::Real getSliding(const SimTK::State& state) const; + /** Get the name used for the discrete state representing the position + of the elastic anchor point (p₀). This name is used to access + informattion related to p₀ via the OpenSim::Component API. For example, + see Component::getDiscreteVariableAbstractValue(). */ + std::string getAnchorPointDiscreteStateName() const { return "anchor"; } + + /** Get the position of the elastic anchor point (p₀) after it has been + updated to be consistent with friction limits. p₀ is the spring zero of + the damped linear spring used in Friction Model 2. The system must be + realized to Stage::Dynamics to access this data. + @param state State object on which to base the calculations. + @param inGround Flag for choosing the frame in which the returned quantity + will be expressed. If true (the default), the quantity will be expressed + in the Ground frame. If false, the quantity will be expressed in the frame + of the contact plane. */ + SimTK::Vec3 getAnchorPointPosition( + const SimTK::State& state, bool inGround = true) const; + //------------------------------------------------------------------------- // Accessors for data cache entries //------------------------------------------------------------------------- @@ -533,18 +559,6 @@ class OSIMSIMULATION_API ExponentialContact : public Force { SimTK::Vec3 getStationVelocity( const SimTK::State& state, bool inGround = true) const; - /** Get the position of the elastic anchor point (p₀), which will always - lie in the Contact Plane. p₀ is the spring zero of the damped linear - spring used in Friction Model 2. The system must be realized to - Stage::Dynamics to access this data. - @param state State object on which to base the calculations. - @param inGround Flag for choosing the frame in which the returned quantity - will be expressed. If true (the default), the quantity will be expressed - in the Ground frame. If false, the quantity will be expressed in the frame - of the contact plane. */ - SimTK::Vec3 getAnchorPointPosition( - const SimTK::State& state, bool inGround = true) const; - //------------------------------------------------------------------------- // Reporting //------------------------------------------------------------------------- @@ -625,12 +639,8 @@ class ExponentialContact::Parameters : public Object { "Elasticity of the friction spring (20,000.0 N/m)."); OpenSim_DECLARE_PROPERTY(friction_viscosity, double, "Viscosity of the friction spring (282.8427 N*s/m)."); - OpenSim_DECLARE_PROPERTY(sliding_time_constant, double, - "Time constant for rise/decay between static and kinetic friction conditions (0.01 s)."); - OpenSim_DECLARE_PROPERTY(settle_velocity, double, + OpenSim_DECLARE_PROPERTY(settle_velocity, double, "Velocity below which static friction conditions are triggered (0.01 m/s) ."); - OpenSim_DECLARE_PROPERTY(settle_acceleration, double, - "Acceleration below which static friction conditions are triggered (1.0 m/s²)."); OpenSim_DECLARE_PROPERTY(initial_mu_static, double, "Initial value of the static coefficient of friction."); OpenSim_DECLARE_PROPERTY(initial_mu_kinetic, double, From 437feb78ba66b9a19b39068b8e878b325b038921 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 12 Feb 2023 15:18:24 -0600 Subject: [PATCH 50/55] [WIP] Modifications to support serializing Discrete Variables. The code is not working in this current commit. Continuing from the current modifications, I am going to experiment with several approaches to support serializing discrete Variaibles. Rather than making changes in the current code, I'm going to branch. When I find an approach that works, I will merge back. --- OpenSim/Common/Component.cpp | 186 +++++++++++++++++- OpenSim/Common/Component.h | 96 ++++++++- .../Test/testExponentialContact.cpp | 130 +++++++++--- 3 files changed, 369 insertions(+), 43 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 241acbc8ea..10564ad17e 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -779,6 +779,55 @@ ComponentPath Component::getRelativePath(const Component& wrt) const return thisP.formRelativePath(wrtP); } +//_____________________________________________________________________________ +// F. C. Anderson (Feb 2023) +// Added so that get, set, and upd methods can be called for discrete +// variables based on an absolute path. +// +// For a discrete variable, the final element in the path string +// (i.e., the leaf) is not a component, but rather simply the name of the +// discrete variable. This name is the key used to look up the discrete +// variable info in the internal std::map _namedDiscreteVariableInfo. +// +// Two things are needed: +// 1. The component owner of the discrete variable, which is not the +// leaf but the second-to-last element in the path. +// 2. +const Component* Component::traverseToLastOwner( + const std::string& pathName) const { + return traverseToStateVariable(ComponentPath{pathName}); +} + +const Component::StateVariable* Component::traverseToStateVariable( + const ComponentPath& path) const { + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + const StateVariable* found = nullptr; + if (path.getNumPathLevels() == 1) { + // There was no slash. The state variable should be in this component. + auto it = _namedStateVariableInfo.find(path.toString()); + if (it != _namedStateVariableInfo.end()) { + return it->second.stateVariable.get(); + } + } else if (path.getNumPathLevels() > 1) { + const auto& compPath = path.getParentPath(); + const Component* comp = traversePathToComponent(compPath); + if (comp) { + // This is the leaf of the path: + const auto& varName = path.getComponentName(); + found = comp->traverseToStateVariable(varName); + } + } + + return found; +} + + + + + + const Component::StateVariable* Component:: traverseToStateVariable(const std::string& pathName) const { @@ -1026,9 +1075,76 @@ void Component:: } } -// Get the value of a discrete variable allocated by this Component by name. -double Component:: + +//_____________________________________________________________________________ +// F. C. Anderson (Feb 2023) +// Added so that discrete states can be serialized and deserialized. +// +// Get the names of discrete state variables maintained by the Component and +// its subcomponents. +Array Component::getDiscreteVariableNames() const { + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + Array dvNames = getDiscreteVariableNamesAddedByComponent(); + + for (int i = 0; i < dvNames.size(); ++i) { + dvNames[i] = (getAbsolutePathString() + "/" + dvNames[i]); + } + + for (auto& comp : getComponentList()) { + const std::string& pathName = comp.getAbsolutePathString(); // *this); + Array subDVNames = + comp.getDiscreteVariableNamesAddedByComponent(); + for (int i = 0; i < subDVNames.size(); ++i) { + dvNames.append(pathName + "/" + subDVNames[i]); + } + } + + return dvNames; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Feb 2023) +// Added so that discrete states can be serialized and deserialized. +Array Component::getDiscreteVariableNamesAddedByComponent() const { + std::map::const_iterator it; + it = _namedDiscreteVariableInfo.begin(); + + Array names("", (int)_namedDiscreteVariableInfo.size()); + + int i = 0; + while (it != _namedDiscreteVariableInfo.end()) { + names[i] = it->first; + it++; + i++; + } + return names; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// Get the value (assumed to be type double) of a discrete variable allocated +// by this Component by name. So that existing code does not need to change, +// this method fulfills the API prior to Jan 2023. +double +Component:: getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const +{ + double value = SimTK::NaN; + value = SimTK::Value::downcast( + getDiscreteVariableAbstractValue(s, name)); + return value; +} + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method was added in order to handle Discrete Variables that are not of +// type double. +const SimTK::AbstractValue& +Component:: +getDiscreteVariableAbstractValue(const SimTK::State& s, + const std::string& name) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); @@ -1039,7 +1155,7 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const if(it != _namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; - // F. C. Anderson + // F. C. Anderson (Jan 2023) // Previously, it was assumed that all discrete states were allocated // from the default Subsystem. This is likely not the case when // discrete states are allocated by native Simbody objects. For @@ -1052,8 +1168,7 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const // default value, then the default Subsystem is used. const SimTK::Subsystem* subsystem = it->second.subsystem; if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); - return SimTK::Value::downcast( - subsystem->getDiscreteVariable(s, dvIndex)).get(); + return subsystem->getDiscreteVariable(s, dvIndex); } else { std::stringstream msg; @@ -1062,13 +1177,61 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const << "for component '"<< getName() << "' of type " << getConcreteClassName(); throw Exception(msg.str(),__FILE__,__LINE__); - return SimTK::NaN; } } -// Set the value of a discrete variable allocated by this Component by name. -void Component:: -setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) const + +//_____________________________________________________________________________ +// F. C. Anderson (Jan 2023) +// This method was added in order to handle Discrete Variables that +// are not of type double. +SimTK::AbstractValue& +Component:: +updDiscreteVariableAbstractValue(SimTK::State& s, + const std::string& name) const +{ + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + std::map::const_iterator it; + it = _namedDiscreteVariableInfo.find(name); + + if (it != _namedDiscreteVariableInfo.end()) { + SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // F. C. Anderson (Jan 2023) + // Previously, it was assumed that all discrete states were allocated + // from the default Subsystem. This is likely not the case when + // discrete states are allocated by native Simbody objects. For + // example, class ExponentialSpringForce allocates 4 discrete states, + // not from the default Subsystem, but from the GeneralForceSubsystem. + // To account for the fact that an object might allocate discrete + // variables from a different Subsystem, a pointer was added to the + // DiscreteVariableInfo struct. This pointer is now consulted for any + // non-default Subsystem. If this pointer is nullptr, which is its + // default value, then the default Subsystem is used. + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + return subsystem->updDiscreteVariable(s, dvIndex); + + } else { + std::stringstream msg; + msg << "Component::getDiscreteVariable: ERR- name '" << name + << "' not found.\n " + << "for component '" << getName() << "' of type " + << getConcreteClassName(); + throw Exception(msg.str(), __FILE__, __LINE__); + } +} + + +//_____________________________________________________________________________ +// Set the value (assumed to be double) of a discrete variable allocated by +// this Component by name. +void +Component:: +setDiscreteVariableValue(SimTK::State& s, const std::string& name, + double value) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); @@ -1079,7 +1242,7 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) if(it != _namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; - // F. C. Anderson + // F. C. Anderson (Jan 2023) // Previously, it was assumed that all discrete states were allocated // from the default Subsystem. This is likely not the case when // discrete states are allocated by native Simbody objects. For @@ -1105,6 +1268,9 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, double value) } } + + + SimTK::CacheEntryIndex Component::getCacheVariableIndex(const std::string& name) const { auto it = this->_namedCacheVariables.find(name); diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index b0bb31664e..989199076b 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -860,6 +860,13 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ Array getStateVariableNames() const; + /** + * Get the names of discrete state variables maintained by the Component + * and its subcomponents. + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + Array getDiscreteVariableNames() const; /** @name Component Socket Access methods Access Sockets of this component by name. */ @@ -1369,16 +1376,45 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const std::string& name) const; /** - * Get the value of a discrete variable allocated by this Component by name. + * Get the value (assumed to be type double) of a discrete variable + * allocated by this Component by name. * * @param state the State from which to get the value * @param name the name of the state variable - * @return value the discrete variable value + * @return value the discrete variable value as a double * @throws ComponentHasNoSystem if this Component has not been added to a * System (i.e., if initSystem has not been called) */ double getDiscreteVariableValue(const SimTK::State& state, - const std::string& name) const; + const std::string& name) const; + + /** + * Retrieve a read-only reference to the abstract value of the discrete + * variable of a specified name. This method provides a more general + * interface to handle discrete variables that may not be type double. + * + * @param state the State from which to get the value + * @param name the name of the discrete state variable + * @return value the discrete variable value as a SimTK::AbstractValue + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + const SimTK::AbstractValue& getDiscreteVariableAbstractValue( + const SimTK::State& state, const std::string& name) const; + + /** + * Retrieve a writable reference to the abstract value of the discrete + * variable of a specified name. This method provides a more general + * interface to handle discrete variables that may not be type double. + * + * @param state the State from which to get the value + * @param name the name of the discrete state variable + * @return value the discrete variable value as a SimTK::AbstractValue + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + SimTK::AbstractValue& updDiscreteVariableAbstractValue( + SimTK::State& state, const std::string& name) const; /** * %Set the value of a discrete variable allocated by this Component by name. @@ -1392,6 +1428,48 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); void setDiscreteVariableValue(SimTK::State& state, const std::string& name, double value) const; + /** + * %Set the value of a discrete variable allocated by this Component by + * name. This method is a template to account for Discrete Variables that + * are not type double. + * + * @param s the State for which to set the value + * @param name the name of the discrete variable + * @param value the value to set + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called) + */ + template + void setDiscreteVariableValue(SimTK::State& s, const std::string& name, + const T& value) const + { + // Must have already called initSystem. + OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + + std::map::const_iterator it; + it = _namedDiscreteVariableInfo.find(name); + + if (it != _namedDiscreteVariableInfo.end()) { + SimTK::DiscreteVariableIndex dvIndex = it->second.index; + + // Account for non-default subsystem + const SimTK::Subsystem* subsystem = it->second.subsystem; + if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); + + // Update the value + SimTK::Value::downcast( + subsystem->updDiscreteVariable(s,dvIndex)) = value; + + } else { + std::stringstream msg; + msg << "Component::setDiscreteVariable: ERR- name '" << name + << "' not found.\n " + << "for component '" << getName() << "' of type " + << getConcreteClassName(); + throw Exception(msg.str(), __FILE__, __LINE__); + } + } + /** * A cache variable containing a value of type T. * @@ -2947,6 +3025,18 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); { return (int)_namedStateVariableInfo.size(); } Array getStateVariableNamesAddedByComponent() const; + // F. C. Anderson (Feb 2023) + // Added so that discrete states can be serialized and deserialized. + // + // Get the number of discrete states that the Component added to the + // underlying computational system. It includes the number of built-in + // discrete states exposed by this component. It represents the number of + // discrete variables managed by this Component. + int getNumDiscreteVariablesAddedByComponent() const { + return (int)_namedDiscreteVariableInfo.size(); + } + Array getDiscreteVariableNamesAddedByComponent() const; + const SimTK::DefaultSystemSubsystem& getDefaultSubsystem() const { return getSystem().getDefaultSubsystem(); } SimTK::DefaultSystemSubsystem& updDefaultSubsystem() const diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index af1510cd0f..61db99044b 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -123,8 +123,8 @@ class ExponentialContactTester void testParameters(); void testSerialization(); void testDiscreteVariable(State& state, const ExponentialContact& ec, - const string& name, double valOrig, - double delta = 0.1, double tol = 1.0e-6); + const string& name); + void testDiscreteVariables(State& state, const ForceSet& fSet); // Simulation void setInitialConditions(SimTK::State& state, @@ -681,22 +681,107 @@ testSerialization() { void ExponentialContactTester:: testDiscreteVariable(State& state, const ExponentialContact& ec, - const string& name, double val, double delta, double tol) + const string& name) { - // Get the starting value, which should be the same as val. - double valDV = ec.getDiscreteVariableValue(state, name); - ASSERT_EQUAL(val, valDV, tol); + // Get the starting value. + const SimTK::AbstractValue& val = + ec.getDiscreteVariableAbstractValue(state, name); + + /* + // Switch depending on the type + if (SimTK::Value::isA(val)) { + SimTK::Value deltaDouble(delta); + SimTK::Value tolDouble(tol); + + // + } // Set a new value. double valNew = val + delta; - ec.setDiscreteVariableValue(state, name, valNew); + ec.setDiscreteVariableValue(state, name, valNew); double valNewDV = ec.getDiscreteVariableValue(state, name); ASSERT_EQUAL(valNew, valNewDV, tol); // Restore the starting value. ec.setDiscreteVariableValue(state, name, val); - valDV = ec.getDiscreteVariableValue(state, name); + double valDV = SimTK::Value::downcast( + ec.getDiscreteVariableAbstractValue(state, name)); ASSERT_EQUAL(val, valDV, tol); + */ +} + +//_____________________________________________________________________________ +// Only types that are handled are double and Vec3. +void +ExponentialContactTester:: +testDiscreteVariables(State& state, const ForceSet& fSet) { + + // Get the names + OpenSim::Array names = fSet.getDiscreteVariableNames(); + + // Loop + int n = names.size(); + for (int i = 0; i < n; ++i) { + + // Starting value + SimTK::AbstractValue& valAbstract = + fSet.updDiscreteVariableAbstractValue(state, names[i]); + + // Declarations + double tol = 1.0e-6; + double deltaDbl = 0.1; + Vec3 deltaVec3(deltaDbl); + double valStartDbl{NaN}; + Vec3 valStartVec3{NaN}; + + // Perturb + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + valStartDbl = valDbl.get(); + valDbl = valStartDbl + deltaDbl; + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + valStartVec3 = valVec3.get(); + valVec3 = valStartVec3 + deltaVec3; + } + + // Check that the value changed correctly + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valDbl.get(), valStartDbl + deltaDbl, tol); + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valVec3.get(), valStartVec3 + deltaVec3, tol); + } + + // Restore + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + valDbl = valStartDbl; + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + valVec3 = valStartVec3; + } + + // Check that the value was restored correctly + if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valDbl = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valDbl.get(), valStartDbl, tol); + } else if (SimTK::Value::isA(valAbstract)) { + SimTK::Value& valVec3 = + SimTK::Value::updDowncast(valAbstract); + ASSERT_EQUAL(valVec3.get(), valStartVec3, tol); + } + + } + } //_____________________________________________________________________________ @@ -726,30 +811,15 @@ simulate() int n = fSet.getSize(); for (i = 0; i < n; ++i) { try { - string name; - double delta = 0.1; - double tol = 1.0e-6; - double val{0.0}, valAfter{0.0}; // Get the ExponentialContact Component - ExponentialContact& ec = - dynamic_cast(fSet.get(i)); - string path = ec.getAbsolutePathString(); - cout << "path = " << path << endl; - - // Static Coefficient of Friction - name = ec.getMuStaticDiscreteStateName(); - val = ec.getMuStatic(state); - testDiscreteVariable(state, ec, name, val, delta, tol); - valAfter = ec.getMuStatic(state); - ASSERT_EQUAL(val, valAfter, tol); - - // Kinetic Coefficient of Friction - name = ec.getMuKineticDiscreteStateName(); - val = ec.getMuKinetic(state); - testDiscreteVariable(state, ec, name, val, delta, tol); - valAfter = ec.getMuKinetic(state); - ASSERT_EQUAL(val, valAfter, tol); + //ExponentialContact& ec = + // dynamic_cast(fSet.get(i)); + //string path = ec.getAbsolutePathString(); + //cout << "path = " << path << endl; + + // Test DiscreteVariable Component API + testDiscreteVariables(state, fSet); } catch (const std::exception& e) { // Nothing should happen here. Execution is just skipping any From 63ff32fea92fdaa1047b35f8508fd15ead4332f4 Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Wed, 26 Apr 2023 21:58:39 -0500 Subject: [PATCH 51/55] Removed wip methods for traversing to a DiscreteVariable. These methods were a work in progress [wip] and were never actually functioning. They will only be needed if the decision is to go forward with serializing/deserializing discrete states. If so, then it's probably best to start from a clean slate on these anyway. This commit is also accompanied by a few minor typo corrections. --- OpenSim/Common/Component.cpp | 52 ++---------------------------------- OpenSim/Common/Component.h | 2 +- 2 files changed, 3 insertions(+), 51 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 10564ad17e..8c83ef5773 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -779,54 +779,6 @@ ComponentPath Component::getRelativePath(const Component& wrt) const return thisP.formRelativePath(wrtP); } -//_____________________________________________________________________________ -// F. C. Anderson (Feb 2023) -// Added so that get, set, and upd methods can be called for discrete -// variables based on an absolute path. -// -// For a discrete variable, the final element in the path string -// (i.e., the leaf) is not a component, but rather simply the name of the -// discrete variable. This name is the key used to look up the discrete -// variable info in the internal std::map _namedDiscreteVariableInfo. -// -// Two things are needed: -// 1. The component owner of the discrete variable, which is not the -// leaf but the second-to-last element in the path. -// 2. -const Component* Component::traverseToLastOwner( - const std::string& pathName) const { - return traverseToStateVariable(ComponentPath{pathName}); -} - -const Component::StateVariable* Component::traverseToStateVariable( - const ComponentPath& path) const { - // Must have already called initSystem. - OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); - - const StateVariable* found = nullptr; - if (path.getNumPathLevels() == 1) { - // There was no slash. The state variable should be in this component. - auto it = _namedStateVariableInfo.find(path.toString()); - if (it != _namedStateVariableInfo.end()) { - return it->second.stateVariable.get(); - } - } else if (path.getNumPathLevels() > 1) { - const auto& compPath = path.getParentPath(); - const Component* comp = traversePathToComponent(compPath); - if (comp) { - // This is the leaf of the path: - const auto& varName = path.getComponentName(); - found = comp->traverseToStateVariable(varName); - } - } - - return found; -} - - - - - const Component::StateVariable* Component:: traverseToStateVariable(const std::string& pathName) const @@ -1139,7 +1091,7 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const //_____________________________________________________________________________ // F. C. Anderson (Jan 2023) -// This method was added in order to handle Discrete Variables that are not of +// This method was added in order to handle Discrete Variables that are not // type double. const SimTK::AbstractValue& Component:: @@ -1184,7 +1136,7 @@ getDiscreteVariableAbstractValue(const SimTK::State& s, //_____________________________________________________________________________ // F. C. Anderson (Jan 2023) // This method was added in order to handle Discrete Variables that -// are not of type double. +// are not type double. SimTK::AbstractValue& Component:: updDiscreteVariableAbstractValue(SimTK::State& s, diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 989199076b..0bc2460424 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -3310,7 +3310,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); // If 'false', allocation in Component::extendRealizeTopology() is // skipped and is assumed to occur elsewhere. In this case, the // derived Component is responsible for initializing the index of the - // discrete state, as well its Subsystem. This should be done by + // discrete state, as well as its Subsystem. This should be done by // implementing an overriding extendRealizeTopology() method. // See ExponentialContact::extendRealizeTopology() for an example. bool allocate{true}; From 69e747ee4c120597ffd824d021358ea9958c57cb Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sat, 6 May 2023 07:14:22 -0500 Subject: [PATCH 52/55] Discrete variables can now be accessed by path. Component methods getDiscreteVariableAbstractValue() and updDiscreteVariableAbstractValue() will now access the value of a discrete variable based on a specified Model hierarchy path. I added a utility method, Component::resolveDiscreteVariableNameAndOwner(), to support this functionality. These additions do not require any adjustments to existing OpenSim code. They are intended to support the ability to serialize and deserialize discrete variables so that a complete SimTK::State can be captured. I have included some testing code in testExponentialContact.cpp that exercise this new functionality. --- OpenSim/Common/Component.cpp | 96 ++++++++----- OpenSim/Common/Component.h | 132 ++++++++++++++---- .../Test/testExponentialContact.cpp | 80 ++++------- 3 files changed, 199 insertions(+), 109 deletions(-) diff --git a/OpenSim/Common/Component.cpp b/OpenSim/Common/Component.cpp index 8c83ef5773..1fa772a729 100644 --- a/OpenSim/Common/Component.cpp +++ b/OpenSim/Common/Component.cpp @@ -812,6 +812,7 @@ const Component::StateVariable* Component::traverseToStateVariable( return found; } + // Get the names of "continuous" state variables maintained by the Component and // its subcomponents. Array Component::getStateVariableNames() const @@ -1090,13 +1091,12 @@ getDiscreteVariableValue(const SimTK::State& s, const std::string& name) const } //_____________________________________________________________________________ -// F. C. Anderson (Jan 2023) -// This method was added in order to handle Discrete Variables that are not -// type double. -const SimTK::AbstractValue& +// Set the value (assumed to be double) of a discrete variable allocated by +// this Component by name. +void Component:: -getDiscreteVariableAbstractValue(const SimTK::State& s, - const std::string& name) const +setDiscreteVariableValue(SimTK::State& s, const std::string& name, + double value) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); @@ -1120,11 +1120,12 @@ getDiscreteVariableAbstractValue(const SimTK::State& s, // default value, then the default Subsystem is used. const SimTK::Subsystem* subsystem = it->second.subsystem; if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); - return subsystem->getDiscreteVariable(s, dvIndex); + SimTK::Value::downcast( + subsystem->updDiscreteVariable(s, dvIndex)).upd() = value; } else { std::stringstream msg; - msg << "Component::getDiscreteVariable: ERR- name '" << name + msg << "Component::setDiscreteVariable: ERR- name '" << name << "' not found.\n " << "for component '"<< getName() << "' of type " << getConcreteClassName(); @@ -1132,23 +1133,49 @@ getDiscreteVariableAbstractValue(const SimTK::State& s, } } +//_____________________________________________________________________________ +// F. C. Anderson (May 2023) +// Added so that a discrete variable can be accessed based on a specified path. +const Component* +Component:: +resolveDiscreteVariableNameAndOwner(const std::string& pathName, + std::string& dvName) const +{ + ComponentPath path{pathName}; + size_t nLevels = path.getNumPathLevels(); + dvName = path.getSubcomponentNameAtLevel(nLevels - 1); + const Component* owner = this; + if (nLevels > 1) { + // Need to traverse to the owner of the DV based on the path. + const ComponentPath& ownerPath = path.getParentPath(); + owner = traversePathToComponent(ownerPath); + } + return owner; +} //_____________________________________________________________________________ -// F. C. Anderson (Jan 2023) -// This method was added in order to handle Discrete Variables that -// are not type double. -SimTK::AbstractValue& +// F. C. Anderson (Jan 2023, May 2023) +// This method was added in order to handle Discrete Variables (DV) that are +// not type double. In addition, a DV can be accessed by specifying its path +// instead of just the name of the DV. +const SimTK::AbstractValue& Component:: -updDiscreteVariableAbstractValue(SimTK::State& s, - const std::string& name) const +getDiscreteVariableAbstractValue(const SimTK::State& s, + const std::string& pathName) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + // Resolve the name of the DV and its owner. + std::string dvName{""}; + const Component* owner = + resolveDiscreteVariableNameAndOwner(pathName, dvName); + + // Find the variable. std::map::const_iterator it; - it = _namedDiscreteVariableInfo.find(name); + it = owner->_namedDiscreteVariableInfo.find(dvName); - if (it != _namedDiscreteVariableInfo.end()) { + if (it != owner->_namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; // F. C. Anderson (Jan 2023) @@ -1164,11 +1191,11 @@ updDiscreteVariableAbstractValue(SimTK::State& s, // default value, then the default Subsystem is used. const SimTK::Subsystem* subsystem = it->second.subsystem; if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); - return subsystem->updDiscreteVariable(s, dvIndex); + return subsystem->getDiscreteVariable(s, dvIndex); } else { std::stringstream msg; - msg << "Component::getDiscreteVariable: ERR- name '" << name + msg << "Component::getDiscreteVariable: ERR- name '" << pathName << "' not found.\n " << "for component '" << getName() << "' of type " << getConcreteClassName(); @@ -1176,22 +1203,28 @@ updDiscreteVariableAbstractValue(SimTK::State& s, } } - //_____________________________________________________________________________ -// Set the value (assumed to be double) of a discrete variable allocated by -// this Component by name. -void +// F. C. Anderson (Jan 2023, May 2023) +// This method was added in order to handle Discrete Variables (DV) that are +// not type double and, in addition, to allow a path to be specified instead +// of just the name of the DV. +SimTK::AbstractValue& Component:: -setDiscreteVariableValue(SimTK::State& s, const std::string& name, - double value) const +updDiscreteVariableAbstractValue(SimTK::State& s, + const std::string& pathName) const { // Must have already called initSystem. OPENSIM_THROW_IF_FRMOBJ(!hasSystem(), ComponentHasNoSystem); + // Resolve the name of the DV and its owner. + std::string dvName{""}; + const Component* owner = + resolveDiscreteVariableNameAndOwner(pathName, dvName); + std::map::const_iterator it; - it = _namedDiscreteVariableInfo.find(name); + it = owner->_namedDiscreteVariableInfo.find(dvName); - if(it != _namedDiscreteVariableInfo.end()) { + if (it != owner->_namedDiscreteVariableInfo.end()) { SimTK::DiscreteVariableIndex dvIndex = it->second.index; // F. C. Anderson (Jan 2023) @@ -1207,22 +1240,19 @@ setDiscreteVariableValue(SimTK::State& s, const std::string& name, // default value, then the default Subsystem is used. const SimTK::Subsystem* subsystem = it->second.subsystem; if (subsystem == nullptr) subsystem = &getDefaultSubsystem(); - SimTK::Value::downcast( - subsystem->updDiscreteVariable(s, dvIndex)).upd() = value; + return subsystem->updDiscreteVariable(s, dvIndex); } else { std::stringstream msg; - msg << "Component::setDiscreteVariable: ERR- name '" << name + msg << "Component::updDiscreteVariable: ERR- name '" << pathName << "' not found.\n " - << "for component '"<< getName() << "' of type " + << "for component '" << getName() << "' of type " << getConcreteClassName(); - throw Exception(msg.str(),__FILE__,__LINE__); + throw Exception(msg.str(), __FILE__, __LINE__); } } - - SimTK::CacheEntryIndex Component::getCacheVariableIndex(const std::string& name) const { auto it = this->_namedCacheVariables.find(name); diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 0bc2460424..e03ff1411b 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1389,44 +1389,117 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); const std::string& name) const; /** - * Retrieve a read-only reference to the abstract value of the discrete - * variable of a specified name. This method provides a more general - * interface to handle discrete variables that may not be type double. + * %Set the value of a discrete variable allocated by this Component by name. * - * @param state the State from which to get the value - * @param name the name of the discrete state variable - * @return value the discrete variable value as a SimTK::AbstractValue + * @param state the State for which to set the value + * @param name the name of the discrete variable + * @param value the value to set * @throws ComponentHasNoSystem if this Component has not been added to a * System (i.e., if initSystem has not been called) */ + void setDiscreteVariableValue(SimTK::State& state, const std::string& name, + double value) const; + + + //------------------------------------------------------------------------- + // F. C. Anderson (January 2023, May 2023) + // Added the ability to get the value of a discrete variable as a + // SimTK::AbstractValue, thereby allowing types like Vec3. + // In addition, getDiscreteVariableAbstractValue() and + // updDiscreteVariableAbstractValue() accept the path of a discrete + // variable, not just its name. + + /** + * Based on a specified path, resolve the name of a discrete variable and + * the component that owns it (i.e., its parent). + * + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @param dvName Returned name of the discrete variable. This string is + * simply the last string in the specified pathName. + * @return Pointer to the Component that owns the discrete variable. + */ + const Component* resolveDiscreteVariableNameAndOwner( + const std::string& pathName, std::string& dvName) const; + + /** + * Retrieve a read-only reference to the abstract value of the discrete + * variable at a specified path. This method provides a more general + * interface that is not limited to values of type double. + * + * To obtain the type-specific value of a discrete variable, perform + * a cast using the template methods provided in class SimTK::Value. + * When the type is unknow, it can be querried using the + * SimTK::Value::isA() method. For example, + * + * ``` + * const SimTK::AbstractValue& valAbstract = + * getDiscreteVariableAbstractValue(state, pathName); + * + * if (SimTK::Value::isA(valAbstract)) { + * const SimTK::Value& valDbl = + * SimTK::Value::downcast(valAbstract); + * double x = valDbl + 0.4; + * + * } else if (SimTK::Value::isA(valAbstract)) { + * const SimTK::Value& valVec3 = + * SimTK::Value::downcast(valAbstract); + * Vec3 x = valDbl + Vec3(0.4); + * } + * ``` + * + * @param state State from which to get the value. + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @return Value of the discrete variable as a reference to an + * AbstractValue. + * @throws ComponentHasNoSystem if this Component has not been added to a + * System (i.e., if initSystem has not been called). + * @throws Exception if the discrete variable is not found. + */ const SimTK::AbstractValue& getDiscreteVariableAbstractValue( - const SimTK::State& state, const std::string& name) const; + const SimTK::State& state, const std::string& pathName) const; /** * Retrieve a writable reference to the abstract value of the discrete - * variable of a specified name. This method provides a more general - * interface to handle discrete variables that may not be type double. - * - * @param state the State from which to get the value - * @param name the name of the discrete state variable - * @return value the discrete variable value as a SimTK::AbstractValue + * variable at a specified path. This method provides a more general + * interface that is not limited to values of type double. + * + * To obtain the type-specific value of a discrete variable, perform + * a cast using the template methods provided in class SimTK::Value. + * When the type is unknow, it can be querried using the + * SimTK::Value::isA() method. For example, + * + * ``` + * SimTK::AbstractValue& valAbstract = + * updDiscreteVariableAbstractValue(state, pathName); + * + * if (SimTK::Value::isA(valAbstract)) { + * SimTK::Value& valDbl = + * SimTK::Value::updDowncast(valAbstract); + * valDbl = 0.4; + * + * } else if (SimTK::Value::isA(valAbstract)) { + * SimTK::Value& valVec3 = + * SimTK::Value::updDowncast(valAbstract); + * valDbl = Vec3(0.4); + * } + * ``` + * + * @param state State from which to get the value. + * @param pathName Specified path of the discrete variable in the Model + * heirarchy. + * @return Value of the discrete variable as a reference to an + * AbstractValue. * @throws ComponentHasNoSystem if this Component has not been added to a - * System (i.e., if initSystem has not been called) + * System (i.e., if initSystem has not been called). + * @throws Exception if the discrete variable is not found. */ SimTK::AbstractValue& updDiscreteVariableAbstractValue( SimTK::State& state, const std::string& name) const; + //-------------------------------------------------------------------------- + - /** - * %Set the value of a discrete variable allocated by this Component by name. - * - * @param state the State for which to set the value - * @param name the name of the discrete variable - * @param value the value to set - * @throws ComponentHasNoSystem if this Component has not been added to a - * System (i.e., if initSystem has not been called) - */ - void setDiscreteVariableValue(SimTK::State& state, const std::string& name, - double value) const; /** * %Set the value of a discrete variable allocated by this Component by @@ -2034,6 +2107,12 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); protected: class StateVariable; + + // F. C. Anderson (May 2023) + // Need declaration up front to support new methods for traversing the + // Component graph to Discrete Variables. + //struct DiscreteVariableInfo; + //template friend class ComponentSet; // Give the ComponentMeasure access to the realize() methods. template friend class ComponentMeasure; @@ -2748,6 +2827,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); */ const StateVariable* traverseToStateVariable( const ComponentPath& path) const; + + #endif /// @name Access to the owning component (advanced). @@ -3316,6 +3397,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); bool allocate{true}; }; + /** * A cache variable, as stored internally by Component. */ diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 61db99044b..b985fa6119 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -122,8 +122,8 @@ class ExponentialContactTester void test(); void testParameters(); void testSerialization(); - void testDiscreteVariable(State& state, const ExponentialContact& ec, - const string& name); + void printDiscreteVariableAbstractValue(const string& pathName, + const AbstractValue& value) const; void testDiscreteVariables(State& state, const ForceSet& fSet); // Simulation @@ -680,38 +680,28 @@ testSerialization() { //_____________________________________________________________________________ void ExponentialContactTester:: -testDiscreteVariable(State& state, const ExponentialContact& ec, - const string& name) +printDiscreteVariableAbstractValue(const string& pathName, + const AbstractValue& value) const { - // Get the starting value. - const SimTK::AbstractValue& val = - ec.getDiscreteVariableAbstractValue(state, name); + cout << pathName << " = "; - /* // Switch depending on the type - if (SimTK::Value::isA(val)) { - SimTK::Value deltaDouble(delta); - SimTK::Value tolDouble(tol); - - // + if (SimTK::Value::isA(value)) { + double x = SimTK::Value::downcast(value); + cout << x << endl; + } else if (SimTK::Value::isA(value)) { + Vec3 x = SimTK::Value::downcast(value); + cout << x << endl; } - - // Set a new value. - double valNew = val + delta; - ec.setDiscreteVariableValue(state, name, valNew); - double valNewDV = ec.getDiscreteVariableValue(state, name); - ASSERT_EQUAL(valNew, valNewDV, tol); - - // Restore the starting value. - ec.setDiscreteVariableValue(state, name, val); - double valDV = SimTK::Value::downcast( - ec.getDiscreteVariableAbstractValue(state, name)); - ASSERT_EQUAL(val, valDV, tol); - */ } //_____________________________________________________________________________ -// Only types that are handled are double and Vec3. +// The only types that are handled are double and Vec3 at this point. +// The significant changes in how Discrete Variables are handled are: +// 1. Values are now not assumed to be doubles but are AbstractValues. +// 2. Discrete variables outside of OpenSim are permitted. +// 3. Discrete variables may be accessed via the Component API by +// specifying the path (e.g., path = "/forceset/Exp0/anchor"). void ExponentialContactTester:: testDiscreteVariables(State& state, const ForceSet& fSet) { @@ -723,9 +713,10 @@ testDiscreteVariables(State& state, const ForceSet& fSet) { int n = names.size(); for (int i = 0; i < n; ++i) { - // Starting value - SimTK::AbstractValue& valAbstract = + // Print values for debugging purposes. + AbstractValue& valAbstract = fSet.updDiscreteVariableAbstractValue(state, names[i]); + printDiscreteVariableAbstractValue(names[i], valAbstract); // Declarations double tol = 1.0e-6; @@ -738,7 +729,7 @@ testDiscreteVariables(State& state, const ForceSet& fSet) { if (SimTK::Value::isA(valAbstract)) { SimTK::Value& valDbl = SimTK::Value::updDowncast(valAbstract); - valStartDbl = valDbl.get(); + valStartDbl = valDbl; valDbl = valStartDbl + deltaDbl; } else if (SimTK::Value::isA(valAbstract)) { SimTK::Value& valVec3 = @@ -746,6 +737,7 @@ testDiscreteVariables(State& state, const ForceSet& fSet) { valStartVec3 = valVec3.get(); valVec3 = valStartVec3 + deltaVec3; } + printDiscreteVariableAbstractValue(names[i], valAbstract); // Check that the value changed correctly if (SimTK::Value::isA(valAbstract)) { @@ -758,7 +750,7 @@ testDiscreteVariables(State& state, const ForceSet& fSet) { ASSERT_EQUAL(valVec3.get(), valStartVec3 + deltaVec3, tol); } - // Restore + // Restore the starting value if (SimTK::Value::isA(valAbstract)) { SimTK::Value& valDbl = SimTK::Value::updDowncast(valAbstract); @@ -768,8 +760,9 @@ testDiscreteVariables(State& state, const ForceSet& fSet) { SimTK::Value::updDowncast(valAbstract); valVec3 = valStartVec3; } + printDiscreteVariableAbstractValue(names[i], valAbstract); - // Check that the value was restored correctly + // Check that the value was correctly restored if (SimTK::Value::isA(valAbstract)) { SimTK::Value& valDbl = SimTK::Value::updDowncast(valAbstract); @@ -807,26 +800,11 @@ simulate() ExponentialContact::resetAnchorPoints(fSet, state); // Check the Component API for discrete states. - int i; int n = fSet.getSize(); - for (i = 0; i < n; ++i) { - try { - - // Get the ExponentialContact Component - //ExponentialContact& ec = - // dynamic_cast(fSet.get(i)); - //string path = ec.getAbsolutePathString(); - //cout << "path = " << path << endl; - - // Test DiscreteVariable Component API - testDiscreteVariables(state, fSet); - - } catch (const std::exception& e) { - // Nothing should happen here. Execution is just skipping any - // OpenSim::Force that is not an ExponentialContact. - cout << e.what() << endl; - } - + try { + testDiscreteVariables(state, fSet); + } catch (const std::exception& e) { + cout << e.what() << endl; } // Integrate From 9f2ee6d94cd518e8c7520462762af3c74e1568ea Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 9 May 2023 12:35:25 -0500 Subject: [PATCH 53/55] Component.h: fixed a few typos The typos were in Doxygen comments. --- OpenSim/Common/Component.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index e03ff1411b..59b89572a7 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -1429,7 +1429,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); * * To obtain the type-specific value of a discrete variable, perform * a cast using the template methods provided in class SimTK::Value. - * When the type is unknow, it can be querried using the + * When the type is unknown, it can be queried using the * SimTK::Value::isA() method. For example, * * ``` @@ -1467,7 +1467,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Component, Object); * * To obtain the type-specific value of a discrete variable, perform * a cast using the template methods provided in class SimTK::Value. - * When the type is unknow, it can be querried using the + * When the type is unknown, it can be queried using the * SimTK::Value::isA() method. For example, * * ``` From 19d99e7f6e8d888d5977975f50ca20b0f240b4ba Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Tue, 9 May 2023 12:37:45 -0500 Subject: [PATCH 54/55] testExponentialContact: output simulated states The states are output using an OpenSim::Storage file. --- OpenSim/Simulation/Test/testExponentialContact.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index b985fa6119..133ec42cf9 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -813,6 +813,7 @@ simulate() manager.setIntegratorAccuracy(integ_accuracy); state.setTime(0.0); manager.initialize(state); + manager.setWriteToStorage(true); std::clock_t startTime = std::clock(); state = manager.integrate(tf); auto runTime = 1.e3 * (std::clock() - startTime) / CLOCKS_PER_SEC; @@ -827,6 +828,10 @@ simulate() // Write the model to file //model->print("C:\\Users\\fcand\\Documents\\block.osim"); + + // Write recorded states to file + Storage& store = manager.getStateStorage(); + store.print("BouncingBlock.states"); } //_____________________________________________________________________________ From 3f8e4d4d4d2cfa5f2ff9ac77b0f83e3cf2893c7c Mon Sep 17 00:00:00 2001 From: Clay Anderson Date: Sun, 14 May 2023 20:22:09 -0500 Subject: [PATCH 55/55] testExponentialContact: added StatesTrajectoryReporter A vector of Simbody::State objects is saved during the simulation at a time interval of 0.001 sec. There is, however, no way to serialize the Discrete Variables. Only the Continuous States can be serialized via a TimeSeriesTable. --- .../Test/testExponentialContact.cpp | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Test/testExponentialContact.cpp b/OpenSim/Simulation/Test/testExponentialContact.cpp index 133ec42cf9..efcb863ea2 100644 --- a/OpenSim/Simulation/Test/testExponentialContact.cpp +++ b/OpenSim/Simulation/Test/testExponentialContact.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include "SimTKsimbody.h" @@ -157,12 +159,15 @@ class ExponentialContactTester Model* model{NULL}; OpenSim::Body* blockEC{NULL}; OpenSim::Body* blockHC{NULL}; - OpenSim::ExponentialContact* sprEC[n]{NULL}; - OpenSim::HuntCrossleyForce* sprHC[n]{NULL}; - OpenSim::ContactGeometry* geomHC[n]{NULL}; + OpenSim::ExponentialContact* sprEC[n]{nullptr}; + OpenSim::HuntCrossleyForce* sprHC[n]{nullptr}; + OpenSim::ContactGeometry* geomHC[n]{nullptr}; Storage fxData; - ExternalForce* fxEC{NULL}; - ExternalForce* fxHC{NULL}; + ExternalForce* fxEC{nullptr}; + ExternalForce* fxHC{nullptr}; + + // Reporters + StatesTrajectoryReporter* statesReporter{nullptr}; }; // End class ExponentialContactTester declarations @@ -346,6 +351,14 @@ buildModel() } } + // Reporters + // StatesTrajectory + statesReporter = new StatesTrajectoryReporter(); + statesReporter->setName("states_reporter"); + statesReporter->set_report_time_interval(0.001); + model->addComponent(statesReporter); + + // Visuals? if (showVisuals) { if (blockEC) { @@ -829,9 +842,12 @@ simulate() // Write the model to file //model->print("C:\\Users\\fcand\\Documents\\block.osim"); - // Write recorded states to file + // Write recorded states + // From the Storage object maintained by the Manager Storage& store = manager.getStateStorage(); store.print("BouncingBlock.states"); + // From the StatesTrajectoryReporter + const StatesTrajectory& statesTrajectory = statesReporter->getStates(); } //_____________________________________________________________________________