get angular accelerations of mobilized bodies in computeControls method (C++)

[Dídac Coll]

Hello,

I was developing my own controller in order to control a joint of a pendulum. I've created class that derives from the Controller class. The thing is that I'm interested in obtain angular acceleration from the "Mobilizedbodies" in the method "void computeControls()", but when I call getBodyAngularAcceleration, an error occours:
terminate called after throwing an instance of 'SimTK::Exception::CacheEntryOutOfDate'
what(): SimTK Exception thrown at StateImpl.h:1678:
State Cache entry was out of date at Stage Dynamics. This entry depends on version 1 of Stage Dynamics but was last updated at version 0.

In order to try to solve this, I've tried calling _model->updMultibodySystem().realize(s,SimTK::Stage::Acceleration) before calling the method that give the accelerations but it doesn't work.

https://www.dropbox.com/s/55sc6i8a5qv1q ... s.png?dl=0

Any idea how to solve this?

Thanks

[Kenechukwu Mbanisi]

Hello Didac,

I've had to design a controller in the past and here's how I would approach this problem.

The equation of motion for a pendulum is:
appliedTorque = inertiaMatrix*angAcc + gravityTerm

In computeControls, you want to compute the controls value for your actuator (I'd use a Coordinate Actuator in this case) and add it to your actuator as you have done.

A simple gravity compensation controller would have appliedTorque = gravityTerm, hence inertiaMatrix*angAcc = 0.

One other thing you might want to do is to scale the controls before adding it to the actuator. That is:
controls = appliedTorque/optimalForce for a Coordinate Actuator.

Here's code for what I've described above:

Code: Select all

Vector pendulumController::gravityTerm(const State& s) const {
	Vector g;
	_model->getMatterSubsystem().multiplyBySystemJacobianTranspose(s, _model->getGravityForce().getBodyForces(s), g);
	return g;
}

// In computeControls: 

	Vector gravity = gravityTerm(s);
	Vector controlTorque = gravity;
	const Actuator& act = _model->getActuators().get("joint_actuator")
	double FOpt = act.getOptimalForce();
	double control_1 = controlTorque.get(0) / FOpt;

Something along these lines should work for a simple gravity compensation controller. You probably want to set initial coordinate speedValue to zero for this to work.

I hope this helps.

Kenechukwu

[Dimitar Stanev]

Realizing to acceleration in your controller causes an infinite loop, because all forces need to be known for you to compute the acceleration. To avoid this problem you need to keep two versions of your model and state. The one will be used for your actual simulation and the other for computing the acceleration (without a controller). It is a bit tricky though.

[Christopher Dembia]

Dimitar is correct. Alternatively, you can use a delay so that you are using the acceleration from a few milliseconds earlier rather than the current acceleration. Look at Simbody's Measure::Delay.

[Dídac Coll]

Right Know I'm working with a double pendulum. Thus, the Kenechukwu Mbanisi's approximation is useful.
Thanks Dimitar Stanev and Christopher Dembia. I will take into account your the advises.