C++ (Cpp) LWRParamPtr::isInitialized Examples

Example #1

File: nc2010_dynamic_movement_primitive.cpp Project: HiroyukiMikita/usc-clmc-ros-pkg

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

Example #2

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