diff options
| -rw-r--r-- | LocomotorPrimitives.cpp | 6 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 68 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.h | 5 | ||||
| -rw-r--r-- | MuscleEMGProfile.cpp | 7 | ||||
| -rw-r--r-- | MuscleEMGProfile.h | 49 | ||||
| -rw-r--r-- | adults_es_r.csv | 35 |
6 files changed, 165 insertions, 5 deletions
diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp index 341fa74..ea5f247 100644 --- a/LocomotorPrimitives.cpp +++ b/LocomotorPrimitives.cpp @@ -14,7 +14,6 @@ #define NO_SIM 0 static const std::string MODEL_NAME = "LocomotorPrimitives"; - // Define the initial and final simulation time static const double initialTime = 0.0; static const double finalTime = 2.0; @@ -25,6 +24,7 @@ static void constructModel(OpenSim::Model &model) // Define controller for the model LocomotorPrimitivesController *control = new LocomotorPrimitivesController(0.01); + control->loadCsvData("es_r", "../../adults_es_r.csv"); control->setActuators(model.updActuators()); model.addController(control); @@ -37,8 +37,8 @@ static void constructModel(OpenSim::Model &model) for (int i = 0; i < sz; i++) { OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast<OpenSim::ActivationFiberLengthMuscle *>(&actSet.get(i)); std::cout << " " << m->getName() << std::endl; - m->setDefaultActivation(0.01); - m->setDefaultFiberLength(0.1); +// m->setDefaultActivation(0.5); +// m->setDefaultFiberLength(0.1); } // Create the force reporter for obtaining the forces applied to the model diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp index 4b7bc1b..381e21f 100644 --- a/LocomotorPrimitivesController.cpp +++ b/LocomotorPrimitivesController.cpp @@ -1,18 +1,75 @@ #include <OpenSim/OpenSim.h> #include "LocomotorPrimitivesController.h" +#include "MuscleEMGProfile.h" #define VERBOSE 1 static const double TIME_DAMP = 0.1; +int LocomotorPrimitivesController::loadCsvData(const std::string &muscleName, const std::string &file) +{ + std::cout << ">> loading CSV data from " << file << std::endl; + std::ifstream data(file); + std::string line; + + std::cout << data.tellg() << std::endl; + + if (!data.is_open()) { + std::cerr << "Error loading CSV data from " << file << std::endl; + return -1; + } + + MuscleEMGProfile p(muscleName, 0); + + while (std::getline(data, line)) { + std::stringstream s(line); + std::string cell; + + int i = 0; + double x; + while (std::getline(s, cell, ',')) { + if (!isdigit(cell[0])) + continue; + + std::cout << "read cell(" << (i == 0 ? "x" : "y") << ") -> " << cell << std::endl; + double val = (double) atof(cell.c_str()); + + if (i == 1) + p.addData(x, val); + else + x = val; + + i = !i; + } + } + + data.close(); + + _act.push_back(p); + std::cout << ">>> loaded EMG profile for " << p.getName() << ", " << p.getIdx() << "/" << p.getCapacity() << std::endl; + + return 0; +} + void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const { double t = s.getTime(); static double last_twitch = t; + /* Extract muscle activation for each of the muscles */ + for (std::vector<MuscleEMGProfile>::const_iterator it = _act.begin(); it != _act.end(); ++it) { + MuscleEMGProfile p = *it; + + std::cout << " " << p.getName() << "(" << p.getIdx() << "/" << p.getCapacity() << ")" << std::endl; + } + //const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("bifemlh_r")); const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("rect_fem_r")); + const OpenSim::Muscle *ercspn_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_r")); + const OpenSim::Muscle *ercspn_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("ercspn_l")); + const OpenSim::Muscle *extobl_r = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_r")); + const OpenSim::Muscle *extobl_l = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("extobl_l")); double v = rectfem->getLengtheningSpeed(s); double act = v * _alpha; @@ -28,10 +85,17 @@ void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK if (act > 0.0) last_twitch = t; - if (VERBOSE && act > 0.0) - std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl; + //if (VERBOSE && act > 0.0) + // std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl; SimTK::Vector ctrl_rectfem(1, act); + SimTK::Vector ctrl_extobl(1, 0.79); + SimTK::Vector ctrl_ercsp(1, 0.375); rectfem->addInControls(ctrl_rectfem, controls); + + ercspn_r->addInControls(ctrl_ercsp, controls); + ercspn_l->addInControls(ctrl_ercsp, controls); + extobl_r->addInControls(ctrl_extobl, controls); + extobl_l->addInControls(ctrl_extobl, controls); }
\ No newline at end of file diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h index 7d0c08a..0760459 100644 --- a/LocomotorPrimitivesController.h +++ b/LocomotorPrimitivesController.h @@ -1,7 +1,9 @@ #ifndef LOCOMOTORPRIMITIVES_CONTROLLER_H_ #define LOCOMOTORPRIMITIVES_CONTROLLER_H_ +#include <map> #include <OpenSim/OpenSim.h> +#include "MuscleEMGProfile.h" class LocomotorPrimitivesController : public OpenSim::Controller { OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller); @@ -9,9 +11,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controll public: LocomotorPrimitivesController(double alpha) : OpenSim::Controller(), _alpha(alpha) { } + int loadCsvData(const std::string &muscleName, const std::string &file); void computeControls(const SimTK::State &s, SimTK::Vector &controls) const; private: double _alpha; + + std::vector<MuscleEMGProfile> _act; }; #endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */ diff --git a/MuscleEMGProfile.cpp b/MuscleEMGProfile.cpp new file mode 100644 index 0000000..b1b4977 --- /dev/null +++ b/MuscleEMGProfile.cpp @@ -0,0 +1,7 @@ +#include "MuscleEMGProfile.h" + +void MuscleEMGProfile::addData(double x, double y) +{ + _x.push_back(x); + _y.push_back(y); +}
\ No newline at end of file diff --git a/MuscleEMGProfile.h b/MuscleEMGProfile.h new file mode 100644 index 0000000..2d31f27 --- /dev/null +++ b/MuscleEMGProfile.h @@ -0,0 +1,49 @@ +#ifndef MUSCLE_EMG_PROFILE_H_ +#define MUSCLE_EMG_PROFILE_H_ + +#include <ostream> +#include <string> +#include <vector> + +class MuscleEMGProfile { +public: + MuscleEMGProfile(const std::string &name, unsigned capacity) + : _name(const_cast<std::string &>(name)) + { + if (capacity == 0) + capacity = 100; + _x.reserve(capacity); + _y.reserve(capacity); + } + + MuscleEMGProfile& operator=(const MuscleEMGProfile &m) + { + _name = m._name; + _x = m._x; + _y = m._y; + + return *this; + } + + unsigned getIdx() { return _x.size(); } + unsigned getCapacity() { return _x.max_size(); } + const std::string &getName() { return _name; } + + void addData(double x, double y); + +private: + std::string _name; + std::vector<double> _x; + std::vector<double> _y; + +// friend std::ostream& operator<<(std::ostream &s, const MuscleEMGProfile &m); +}; + +/* +std::ostream& operator<<(std::ostream &s, const MuscleEMGProfile &m) +{ + return s << "MuscleEMGProfile(" << m._name << ", " << m._idx << "/" << m._capacity << ")" << std::endl; +} +*/ + +#endif /* MUSCLE_EMG_PROFILE_H_ */
\ No newline at end of file diff --git a/adults_es_r.csv b/adults_es_r.csv new file mode 100644 index 0000000..2481326 --- /dev/null +++ b/adults_es_r.csv @@ -0,0 +1,35 @@ +x,y +0.0,6.09756 +2.54237,7.31707 +4.23729,9.7561 +7.62712,9.7561 +11.0169,7.31707 +12.7119,7.31707 +14.4068,6.09756 +17.7966,6.09756 +19.4915,4.87805 +21.1864,4.87805 +22.8814,3.65854 +29.661,3.65854 +31.3559,2.43902 +35.5932,2.43902 +43.2203,2.43902 +47.4576,6.09756 +50.8475,10.9756 +55.0847,17.0732 +58.4746,17.0732 +60.1695,14.6341 +61.8644,13.4146 +65.2542,9.7561 +68.6441,8.53659 +72.0339,7.31707 +73.7288,6.09756 +77.1186,4.87805 +79.661,4.87805 +81.3559,3.65854 +85.5932,3.65854 +89.8305,3.65854 +91.5254,2.43902 +94.9153,3.65854 +98.3051,4.87805 +100,4.87805 |