Measuring Contact Force Vectors in Forward Simulation

[Kenechukwu Mbanisi]

Hello there,

I am interested in obtaining forces generated between contact interfaces in simulation.

I have prepared a simple model for this; it's a two link arm which makes contact with a wall. Both the arm and the wall have contact interfaces modeled as ElasticFoundationForce.

What I want to do is to be able to measure the contact forces generated during simulation (at every integration step) when the arm pushes against the wall. Can anyone help me with how to go about this?

I have attached the simple model here.

Thank you.

Kenechukwu

[Ayman Habib]

Hello,

If using the OpenSim application, you can add a ForceReporter (Analysis) while running the (forward simulation) tool and that will produce a file with contact forces computed during the run.

Hope this helps,
-Ayman

[Hide Kimpara]

Hello Ayman,

Yes, we use ForceReporter for post-processing. However, through discussion on the forum (viewtopicPhpbb.php?f=91&t=8162&p=0), we found a more convenient method using (https://simtk.org/api_docs/simbody/3.5/ ... ystem.html) following the example (https://github.com/mitkof6/simbody/blob ... ground.cpp)

The challenge we faced was that we were unable to get access to the compliant contact subsystem in the model to obtain the contact forces using this function:

Code: Select all

const ContactForce& force = m_contactForces.getContactForce(state, i)

When we made the model programmatically on SimBody directly, we could obtain force data following the ExampleContactPlayground.cpp example. But we could not get force data using already defined .osim models.

Also we hope the method would be available from MATLAB in 4.0 directly.

Thank you,
Hide

[Carmichael Ong]

I'm not sure if you need to get the contact subsystem directly, depending on what information you are trying to extract. ElasticFoundationForce implements getRecordValues() which should give an Array of the force and torque values, and getRecordLabels() will give you an Array of the same size that tells you which values correspond to the components of the force and torque. So, as long as you know which index your ElasticFoundationForce is, you can grab it from the model's ForceSet.

[Kenechukwu Mbanisi]

Hello Carmichael,

Thanks for the suggestion. I tried that out with my model, but I am getting an access violation error (Access violation reading location 0x0000000000001030).

Here's how I set up my code:

Code: Select all

ForceSet forces = _model->getForceSet();
OpenSim::Force& contact = forces.get(3);  // the elasticFoundationForce is in index 3
Array<std::string> Fn;
Array<double> F;
Fn = contact.getRecordLabels(s);
F = contact.getRecordValues(s);

Any quick pointers on how to troubleshoot this?

Thanks.

[Ayman Habib]

Hi Kenechukwu,

Many things can go wrong here that are hard to tell from this code snippet, for example How did you get the state? this state needs to be realized to a specific Stage before you can ask for Forces. Ideally you build the code debuggable (in C++) and you can get a clearer picture of errors/exceptions happen at which lines.

Let us know how that goes,
-Ayman

[Kenechukwu Mbanisi]

Hello,

Thanks for your response. I am still experiencing the challenge. I realized the state to Dynamic (and also Acceleration) stage and the result is still same. It fails at the "F = contact.getRecordValues(s)" line and the error is "Access violation reading location 0x0000000000001030"

I added the code here so you can get a better idea of how my code flows. I am calculating the force in a controller class and I intend to use this force value in computing my controls (not yet in the code).

Code: Select all

class PDController2 : public Controller
{
	OpenSim_DECLARE_CONCRETE_OBJECT(PDController2, Controller);

private:
	double kp;
	double kv;

public:

	PDController2::PDController2() : Controller() {
		setKp(100);
		setKv(20);
	}

	Array<double> PDController2::ForceValue(const State& s) const {
		ForceSet forces = _model->getForceSet();
		OpenSim::Force& contact = forces.get(3);

		Array<double> F;
		_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics);
		F = contact.getRecordValues(s);
		return F;
	};
	
	//_____________________________________________________________________________
	void PDController2::computeControls(const SimTK::State& s, SimTK::Vector &controls) const
	{
		double t = s.getTime();

		Array<double> contactForce = ForceValue(s);
		std::cout << "print force value:  " << contactForce << std::endl;
		
		double control_1 =0.5;
		double control_2 = 0.5;

		Vec2 control(control_1, control_2);

		Vector control1(1, control[0]);
		Vector control2(1, control[1]);

		j1_act.addInControls(control1, controls);
		j2_act.addInControls(control2, controls);

	}
};

