diff --git a/entity_generation/smurf/src/smurf.cpp b/entity_generation/smurf/src/smurf.cpp index 8c2f6db42..278ce5c44 100644 --- a/entity_generation/smurf/src/smurf.cpp +++ b/entity_generation/smurf/src/smurf.cpp @@ -929,9 +929,7 @@ namespace mars { if (!loadMotor(motorList[i])) return 0; - for (std::map::iterator it = mimicmotors.begin(); - it != mimicmotors.end(); ++it) - loadMimic(it->first, it->second); + control->motors->connectMimics(); for (unsigned int i = 0; i < sensorList.size(); ++i) if (!loadSensor(sensorList[i])) @@ -1049,50 +1047,9 @@ namespace mars { entity->addMotor(motor.index, motor.name); - sim::SimMotor* newMotor = control->motors->getSimMotor(newId); - - // set motor mimics - if (config.find("mimic_motor") != config.end()) { - mimicmotors[newId] = (std::string)config["mimic_motor"]; - newMotor->setMimic( - (sReal)config["mimic_multiplier"], (sReal)config["mimic_offset"]); - } - - // set approximation functions - if (config.find("maxeffort_approximation") != config.end()) { - std::vector* maxeffort_coefficients = new std::vector; - ConfigVector::iterator vIt = config["maxeffort_coefficients"].begin(); - for (; vIt != config["maxeffort_coefficients"].end(); ++vIt) { - maxeffort_coefficients->push_back((double)(*vIt)); - newMotor->setMaxEffortApproximation( - utils::getApproximationFunctionFromString((std::string)config["maxeffort_approximation"]), - maxeffort_coefficients); - } - } - if (config.find("maxspeed_approximation") != config.end()) { - fprintf(stderr, "found maxspeed_approximation in %s\n", ((std::string)config["name"]).c_str()); - std::vector* maxspeed_coefficients = new std::vector; - ConfigVector::iterator vIt = config["maxspeed_coefficients"].begin(); - for (; vIt != config["maxspeed_coefficients"].end(); ++vIt) { - maxspeed_coefficients->push_back((double)(*vIt)); - newMotor->setMaxSpeedApproximation( - utils::getApproximationFunctionFromString((std::string)config["maxspeed_approximation"]), - maxspeed_coefficients); - } - } return true; } - void SMURF::loadMimic(unsigned long mimicId, std::string parent_name) { - sim::SimMotor* parentmotor = - control->motors->getSimMotorByName(parent_name); - if (parentmotor != NULL) - fprintf(stderr, ", %s\n", parentmotor->getName().c_str()); - else - fprintf(stderr, "no parentmotor found\n"); - parentmotor->addMimic(control->motors->getSimMotor(mimicId)); - } - BaseSensor* SMURF::loadSensor(ConfigMap config) { config["mapIndex"].push_back(ConfigItem(mapIndex)); BaseSensor *sensor = control->sensors->createAndAddSensor(&config); diff --git a/entity_generation/smurf/src/smurf.h b/entity_generation/smurf/src/smurf.h index efdd0b5e1..92b00ed56 100644 --- a/entity_generation/smurf/src/smurf.h +++ b/entity_generation/smurf/src/smurf.h @@ -84,7 +84,6 @@ namespace mars { std::vector controllerList; std::vector graphicList; std::vector lightList; - std::map mimicmotors; private: int groupID; @@ -112,8 +111,6 @@ namespace mars { boost::shared_ptr model; sim::SimEntity* entity; - void loadMimic(unsigned long mimicId, std::string parent_name); - void handleURI(configmaps::ConfigMap *map, std::string uri); void handleURIs(configmaps::ConfigMap *map); void getSensorIDList(configmaps::ConfigMap *map); diff --git a/interfaces/src/MotorData.cpp b/interfaces/src/MotorData.cpp index 0d50353d2..d40af4c62 100644 --- a/interfaces/src/MotorData.cpp +++ b/interfaces/src/MotorData.cpp @@ -136,6 +136,8 @@ namespace mars { GET_VALUE("maxPosition", maxValue, Double); GET_VALUE("minPosition", minValue, Double); + this->config = *config; + return 1; } diff --git a/interfaces/src/sim/MotorManagerInterface.h b/interfaces/src/sim/MotorManagerInterface.h index 25d1bfd9d..24392db6b 100644 --- a/interfaces/src/sim/MotorManagerInterface.h +++ b/interfaces/src/sim/MotorManagerInterface.h @@ -320,6 +320,8 @@ namespace mars { virtual void getDataBrokerNames(unsigned long jointId, std::string *groupName, std::string *dataName) const = 0; + + virtual void connectMimics() = 0; }; // class MotorManagerInterface } // end of namespace interfaces diff --git a/sim/src/core/MotorManager.cpp b/sim/src/core/MotorManager.cpp index 8ed1efa2f..6968a9dad 100644 --- a/sim/src/core/MotorManager.cpp +++ b/sim/src/core/MotorManager.cpp @@ -41,6 +41,7 @@ #include #include #include +#include namespace mars { namespace sim { @@ -96,6 +97,39 @@ namespace mars { simMotors[newMotor->getIndex()] = newMotor; iMutex.unlock(); control->sim->sceneHasChanged(false); + + configmaps::ConfigMap &config = motorS->config; + + // set motor mimics + if (config.find("mimic_motor") != config.end()) { + mimicmotors[motorS->index] = (std::string)config["mimic_motor"]; + newMotor->setMimic( + (sReal)config["mimic_multiplier"], (sReal)config["mimic_offset"]); + } + + // set approximation functions + if (config.find("maxeffort_approximation") != config.end()) { + std::vector* maxeffort_coefficients = new std::vector; + ConfigVector::iterator vIt = config["maxeffort_coefficients"].begin(); + for (; vIt != config["maxeffort_coefficients"].end(); ++vIt) { + maxeffort_coefficients->push_back((double)(*vIt)); + newMotor->setMaxEffortApproximation( + utils::getApproximationFunctionFromString((std::string)config["maxeffort_approximation"]), + maxeffort_coefficients); + } + } + if (config.find("maxspeed_approximation") != config.end()) { + fprintf(stderr, "found maxspeed_approximation in %s\n", ((std::string)config["name"]).c_str()); + std::vector* maxspeed_coefficients = new std::vector; + ConfigVector::iterator vIt = config["maxspeed_coefficients"].begin(); + for (; vIt != config["maxspeed_coefficients"].end(); ++vIt) { + maxspeed_coefficients->push_back((double)(*vIt)); + newMotor->setMaxSpeedApproximation( + utils::getApproximationFunctionFromString((std::string)config["maxspeed_approximation"]), + maxspeed_coefficients); + } + } + return motorS->index; } @@ -384,9 +418,10 @@ namespace mars { void MotorManager::clearAllMotors(bool clear_all) { MutexLocker locker(&iMutex); map::iterator iter; - for(iter = simMotors.begin(); iter != simMotors.end(); iter++) + for(iter = simMotors.begin(); iter != simMotors.end(); iter++) delete iter->second; simMotors.clear(); + mimicmotors.clear(); if(clear_all) simMotorsReload.clear(); next_motor_id = 1; } @@ -407,6 +442,7 @@ namespace mars { iMutex.lock(); } iMutex.unlock(); + connectMimics(); } /** @@ -487,5 +523,14 @@ namespace mars { iter->second->getDataBrokerNames(groupName, dataName); } + void MotorManager::connectMimics() { + std::map::iterator it; + for (it = mimicmotors.begin(); it!=mimicmotors.end(); ++it) { + SimMotor* parentmotor = getSimMotorByName(it->second); + if (parentmotor != NULL) + parentmotor->addMimic(simMotors[it->first]); + } + } + } // end of namespace sim } // end of namespace mars diff --git a/sim/src/core/MotorManager.h b/sim/src/core/MotorManager.h index 47b725617..8e189307e 100644 --- a/sim/src/core/MotorManager.h +++ b/sim/src/core/MotorManager.h @@ -313,6 +313,8 @@ namespace mars { std::string *groupName, std::string *dataName) const; + virtual void connectMimics(); + private: //! the id of the next motor that is added to the simulation unsigned long next_motor_id; @@ -329,6 +331,8 @@ namespace mars { //! a mutex for the motor containters mutable utils::Mutex iMutex; + // map of mimicmotors + std::map mimicmotors; }; // class MotorManager } // end of namespace sim diff --git a/sim/src/core/SimMotor.cpp b/sim/src/core/SimMotor.cpp index bcf16a546..463d7f231 100644 --- a/sim/src/core/SimMotor.cpp +++ b/sim/src/core/SimMotor.cpp @@ -118,6 +118,7 @@ namespace mars { // delete any coefficient vectors we might have created delete maxspeed_coefficients; delete maxeffort_coefficients; + mimics.clear(); } void SimMotor::addMimic(SimMotor* mimic) {