simtk.org

Code: Select all

void forwardForInverseResult(OpenSim::Model model, const std::string& inputSTOfn, const std::string& inputForceSTOfn) {
	std::cout << "Running simulation..." << std::endl;

	// Contains the output of inverse dynamics (forces and torques)
	auto inputSTO = std::unique_ptr<OpenSim::Storage>(new OpenSim::Storage(inputForceSTOfn));
	// Contains the initial position of the model, taken from the recorded original motion.
	auto inputSTOposition = std::unique_ptr<OpenSim::Storage>(new OpenSim::Storage(inputSTOfn));
	
	if (inputSTO->isInDegrees())
		model.getSimbodyEngine().convertDegreesToRadians(*inputSTO);
	
	const double firstTime = inputSTO->getFirstTime();
	const double lastTime = inputSTO->getLastTime();

	/*  Adding the controller actuators */
	/* This section adds the missing actuators acting on the pelvis: yaw/roll/pitch/longitudinal/vertical/lateral */
	auto controller = new OpenSim::PrescribedController();
	model.addController(controller);
	for (int i = 0; i < model.getNumCoordinates(); ++i) {
		const auto& c = model.getCoordinateSet().get(i);

		OpenSim::CoordinateActuator* ca;
		if (model.getActuators().contains(c.getName() + "_ca")) {
			std::cout << "Using the existing " << c.getName() << "_ca." << std::endl;
			ca = OpenSim::CoordinateActuator::safeDownCast(
				&(model.updActuators().get(c.getName() + "_ca")));
			controller->addActuator(*ca);
		}
		else {
			std::cout << "Adding a new " << c.getName() << "_ca." << std::endl;
			ca = new OpenSim::CoordinateActuator(c.getName());
			ca->setName(c.getName() + "_ca");
			ca->setOptimalForce(1.);
			ca->setMinControl(-1e3);
			ca->setMaxControl(1e3);
			model.addForce(ca);
			controller->addActuator(*ca);
		}
		controller->prescribeControlForActuator(ca->getName(), new OpenSim::Constant(0.));
	}
	/*  END Adding the controller actuators */
	
	
	model.setUseVisualizer(true);
	auto state = model.initSystem();


	/* Setting the model's initial position from coordinates file. */
	for (int i = 0; i < model.getNumCoordinates(); ++i) {
		const auto& coord = model.getCoordinateSet().get(i);
		double initialValue = inputSTOposition->getStateVector(0)->getData()[i];
		coord.setValue(state, initialValue);
	}
	model.realizePosition(state);
	/* End setting the model's initial position. */


	/* Setting the times column */
	OpenSim::Array<double> times;
	inputSTO->getTimeColumn(times);
	std::vector<double> timesVec(times.getSize());
	// set the times vetor only once at the beginning
		for (int p = 0; p < times.getSize(); p++) {
			timesVec[p] = times[p];
		}
	/* End Setting the times column */

	// Define the coordinate names
	static const std::unordered_set<std::string> coordinateNames = {
		"Hip_RR_r", "Hip_DB_r", "Knee_FE_r", "Hip_EF_r",
		"Hip_EF_l", "Hip_RR_l", "Hip_BD_l", "Knee_FE_l",
		"Shoulder_BD_r", "Shoulder_EF_r", "Elbow_EF_r",
		"Shoulder_DB_l", "Shoulder_EF_l", "Shoulder_RR_l", "Elbow_EF_l",
		"Neck_LL", "Neck_RR", "Neck_FE", "Shoulder_RR_r", "SpineL_LL", "SpineL_RR", "SpineL_FE", "SpineU_FE",
	};

	state.setTime(firstTime);

	for (int i = 0; i < model.getNumCoordinates(); ++i) {
		auto& c = model.updCoordinateSet().get(i);

		// If coordinate is excluded, skip the rest of the loop
		// (Here if the "find" function returns the end iterator, it means the coordinate is not found)
		if (coordinateNames.find(c.getName()) != coordinateNames.end()) {
			continue;
		}

		// Iterate through CoordinateLimitForce components to apply limits
		for (const auto& x : model.getComponentList<OpenSim::CoordinateLimitForce>()) {
			if (x.get_coordinate() == c.getName()) {
				double upperLimitRad = SimTK::convertDegreesToRadians(x.getUpperLimit());
				double lowerLimitRad = SimTK::convertDegreesToRadians(x.getLowerLimit());

				// Adjust the coordinate value to be within the limits
				double currentValue = c.getValue(state);
				if (currentValue > upperLimitRad) {
					c.setValue(state, upperLimitRad);
				}
				else if (currentValue < lowerLimitRad) {
					c.setValue(state, lowerLimitRad);
				}

				// Break out of the loop once the match is found
				break;
			}
		}

	}

	// Start simulation
	auto manager = std::make_unique<OpenSim::Manager>(model, state);

	// Loop over coordinates
	for (int k = 0; k < model.getNumCoordinates(); ++k) {
		const auto& c = model.getCoordinateSet().get(k);
		cout << "Getting coordinate at k:" << k << " that is: " << c.getName() << endl;

		// Skip excluded coordinates
		if (coordinateNames.find(c.getName()) == coordinateNames.end()) {
			cout << "Skipping excluded Coordinate: " << c.getName() << endl;
			continue;
		}

		// Safe downcast to CoordinateActuator
		auto* ca = OpenSim::CoordinateActuator::safeDownCast(
			&(model.updActuators().get(c.getName() + "_ca")));
		if (!ca) continue;

		// Get the force value from the input forces file
		OpenSim::Array<double> forceValues;
		
		auto columnName = c.getAbsolutePathString() + "/value";
		
		inputSTO->getDataColumn(columnName, forceValues);

		// Convert OpenSim::Array to std::vector for time and force values
		std::vector<double> forceValuesVec(forceValues.getSize());
		for (int p = 0; p < times.getSize(); p++) {
			forceValuesVec[p]= forceValues[p];
		}
	 

		auto newSpline = OpenSim::GCVSpline(5, forceValuesVec.size(), timesVec.data(), forceValuesVec.data(), "");

		controller->prescribeControlForActuator(
			controller->getActuatorSet().get(c.getName() + "_ca").getName(),
			new OpenSim::GCVSpline(newSpline));

	}

	// Integrate the model's state over time
	state = manager->integrate(lastTime);
}