[WIP] Exponential Contact

[fcanderson]

@jenhicks @aymanhab @carmichaelong @nickbianco @aseth1 @adamkewley @tkuchida

Apologies in advance; this is a LONG writeup. I thought it best, though, to take the time to get the big-picture stuff written down. The info builds toward some questions that I formulate toward the end.

This PR contains a first draft of class ExponentialContact. 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.

The underlying mechanics are implemented natively in Simbody. In Simbody, the code resides in 5 files: ExponentialSpringForce.h and .cpp, ExponentialSpringForceImpl.cpp, and ExponentialSpringParameters.h and .cpp. This code is accompanied in the Simbody build system by a utility for visualizing and evaluating performance ([Test_Adhoc] ExperimentalSpringsComparison.cpp), an extensive unit test ([Test_Regr] testExponentialSpringForce.cpp), and Doxygen-compliant comments in .h files. The code was reviewed by Michael Sherman.

OpenSim::ExponentialContact is essentially a wrapper class of SimTK::ExponentialSpringForce. The name change between OpenSim and SimTK was made to better assist potential users in identifying the purpose of the class (e.g., it's a contact class with friction, not a simple spring actuator) and to reduce confusion between the two classes.

To write class ExponentialContact, I basically mimicked class OpenSim::HuntCrossleyForce.

IMPORTANT: ExponentialContact relies on Simbody d685ed2 (PR #746) or later.
PR #746 delivered some performance and API enhancements to class ExponentialSpringForce. It was merged into Simbody on Oct. 26, 2022.  To compile this PR, you'll need to update to d685ed2 or later.

This PR is tagged as a work-in-progress [WIP] because some aspects of ExponentialContact don't fully function as would be expected for an OpenSim::Component.

In the remainder of this PR, I'll go over an Inventory of Assets,  What's Working, What's Not Working, and Questions / Issues.

---

Inventory of Assets

1. Class ExponentialContact
   Class ExponentialContact is the wrapper class for SimTK::ExponentialSpringForce.  It is implemented in two files in OpenSim-Core: ExponentialContact.h and ExponentialContact.cpp. It encapsulates subclass ExponentialContact::Parameters to handle non-default parameter choices for quantities such as stiffness, viscosity, coefficients of friction, etc., and interface with the underlying SimTK::ExponentialSpringParameters class. Fairly detailed Doxygen-compliant comments are in place in file ExponentialContact.h.  An introduction explains how the class works, and each method is accompanied by a description and Doxygen-style argument list using the @param keyword.

2. Test Utility (testExponentialContact.exe)
   A command-line utility is available (see testExponentialContact.cpp) to evaluate the performance of class ExponentialContact relative to class HuntCrossleyForce for a bouncing 10-kg, 6-dof block. The utility takes a set of command arguments that can be used to select a) between pre-set initial conditions, b) which contact model is used (both at the same time is possible), c) whether or not damping is present, d) if a ramping external force is applied, and e) whether or not visuals are shown. Upon completion, a summary of modeling choices, integrator settings, and cpu times are printed:

$ testExponetialContact [InitCond] [Contact] [NoDamp] [Fx] [Vis]
        InitCond (choose one): Static Bounce Slide Spin SpinSlide SpinTop Tumble
         Contact (choose one): Exp Hunt Both

3. Additional Testing Code
   A routine (void testExponentialContact()) was also added to testForces.cpp.  This routine does very much the same thing as rountines testElasticFouncation() and testHuntCrossleyForce().

---

What's Working

Some basics of class ExponentialContact are working in OpenSim.  Here's a partial list that hits the high points:

• An ExponentialContact instance can be assembled and used within an OpenSim model by specifying the name of the body on which the instance acts, along with a few other required constructor arguments.
• Contact simulations run at computational speeds in OpenSim that are very similar to speeds achieved in a native Simbody implementation. Roughly, for common parameter choices, ExponentialContact runs anywhere from 1x to 10x faster than HuntCrossleyForce, depending on the circumstances (e.g., sliding or static).
• ExponentialContact instances are serialized and deserialized (as XML) via Properties.
• Non-default parameters (mus, muk, elasticity, viscosity, etc.) can be specified by the user via the API or by modifying OpenSim XML files.
• Many quantities (i.e., data cache entries) are available via accessor methods of class ExponentialContact when realization stages are computed to appropriate levels. The realization stage required by each accessor method is noted in the documentation. Examples of available quantities include normal force, friction force, damping part of the normal force, elastic part of the normal force, total contact force, instantaneous coefficient of friction, elastic anchor point, etc.
• Detailed output for ExponentialContact instances may be obtained via a ForceReporter.  Side Note - I don't think I filled out the storage with the expected entries. I did not, for example, convert the applied forces into generalized body force.