int main()
{
	try {
		// Create an OpenSim model and set its name
		Model osimModel("TwoLinkArmModel.osim");
		osimModel.setUseVisualizer(true);
		
		PDController2 *controller = new PDController2();
		controller->setName("my_controller");
		controller->setActuators(osimModel.updActuators());
		osimModel.addController(controller);

		State& s = osimModel.initSystem();

		std::cout << "System Initialized" << std::endl;

		std::cout << "getMultibodySystem Done" << std::endl;

		SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
		integrator.setAccuracy(1.0e-3);
		Manager manager(osimModel, integrator);

		// Define the initial and final simulation times
		double initialTime = 0.0;
		double finalTime = 1.0;

		osimModel.updMatterSubsystem().setShowDefaultGeometry(true);
		Visualizer& viz = osimModel.updVisualizer().updSimbodyVisualizer();

		// Integrate from initial time to final time
		manager.setInitialTime(initialTime);
		manager.setFinalTime(finalTime);
		std::cout << "\n\nIntegrating from " << initialTime << " to " << finalTime << std::endl;
		manager.integrate(s);

	}
	catch (OpenSim::Exception ex)
	{
		std::cout << ex.getMessage() << std::endl;
		return 1;
	}
	catch (std::exception ex)
	{
		std::cout << ex.what() << std::endl;
		return 1;
	}
	catch (...)
	{
		std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
		return 1;
	}

	std::cout << "OpenSim example completed successfully.\n";
	std::cin.get();
	return 0;
}

Please share thoughts on this.

Kenechukwu

[Thomas Uchida]

I don't know whether you're using 3.3 or 4.0, but it looks like your PDController2 class derives from OpenSim's Controller class and I don't see setKp() or setKv() methods in the Controller class in either OpenSim version (e.g., http://myosin.sourceforge.net/1987/clas ... oller.html) and these methods aren't implemented in your code. Where are the methods that are called in the constructor? I seem to be missing something here.

[Ayman Habib]

Hello,

Since you're building 4.0 from source, you can step into this code/line (contact.getRecordValues(s)) to see where the exception is thrown. Possible candidates of the top of my head:
- The variable "contact" is null or does not refer to a Force (you can call contact.getConcreteClassName() to see what kind of object it is, if it's a valid object to start)
- Not sure if the configuration specified by state has the ContactForce engaged/in-contact you need to step into the call to getRecordValues and maybe follow it to the Simbody side.

Hope this helps,
-Ayman

[Kenechukwu Mbanisi]

Hello,

I don't know whether you're using 3.3 or 4.0, but it looks like your PDController2 class derives from OpenSim's Controller class and I don't see setKp() or setKv() methods in the Controller class in either OpenSim version (e.g., http://myosin.sourceforge.net/1987/clas ... oller.html) and these methods aren't implemented in your code.

Tom, you're right, I took out the setKp() and setKv() methods in the Controller class to make the code less lengthy. I'm not sure that's a cause of the problem at the moment. I am using OpenSim 3.3. I haven't been able to install 4.0 on my PC and I don't want to spend the entire workshop doing that.

Ayman, I was unable to step into the line, once I get to that line, it throws the exception (I'm using RelWithDebInfo).

The variable "contact" is null or does not refer to a Force (you can call contact.getConcreteClassName() to see what kind of object it is, if it's a valid object to start)

I called contact.getConcreteClassName() and it returned "ElasticFoundationForce" which I think is the right thing.

I have uploaded my code and model, if you would like to have a look. This is a primary goal we hope to achieve in this workshop.

Thank you very much for your help.
Kenechukwu