I am very new to OpenSim, so this could be a trivial issue but still i could not find an answer in the forum or the internet
.I am using OpenSim4.5 and I want to visualize motion based on the output of Inverse Dynamics (that means using forces and torques).
I can see the window popup, but the motion displayed is not even close to the original one. My initial motion is 15s long but the simulation often crashes after ~5seconds. Moreover, i get different movements when i run the same code multiple times even without changing the motion file.
I will post my code and I hope someone more experienced that me can take a look because i feel there is something simple that I am just overlooking
.Here is a brief summary of the code: the function has the model, the original motion .sto file (for initial position) and the forces produced by the inverse dynamics simulation (also .sto).
1) First, the missing actuators to the model (the 6 forces and torques acting on the pelvis) are added.
2) Then, I set the upper and lowel limits for the coordinates
3) I iterate over the coorinates specified in the list "coordinateNames" and I use a GCV spline over each coordinate to set the prescribeControlForActuator.
4) At the end I use the manager to integrate the until the last time of the motion.
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);
}
Thanks a lot in advance!