---

What's Not Working

Although I have done a fair amount of reading, I don't yet have a clear picture of the essential functionality required by an OpenSim::Component. I know enough, however, to realize that class ExponentialContact falls short of satisfying important functionality in the following categories.

States.

The underlying Simbody Subsystem, ExponentialSpringForce, has 4 state variables.

There are 2 Discrete States:  1) Static Coefficient of Friction (MUS) [Real] and 2) Kinetic Coefficient of Friction (MUK) [Real]. These 2 states, as discrete variables, can be changed discontinuously during a simulation without invalidating the System Topology.  They are intended to enable the user to simulate a slippery spot on the floor, for example.

There are 2 Auto Update Discrete States: 1) Elastic Anchor Point (p₀) [Vec3] and 2) Sliding (K) [Real]. From an initial value, the values of these 2 states evolve over the course of a simulation. Because they cannot be computed uniquely from the other states of the System, they cannot be treated simply as data cache entries.

Currently, none of these states are exported to OpenSim. They are hidden beneath the covers.

Data Cache Entries

Similarly, the private implementation of ExponentialSpringForce (i.e., ExponentialSpringForceImpl) possesses an extensive data cache, which is listed below.

struct ExponentialSpringData {
    struct Pos {
        // Position of the body station in the ground frame.
        Vec3 p_G{NaN};
        // Position of the body station in the frame of the contact plane.
        Vec3 p_P{NaN};
        // Displacement of the body station normal to the floor expressed in
        // the frame of the contact plane.
        Real pz{NaN};
        // Position of the body station projected onto the contact plane
        // expressed in the frame of the contact plane.
        Vec3 pxy{NaN};
    };
    struct Vel {
        // Velocity of the body station in the ground frame.
        Vec3 v_G{NaN};
        // Velocity of the body station in the contact plane frame.
        Vec3 v_P{NaN};
        // Velocity of the body station normal to the contact plane expressed
        // in the contact plane frame.
        Real vz{NaN};
        // Velocity of the body station in the contact plane expressed in
        // the contact plane frame.
        Vec3 vxy{NaN};
    };
    struct Dyn {
        // Note that variables fzElas, fzDamp, and fz below are scalars.
        // They are normal components of the contact force when the contact
        // force is expressed in the frame of the contact plane.
        // Elastic force in the normal direction.
        Real fzElas{NaN};
        // Damping force in the normal direction.
        Real fzDamp{NaN};
        // Total normal force.
        Real fz{NaN};
        // Instantaneous coefficient of friction.
        Real mu{NaN};
        // Limit of the friction force.
        Real fxyLimit{NaN};
        // Flag indicating if the friction limit was exceeded.
        bool limitReached{false};
        // Damping part of the friction force in Model 1.
        Vec3 fricDampMod1_P{NaN};
        // Total friction force in Model 1.
        Vec3 fricMod1_P{NaN};
        // Elastic part of the friction spring force in Model 2.
        Vec3 fricElasMod2_P{NaN};
        // Damping part of the friction spring force in Model 2.
        Vec3 fricDampMod2_P{NaN};
        // Total friction spring force in Model 2.
        Vec3 fricMod2_P{NaN};
        // Elastic friction force after blending.
        Vec3 fricElas_P{NaN};
        // Damping friction force after blending.
        Vec3 fricDamp_P{NaN};
        // Total friction force after blending.
        Vec3 fric_P{NaN};
        // Resultant force (normal + friction) expressed in the frame of the
        // contact frame. This is the force that will be applied to the body
        // after expressing it in the appropriate frame.
        Vec3 f_P{NaN};
        // Resultant force (normal + friction) expressed in the Ground frame.
        // This is the force applied to the body.
        Vec3 f_G{NaN};
    };
};

Data cache indices are acquired in ExponentialSpringForceImpl, but not for each individual quantity. Rather, just one index for each struct at the separate realization levels listed above (Pos, Vel, and Dyn).

Most of the above quantities are accessible via standard accessor methods in class ExponentialSpringForce on the Simbody side.  A subset of these are also currently available in OpenSim via an identical pass-through API.

