bool NC2010DynamicMovementPrimitive::initialize(const vector<string>& variable_names, lwr_lib::LWRParamPtr lwr_parameters, const double k_gain, const double d_gain) { assert(!variable_names.empty()); assert(lwr_parameters->isInitialized()); vector<TSPtr> nc2010_transformation_systems; for (int i = 0; i < (int)variable_names.size(); ++i) { NC2010TSParamPtr nc2010_transformation_system_parameters(new NC2010TransformationSystemParameters()); lwr_lib::LWRPtr lwr_model(new lwr_lib::LWR()); if (!lwr_model->initialize(lwr_parameters)) { Logger::logPrintf("Could not initialize LWR model with provide parameters.", Logger::ERROR); return false; } if (!nc2010_transformation_system_parameters->initialize(lwr_model, variable_names[i], k_gain, d_gain)) { Logger::logPrintf("Could not initialize transformation system parameters.", Logger::ERROR); return false; } NC2010TSStatePtr nc2010_transformation_system_state(new NC2010TransformationSystemState()); NC2010TSPtr nc2010_transformation_system(new NC2010TransformationSystem()); if (!nc2010_transformation_system->initialize(nc2010_transformation_system_parameters, nc2010_transformation_system_state, TransformationSystem::NORMAL)) { Logger::logPrintf("Could not initialize transformation system.", Logger::ERROR); return false; } nc2010_transformation_systems.push_back(nc2010_transformation_system); } NC2010CSParamPtr nc2010_canonical_system_parameters(new NC2010CanonicalSystemParameters()); NC2010CSStatePtr nc2010_canonical_system_state(new NC2010CanonicalSystemState()); NC2010CSPtr nc2010_canonical_system(new NC2010CanonicalSystem()); if (!nc2010_canonical_system->initialize(nc2010_canonical_system_parameters, nc2010_canonical_system_state)) { Logger::logPrintf("Could not initialize canonical system.", Logger::ERROR); return false; } DMPParamPtr dmp_parameters(new DynamicMovementPrimitiveParameters()); if(!dmp_parameters->setCutoff(0.001)) { Logger::logPrintf("Could not set cutoff.", Logger::ERROR); return false; } if (!DynamicMovementPrimitive::initialize(dmp_parameters, nc2010_transformation_systems, nc2010_canonical_system)) { Logger::logPrintf("Could not initialize dmp.", Logger::ERROR); return false; } return (initialized_ = true); }
bool ICRA2009DynamicMovementPrimitive::initialize(const vector<string>& variable_names, lwr_lib::LWRParamPtr lwr_parameters, const double k_gain, const double d_gain) { assert(!variable_names.empty()); assert(lwr_parameters->isInitialized()); vector<ICRA2009TSPtr> icra2009_transformation_systems; for (int i = 0; i < (int)variable_names.size(); ++i) { ICRA2009TSParamPtr icra2009_transformation_system_parameters(new ICRA2009TransformationSystemParameters()); lwr_lib::LWRPtr lwr_model(new lwr_lib::LWR()); if (!lwr_model->initialize(lwr_parameters)) { Logger::logPrintf("Could not initialize LWR model with provide parameters.", Logger::ERROR); return false; } if (!icra2009_transformation_system_parameters->initialize(lwr_model, variable_names[i], k_gain, d_gain)) { Logger::logPrintf("Could not initialize transformation system parameters.", Logger::ERROR); return false; } // TODO: fix integration method !! ICRA2009TSStatePtr icra2009_transformation_system_state(new ICRA2009TransformationSystemState()); ICRA2009TSPtr icra2009_transformation_system(new ICRA2009TransformationSystem()); if (!icra2009_transformation_system->initialize(icra2009_transformation_system_parameters, icra2009_transformation_system_state, TransformationSystem::NORMAL)) { Logger::logPrintf("Could not initialize transformation system.", Logger::ERROR); return false; } icra2009_transformation_systems.push_back(icra2009_transformation_system); } ICRA2009CSParamPtr icra2009_canonical_system_parameters(new ICRA2009CanonicalSystemParameters()); ICRA2009CSStatePtr icra2009_canonical_system_state(new ICRA2009CanonicalSystemState()); ICRA2009CSPtr icra2009_canonical_system(new ICRA2009CanonicalSystem()); if (!icra2009_canonical_system->initialize(icra2009_canonical_system_parameters, icra2009_canonical_system_state)) { Logger::logPrintf("Could not initialize canonical system.", Logger::ERROR); return false; } ICRA2009DMPParamPtr dmp_parameters(new ICRA2009DynamicMovementPrimitiveParameters); if(!dmp_parameters->setCutoff(0.001)) { Logger::logPrintf("Could not set cutoff.", Logger::ERROR); return false; } ICRA2009DMPStatePtr state(new ICRA2009DynamicMovementPrimitiveState()); //Change because it fails, the same function it's done by the next lines /*if (!DynamicMovementPrimitive::initialize(dmp_parameters, icra2009_transformation_systems, icra2009_canonical_system)) { Logger::logPrintf("Could not initialize dmp.", Logger::ERROR); return false; }*/ if(initialize(dmp_parameters, state, icra2009_transformation_systems, icra2009_canonical_system)) { if(!setupIndices()) { return (initialized_ = false); } //zero_feedback_ = Eigen::VectorXd::Zero(indices_.size()); return (initialized_ = true); } return (initialized_ = false); }