A problem with forward Dynamics (API)

[Sina Porsa]

Hi all;
I have modified the TugOfWar example in this code and I have a problem with that.
This code (attached to the end of this post) accepts a 2D array as an input and extracts muscle excitation patterns from this array to perform a forward dynamic (FD) analysis.

The problem is that it only can perform ONE FD analysis and as soon as I want to perform the second analysis, I receive this error message:
An unhandled win32 exception occured in CalcCost.exe[4252].

e.g. if I delete the last two lines of the main function, I will not have any errors. But as soon as I add the last two lines, it ends up in the mentioned error.
Thanks for your help.
I also understood that if I delete this line from the code, the problem will be solved:

Code: Select all

//Add the controller
osimModel.addController(muscleController);

Is there any other method to add the controller to the model?
Thanks.
Sina.

Code: Select all

//2 Muscles Optimization
#include <OpenSim/OpenSim.h>
#include <ctime>    // for clock()

using namespace OpenSim;
using namespace SimTK;

const int numOfNodes = 5; // Number of excitation nodes per muscle.
const double initialTime = 0.0; // initial simulation time.
const double finalTime = 3.00; //Final simulation time.
const double desiredZ = 0.04;
double timeVec[numOfNodes];
double initState[2][2] = {{ 0.0 , 0.09911815 },{ 0.0 , 0.09911815 }};

Model osimModel ("tugOfWar_model.osim");
PrescribedController *muscleController= new PrescribedController();

double calcCost( double excitation[][numOfNodes]);
int main (){
		srand ( time(NULL) ); 
		std::clock_t TIC1 = std::clock(); 

		muscleController->setActuators(osimModel.updActuators()); 

		for (int i=0; i<numOfNodes; i++){
			timeVec[i] = i*( (finalTime-initialTime)/(numOfNodes-1) ); 
		}
		double excitation[2][numOfNodes] = {{1,1,1,0,0},{0,1,1,0,1}}; //Excitation pattern of the muscles

		//////////////////////////////////
		// Initialize the muscle states //
		//////////////////////////////////
		const Set<Actuator>& actuatorSet = osimModel.getActuators(); 
		ActivationFiberLengthMuscle* muscle1 = dynamic_cast<ActivationFiberLengthMuscle*>( &actuatorSet.get(0) ); 
		ActivationFiberLengthMuscle* muscle2 = dynamic_cast<ActivationFiberLengthMuscle*>( &actuatorSet.get(1) ); 
		muscle1->setDefaultActivation (initState[0][0]); // muscle1 activation 
		muscle1->setDefaultFiberLength(initState[0][1]); // muscle1 fiber length 
		muscle2->setDefaultActivation (initState[1][0]); // muscle2 activation 
		muscle2->setDefaultFiberLength(initState[1][1]); // muscle2 fiber length 

		double myCost; 
		myCost= calcCost( excitation ); 

		double excitationNew[2][numOfNodes] = {{1,0,1,0,0},{1,1,0,0,1}};
		double dummy1 = calcCost( excitationNew ); //Line 19

		return 0; //Line 25
}
double calcCost( double excitation[][numOfNodes]){
	double cost = 0.0; // Initial condition is equal to zero
	try {
		double muscle1ExcVec[numOfNodes];
		double muscle2ExcVec[numOfNodes];
		std::cout << "Input:\n";
		for (int n=0; n<numOfNodes; n++){
			muscle1ExcVec[n] = excitation[0][n];
			muscle2ExcVec[n] = excitation[1][n];
			std::cout << muscle1ExcVec[n] << " " << muscle2ExcVec[n] << "\n";
		}	
		// Set the indiviudal muscle control functions for the prescribed muscle controller
		muscleController->prescribeControlForActuator("muscle1", new PiecewiseLinearFunction(numOfNodes, timeVec, muscle1ExcVec));
		muscleController->prescribeControlForActuator("muscle2", new PiecewiseLinearFunction(numOfNodes, timeVec, muscle2ExcVec));

		//Add the controller
		osimModel.addController(muscleController);

		// Initialize the system and get the default state
		SimTK::State& system = osimModel.initSystem();

		// Create the integrator
		SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
		integrator.setAccuracy(1.0e-4);
		
		// Create the manager
		Manager manager(osimModel,  integrator);

		// Integrate from initial time to final time
		manager.setInitialTime(initialTime);
		manager.setFinalTime(finalTime);
		manager.integrate(system);
		
		// Save the simulation results
		Storage states(manager.getStateStorage());
		//states.print("results.sto");
	}
    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 << "\nOutput: " << cost << "\n------------------\n";
	return cost;
} //calcCost