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);
}