However, just as for the states, OpenSim does not know that these quantities are data cache entries. Therefore, in the present version of ExponentialContact, it is not possible to access these entries via the Data Cache API provided by class Component. 

Inputs, Outputs, & Sockets

Class ExponentialContact does not currently define any inputs, outputs, or sockets.  So, the class is not plug-n-play.  It would be cool to get that working though!

---

Questions / Issues

1. From the items described in the above section, What's Not Working, what things should I prioritize? Are there some "must have" items in the list that need to work?
2. It certainly looks possible to expose the discrete variables MUS and MUK (see above) in OpenSim.  As they have already been allocated as part of the Simbody System, however, it seems that I will need to do something other than use Component::addDiscreteVariable(). Is there a way to generate the proper map entries on the OpenSim side without allocating a new discrete state variable?  Can I just manually add the proper map entries? If so, is the order important?
3. Issue? I haven't yet crossed any part of the OpenSim code that mentions Auto Update Discrete States.  They do exist in Simbody as formal creatures. Do I need to expose the Auto Update Discrete Variables p₀ and Sliding in OpenSim?  They are members of the underlying Simbody State. If there are times when OpenSim serializes or deserializes the OpenSim State and then pushes that State to the Simbody State, there might be issues.
4. Issue?  It looks like OpenSim assumes that Discrete Variables are type double. This is not a requirement in Simbody. In fact, the elastic anchor point (p₀ ) which is an Auto Update Discrete State is type Vec3. Does OpenSim's class infrastructure permit such a discrete variable to be exposed on the OpenSim side?
5. Should I hook up the underlying data cache structs to OpenSim so that users can use the Component API for accessing data cache information? Since the data cache entries have already been allocated on the Simbody side in the private implementation, I would need to add fake map entries and redirect queries to the correct accessor method.
6. Should I worry about inputs, outputs, and/or sockets?

---

Thanks!

Thank you to those of you who managed to make your way through the post. I realize you are all quite busy with many things.

I look forward to hearing back from any of you in a position to lend some expertise. A little guidance, particularly when it comes to class OpenSim::Component, will go a long way.

-Clay

---

This change is 

[fcanderson]

I should add some clarifying notes...

In commit 437feb7, I added a few accessor (get/set) methods to class Component to handle Discrete Variables (DVs) that are not type double. These accessors address point 3 in my previous comment.

The original accessor methods are:

double getDiscreteVariableValue(const SimTK::State& s, const string& name) const;
void setDiscreteVariableValue(SimTK::State& s, const string& name, double value) const;

The added accessor methods are:

const AbstractValue& getDiscreteVariableAbstractValue(const SimTK::State& s, const string& name) const;
AbstractValue& Component::updDiscreteVariableAbstractValue(SimTK::State& s, const string& name) const

In Simbody, DVs are handled using SimTK::AbstractValue. So, for example, a call to

SimTK::Subsystem::getDiscreteVariable(state, index);

returns an AbstractValue, which the caller then should cast to the appropriate concrete type. In the case of a double, for example, one would do the following:

double value = SimTK::Value<double>::downcast( subsystem->getDiscreteVariable(state, index) );

The added accessor methods allow OpenSim to interface with DVs that are not type double via the Component API. Note that because the signatures of the original accessor methods haven't changed, no changes are needed elsewhere in OpenSim. That is, other classes can keep interacting with Components as usual, provided the DVs are type double.

[nickbianco]

@fcanderson, in Moco, we use a class we created called DiscreteController, which allows us to store the values of the controls in the model as discrete variables (so we can carry them around with the state). Something like this for your class could make sense here for serializing/deserializing.

I also like @aseth1's idea to update StatesTrajectory to handle discrete variables directory. Specifically, I think we'd need to update some of the utilities (e.g., createFromStatesTable) that take a trajectory of discrete variables (and controls, states, etc) and create a StatesTrajectory from it.

[fcanderson]

@nickbianco, thanks for sending the .h file for the DiscreteController. It's nice that the interface is so clean. I look forward to brainstorming with you and identifying the most promising/productive way forward. Thanks again for the time you are spending on this!

[fcanderson]

@nickbianco, just a quick update.

I've implemented the capability to access (get and set) a Discrete Variable by specifying its absolute component path-- a step toward being able to serialize/deserialize all discrete variables in a model.

