summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--LocomotorPrimitives.cpp77
-rw-r--r--LocomotorPrimitivesManager.cpp12
-rw-r--r--LocomotorPrimitivesManager.h3
3 files changed, 80 insertions, 12 deletions
diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp
index 0a0ebcc..de031f2 100644
--- a/LocomotorPrimitives.cpp
+++ b/LocomotorPrimitives.cpp
@@ -7,6 +7,8 @@
* Copyright (C) 2013 Tobias Klauser <tobias.klauser@uzh.ch>
*/
+#include <signal.h>
+
#include <OpenSim/OpenSim.h>
#include <OpenSim/Common/IO.h>
@@ -21,6 +23,19 @@ static const std::string MODEL_NAME = "LocomotorPrimitives";
static const double INTEGRATOR_ACCURACY = 1.0e-3;
static Logger &logger = Logger::getInstance();
+static LocomotorPrimitivesManager *manager = NULL;
+
+static void signal_handler(int signr)
+{
+ switch (signr) {
+ case SIGINT:
+ logger.log("Got signal SIGINT. Quitting...\n");
+ if (manager)
+ manager->halt();
+ default:
+ break;
+ }
+}
static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
{
@@ -44,15 +59,28 @@ static void constructModel(OpenSim::Model &model, OpenSim::Storage actData)
for (int i = 0; i < sz; i++) {
OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast<OpenSim::ActivationFiberLengthMuscle *>(&actSet.get(i));
logger.log(" => %s\n", m->getName().c_str());
-// m->setDefaultActivation(0.5);
-// m->setDefaultFiberLength(0.1);
+ m->setDefaultActivation(0.01);
+ m->setDefaultFiberLength(0.1);
}
+// logger.log("+ Adding Actuation Analysis\n");
+// OpenSim::Actuation *actuation = new OpenSim::Actuation(&model);
+// actuation->setStepInterval(1);
+// model.addAnalysis(actuation);
+
// Create the force reporter for obtaining the forces applied to the model
// during a forward simulation
+ logger.log("+ Adding Force Reporter\n");
OpenSim::ForceReporter *reporter = new OpenSim::ForceReporter(&model);
model.addAnalysis(reporter);
+ // Muscle analysis for recording and computting basic quantities
+ // (length, shortening velocity, tendon length, ...) for muscles during
+ // a simulation
+// logger.log("+ Adding Muscle Analysis\n");
+// OpenSim::MuscleAnalysis *analysis = new OpenSim::MuscleAnalysis(&model);
+// model.addAnalysis(analysis);
+
logger.log("Finished constructing model\n");
}
@@ -80,7 +108,8 @@ void simulateModel(OpenSim::Model &model, const double initial_time,
// model.printDetailedInfo(si, std::cout);
// std::cout << "=== END MODEL SUMMARY ===" << std::endl << std::endl;
- logger.log("+ Creating integrator (RungeKuttaMerson) with accuracy %e and step size %f (0.0 means dynamic)\n", INTEGRATOR_ACCURACY, fixed_step_size);
+ logger.log("+ Creating integrator (RungeKuttaMerson) with accuracy %e and step size %f (0.0 means dynamic)\n",
+ INTEGRATOR_ACCURACY, fixed_step_size);
// Create the integrator and manager for the simulation
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
if (fixed_step_size > 0)
@@ -88,22 +117,38 @@ void simulateModel(OpenSim::Model &model, const double initial_time,
integrator.setAccuracy(INTEGRATOR_ACCURACY);
logger.log("+ Creating simulation manager\n");
- LocomotorPrimitivesManager manager(model, integrator);
+ manager = new LocomotorPrimitivesManager(model, integrator);
// Integrate from initial time to final time
- manager.setInitialTime(initial_time);
- manager.setFinalTime(final_time);
+ manager->setInitialTime(initial_time);
+ manager->setFinalTime(final_time);
logger.log("+ Integrating from time %f to %f\n", initial_time, final_time);
- manager.integrate(si);
+ try {
+ manager->integrate(si);
+ } catch (...) {
+ logger.log("Got exception during integration\n");
+ }
// Save the model states from forward integration
logger.log("+ Writing states to %s_states.sto\n", MODEL_NAME.c_str());
- OpenSim::Storage statesDegrees(manager.getStateStorage());
+ OpenSim::Storage statesDegrees(manager->getStateStorage());
statesDegrees.print(MODEL_NAME + "_states.sto");
+ logger.log("+ Writing states (degrees) to %s_states_degrees.mot\n", MODEL_NAME.c_str());
+ model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
+ statesDegrees.setWriteSIMMHeader(true);
+ statesDegrees.print(MODEL_NAME + "_states_degrees.mot");
+
// Save the forces
logger.log("+ Writing forces to %s_forces.mot\n", MODEL_NAME.c_str());
- OpenSim::Analysis &reporter = model.getAnalysisSet()[0];
- ((OpenSim::ForceReporter &)reporter).getForceStorage().print(MODEL_NAME + "_forces.mot");
+ const OpenSim::Analysis &a = model.getAnalysisSet().get("ForceReporter");
+ ((OpenSim::ForceReporter &) a).getForceStorage().print(MODEL_NAME + "_forces.mot");
+
+ // Save the actuation
+// logger.log("+ Writing actuation to %s_actuation.sto\n", MODEL_NAME.c_str());
+ // TODO
+
+// logger.log("+ Writing muscle analysis to %s_muscles\n", MODEL_NAME.c_str());
+ // TODO
logger.log("Finished simulating model\n");
}
@@ -127,6 +172,9 @@ int main(int argc, char **argv)
std::string data_file = "../../data/adults.sto";
std::string output_dir = ".";
+ sigset_t block_mask;
+ struct sigaction saction;
+
for (int i = 1; (i < argc) && (argv[i][0] == '-'); i++) {
switch (argv[i][1]) {
case 'd':
@@ -160,7 +208,14 @@ int main(int argc, char **argv)
exit(-1);
}
- logger.log("Starting simulation %s with model file %s, actuation data file %s\n, fixed step size: %f", MODEL_NAME.c_str(), model_file.c_str(), data_file.c_str(), fixed_step_size);
+ /* Register signal handler */
+ sigfillset(&block_mask);
+ saction.sa_handler = signal_handler;
+ saction.sa_mask = block_mask;
+ saction.sa_flags = SA_RESTART;
+ sigaction(SIGINT, &saction, NULL);
+
+ logger.log("Starting simulation %s with model file %s, actuation data file %s\n, fixed step size: %f\n", MODEL_NAME.c_str(), model_file.c_str(), data_file.c_str(), fixed_step_size);
try {
// Create an OpenSim model and set its name
diff --git a/LocomotorPrimitivesManager.cpp b/LocomotorPrimitivesManager.cpp
index d1925db..eb267e2 100644
--- a/LocomotorPrimitivesManager.cpp
+++ b/LocomotorPrimitivesManager.cpp
@@ -5,5 +5,17 @@
bool LocomotorPrimitivesManager::doIntegration(SimTK::State &s, int step, double dtFirst)
{
// TODO possibly additional calculations
+ std::cout << "==> doIntegration" << std::endl;
return OpenSim::Manager::doIntegration(s, step, dtFirst);
}
+
+bool LocomotorPrimitivesManager::integrate(SimTK::State &s, double dtFirst)
+{
+ int step = 0;
+
+ std::cout << "==> integrate" << std::endl;
+
+ s.setTime(getInitialTime());
+
+ return doIntegration(s, step, dtFirst);
+}
diff --git a/LocomotorPrimitivesManager.h b/LocomotorPrimitivesManager.h
index 2bd6e3d..f58dd17 100644
--- a/LocomotorPrimitivesManager.h
+++ b/LocomotorPrimitivesManager.h
@@ -8,7 +8,8 @@ class LocomotorPrimitivesManager : public OpenSim::Manager
public:
LocomotorPrimitivesManager(OpenSim::Model &model, SimTK::Integrator &integrator)
: OpenSim::Manager(model, integrator) { }
+ bool integrate(SimTK::State &s, double dtFirst=1.0e-6);
bool doIntegration(SimTK::State &s, int step, double dtFirst);
};
-#endif /* LOCOMOTORPRIMITIVES_MANAGER_H_ */ \ No newline at end of file
+#endif /* LOCOMOTORPRIMITIVES_MANAGER_H_ */