C++ (Cpp) Model::getLinks Beispiele

Beispiel #1

Datei: rbdl_parser.cpp Projekt: NimbRo-Copter/nimbro-op-ros

bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
	// Reset root mass to zero and give it the proper name
	mBodies[0].mMass = 0;

	// Set gravity to ROS standards
	gravity << 0, 0, -9.81;

	std::vector<boost::shared_ptr<urdf::Link> > links;
	model.getLinks(links);

	boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
	boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);

	if(oldRoot == newRoot || !newRoot)
		process(*oldRoot, 0, Math::SpatialTransform());
	else
		processReverse(*newRoot, 0, 0, Math::SpatialTransform());

	// Rename the root body to our URDF root
	mBodyNameMap.erase("ROOT");
	mBodyNameMap[root] = 0;

	return true;
}

Beispiel #2

Datei: urdf_renderer.cpp Projekt: CMobley7/realtime_urdf_filter

  /// /////////////////////////////////////////////////////////////////////////////
  /// @brief load URDF model description from string and create search operations data structures
  void URDFRenderer::loadURDFModel
    (urdf::Model &model)
  {
    typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
    V_Link links;
    model.getLinks(links);

    V_Link::iterator it = links.begin();
    V_Link::iterator end = links.end();

    for (; it != end; ++it)
      process_link (*it);
  }

Beispiel #3

Datei: rbdl_parser.cpp Projekt: AIS-Bonn/humanoid_op_ros

bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif
	
	// Save the root link name
	m_nameURDFRoot = model.getRoot()->name;
	m_indexURDFRoot = (unsigned int) -1;
	
	// Reset root mass to zero and give it the proper name
	mBodies[0].mMass = 0;

	// Set gravity to ROS standards
	gravity << 0, 0, -9.81;

	std::vector<boost::shared_ptr<urdf::Link> > links;
	model.getLinks(links);

	boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
	boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);

	if(oldRoot == newRoot || !newRoot)
		process(*oldRoot, 0, Math::SpatialTransform());
	else
		processReverse(*newRoot, 0, NULL, Math::SpatialTransform());

	// Rename the root body to our URDF root
	mBodyNameMap.erase("ROOT");
	mBodyNameMap[root] = 0;

	// Display debug information
#if DISPLAY_DEBUG_INFO
	ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif

	return true;
}

Beispiel #4

Datei: RobotStateAnnotator.cpp Projekt: ahoereth/RobotTrajectoryAnalyzerCpp

  /**
   * TaskSpacePosition annotations.
   *
   * @return UIMA error id. UIMA_ERR_NONE on success.
   */
  uima::TyErrorId annotateTaskSpace(
    uima::CAS& cas,
    const urdf::Model& model
  ) {
    uima::FSIndexRepository& index = cas.getIndexRepository();
    uima::FeatureStructure ts;

    boost::shared_ptr<urdf::Link> link;
    std::vector< boost::shared_ptr<urdf::Link> > links;
    model.getLinks(links);

    for (std::size_t i = 0, size = links.size(); i < size; i++) {
      link = links[i];
      ts = cas.createFS(TaskSpacePosition);
      ts.setFSValue(tsXyzFtr,
        utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
      ts.setFSValue(tsRpyFtr,
        utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
      index.addFS(ts);
    }

    return UIMA_ERR_NONE;
  }