I will clean up the code and enhance the testing this weekend, and then commit my additions early next week. That's the plan anyway.

Also, just wanted to note that the reason for the check failures on GitHub is that some changes I made to the Simbody contact model (SimTK::ExponentialSpringForce) haven't been merged into the Simbody master yet. Before requesting a merge from Michael Sherman, I figured I would wait until everything is functioning as desired on the OpenSim side and I'm relatively confident that no more tweaks to SimTK::ExponentialSpringForce are needed.

If you want to run my latest code, you'll have to check out and build my Simbody branch: https://github.com/fcanderson/simbody/tree/master

[fcanderson]

@nickbianco The cleanup and testing went faster than expected. That doesn't happen very often for me. I have committed my latest round of changes. A discrete variable can now be accessed via the OpenSim::Component API by specifying the path of the discrete variable. That is, as necessary, the getter and setter code traverses the Component tree to locate the discrete variable.

I will next take a look at the OpenSim Storage, Table, and StateTrajectory classes to see how discrete variables might be efficiently and reproducibly serialized.

[nickbianco]

A couple of typos here: unknow --> unknown and querried -- > queried.

[nickbianco]

Same typos here.

[nickbianco]

@fcanderson the new changes are looking good! I did a quick pass over everything (and left some quick comments for some typos), but I'll leave a full review once everything is finalized.

For serialization and deserialization, you'll want to look at StatesTrajectory::createFromStatesTable and StatesTrajectory::exportToTable. These methods utilize Model::setStateVariableValue and Model::getStateVariableValue to populate the States or the rows of TimeSeriesTable. I think you could use your new accessors to update these methods to set and get the discrete variables as well.

[fcanderson]

@nickbianco thanks for the review. I'll get those typos fixed.

I have been looking at StatesTrajectory. Using either class TimeSeriesTable or even class Storage seem viable, but not ideal. The primary reason being the possibility of Discrete States being different types (Real, Vec3, Rotation, Quaternion, etc) and DiscreteVariables being co-mingled with StateVariables.

What looks best to me is following through on the proposed .OSTATES file format. See the comments at very end of StatesTrajectory.h. Using XML as a framework for output/input offers a great deal of flexibility. And, I have in mind an XML format that would make serialization and, in particular, de-serialization pretty efficient. Specifically, it would minimize the number of times a Component would need to be found using a string-based path traversal. In addition, the XML format I have in mind would be insensitive to the order in which States appeared in the file.

What are your thoughts on proceeding with this approach?

I would essentially need to implement something like the following methods:

- OStatesDoc* StatesTrajectory::exportToOStatesDocument(const Model& model);
- static StatesTrajectory StatesTrajectory::createFromOStatesDocument(const Model& model, const OStatesDoc& doc);
- void Component::setContinuousState(SimTK::Vector<SimTK::AbstractValue> &values, std::String& pathName);
- void Component::setDiscreteState((SimTK::Vector<SimTK::AbstractValue> &values, std::String& pathName);
- void Component::setModelingOption(???);   // I need to investigate this further to know what the arguments would be.

I would also need to write an OStatesDoc class, which would encapsulate the XML DOM object and handle things like setting/getting the appropriate XML attributes as well as the data held in the XML child nodes (i.e., the individual trajectories of the SimTK states).

For the XML Document, I'm thinking something like this...

<?xml version="1.0" encoding="UTF-8" ?>
<OStatesDoc Version="40000">
	<Model name="BouncingBlock">
		<time type="double" num="5">
			0.000000000000000
			0.100000000000000
			0.200000000000000
			0.300000000000000
			0.400000000000000
		</time>
		<continuous>
			<state path="/jointset/freeEC/freeEC_coord_0/value" type="double" num="5">
				0.000000000000000
				0.100000000000000
				0.200000000000000
				0.300000000000000
				0.400000000000000
			</state>
			<state path="/jointset/freeEC/freeEC_coord_0/speed" type="double" num="5">
				1.000000000000000
				1.000000000000000
				1.000000000000000
				1.000000000000000
				1.000000000000000
			</state>
			...
		</continuous>
		<discrete>
			<state path="/forcset/EC0/mu_kinetic" type="double" num="5">
				0.500000000000000
				0.500000000000000
				0.500000000000000
				0.500000000000000
				0.500000000000000
			</state>
			<state path="/forcset/EC0/mu_static" type="double" num="5">
				0.700000000000000
				0.700000000000000
				0.700000000000000
				0.700000000000000
				0.700000000000000
			</state>
			<state path="/forcset/EC0/anchor" type="Vec3" num="5">
				<Vec3> 2.000000000000000 1.000000000000000 0.000000000000000 </Vec3>
				<Vec3> 2.100000000000000 1.000000000000000 0.000000000000000 </Vec3>
				<Vec3> 2.200000000000000 1.000000000000000 0.000000000000000 </Vec3>
				<Vec3> 2.300000000000000 1.000000000000000 0.000000000000000 </Vec3>
				<Vec3> 2.400000000000000 1.000000000000000 0.000000000000000 </Vec3>
			</state>
			...
		</discrete>
                <modeling>
                        ???
                </modeling>
	</Model>
</OStatesDoc>

[nickbianco]

@fcanderson, I think this could be a reasonable approach! At first, I was worrying about compatibilty with other classes/functions that need TimeSeriesTables (e.g., the GUI), but you could always convert your trajectory back to a StatesTrajectory and export the continuous states to a TimeSeriesTable whenever needed.

For larger trajectories, do you find this file structure to be unwieldy at all? How about the file size?

[fcanderson]

@nickbianco Excellent point about compatibility with other classes. But great solution! It is nice that we will be able to go back and forth between the different formats via a StatesTrajectory. Keep in mind, however, that while a StatesTrajectory object will have all states (Continuous and Discrete), there will likely be issues encountered with Storage objects and TimeSeriesTable objects containing DiscreteVariables. For example, TimeSeriesTable looks like it assumes that all data are of the same type, which is in large part the motivation for creating an OStatesDoc class where type can vary.

I am not too worried about file size. The tags will represent a very small portion of the characters that appear in a file. The possible exception being the tags associated with data types like Vec3. The most common data type will double, and I envision not having a tag for doubles. In addition, the tags for types like Vec3s are not necessary. They are just there to make it easier to read should someone open the file in an editor. Removing type tags is a call that can be made if the file sizes become unwieldy. Most of the file size will come from the number of decimal places chosen for the data. For reproducing the states, it seems we should write numbers out in full precision.

I will proceed with this plan! It strikes me as the best way forward. Everything seems to line up.

[fcanderson]

ps - I am going to branch my current OpenSim fork to add in the OStatesDoc functionality, just to keep things a bit more compartmentalized.

[nickbianco]

@fcanderson I just realized that StatesTrajectory supports its own serialization to XML via the .ostates filetype:

opensim-core/OpenSim/Simulation/StatesTrajectory.h

Line 513 in ffce474

* ### File format

.

Have you tried creating a StatesTrajectory (including your discrete states) and calling .print()?

[fcanderson]

@nickbianco Ooooo. Ok. Since there were still "TODO" items at the end of StatesTrajectory.h I figured that the .ostates format hadn't been implemented yet. I took the statements to which you directed me to be a roadmap for future implementation, not something that had already been implemented.

I haven't spent any time programming for the last two days, so no time lost. I will try what you suggest to see what's there before I implement anything. Thanks for the msg!

[fcanderson]

@nickbianco It looks like the .ostates format has not been implemented yet. I added a StatesTrajectoryReporter to my ExponentialContact simulation and got the resulting StatesTrajectory, but I don't see a way to output the StatesTrajectory other than via StatesTrajectory::exportToTable(). I tried doing a .print(), but no method found. I also searched the OpenSim solution space and didn't find anything having to do with "ostates" except in the StatesTrajectory.h.

I am going to proceed with the OStatesDoc class. If there actually is a .print() method (or something else), by all means let me know!

[nickbianco]

@fcanderson, I clearly read through the header too quickly and didn't notice the TODO above the .ostates file description! I had also thought StatesTrajectory inherited from Object which provides serialization/deserialization features. Speaking of, you might want to check those methods out before rewriting XML serialization from scratch. Perhaps it even makes sense to change StatesTrajectory to derive from Object, but I haven't thought that all the way through.

[fcanderson]

@nickbianco I will definitely consult class Object as I implement the OStatesDoc class. My inclination is to keep the serialization formats for an Object and OStatesDoc separate, although there might be considerable borrowing from what Object does. Since the .ostates files will be somewhat large, speed will be a priority. Best to keep the OStatesDoc class as lightweight as possible I think. In addition, it might be wise to allow some flexibility for the format to change and optimize as we become more familiar with the what we want from a .ostates file.