C++ Joint::getName方法代码示例

bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
    // clear chain
    chain = Chain();

    // walk down from chain_root and chain_tip to the root of the tree
    vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
    for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
        parents_chain_root.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
    for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
        parents_chain_tip.push_back(s->first);
        if (s->first == root_name) break;
    }
    if (parents_chain_tip.empty() || parents_chain_tip.back()  != root_name) return false;

    // remove common part of segment lists
    SegmentMap::key_type last_segment = root_name;
    while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
           parents_chain_root.back() == parents_chain_tip.back()){
        last_segment = parents_chain_root.back();
        parents_chain_root.pop_back();
        parents_chain_tip.pop_back();
    }
    parents_chain_root.push_back(last_segment);


    // add the segments from the root to the common frame
    for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
        Segment seg = getSegment(parents_chain_root[s])->second.segment;
        Frame f_tip = seg.pose(0.0).Inverse();
        Joint jnt = seg.getJoint();
        if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
            jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
        else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
            jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
        chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
                                 jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
    }

    // add the segments from the common frame to the tip frame
    for (int s=parents_chain_tip.size()-1; s>-1; s--){
        chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
    }
    return true;
}

void check_self_consistency(SkeletonPtr skeleton)
{
  for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = skeleton->getBodyNode(i);
    EXPECT_TRUE(bn->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn);

    Joint* joint = bn->getParentJoint();
    EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint);

    for(size_t j=0; j<joint->getNumDofs(); ++j)
    {
      DegreeOfFreedom* dof = joint->getDof(j);
      EXPECT_TRUE(dof->getIndexInJoint() == j);
      EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
    }
  }

  for(size_t i=0; i<skeleton->getNumDofs(); ++i)
  {
    DegreeOfFreedom* dof = skeleton->getDof(i);
    EXPECT_TRUE(dof->getIndexInSkeleton() == i);
    EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof);
  }
}

/**
 * Perform set up functions after model has been deserialized or copied.
 *
 * @param aEngine dynamics engine containing this dof.
 * @param aJoint joint containing this dof.
 */
void TransformAxis::connectToJoint(const Joint& aJoint)
{
    string errorMessage;

    _joint = &aJoint;

    // Look up the coordinates by name.
    const Property<string>& coordNames = getProperty_coordinates();
    int nc = coordNames.size();
    const auto& coords = _joint->getProperty_coordinates();

    if (!hasFunction()) {
        SimTK_ASSERT2_ALWAYS(coordNames.size() == 0,
            "CustomJoint (%s) %s axis has no function but has coordinates.",
            _joint->getName().c_str(), getName().c_str());
        return;
    }

    for(int i=0; i< nc; ++i) {
        if (coords.findIndexForName( coordNames[i] ) < 0) {
            errorMessage += "Invalid coordinate (" 
                            + coordNames[i] 
                            + ") specified for TransformAxis " 
                            + getName() + " in joint " + aJoint.getName();
            throw (Exception(errorMessage));
        }
    }
}

/**
	HACK!
*/
void Character::updateJointOrdering(){
	if( jointOrder.size() == joints.size() ) 
		return; // HACK assume ordering is ok
	jointOrder.clear();

	if (!root)
		return;
	DynamicArray<ArticulatedRigidBody*> bodies;
	bodies.push_back(root);

	int currentBody = 0;

	while ((uint)currentBody<bodies.size()){
		//add all the children joints to the list
		for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){
			Joint *j = bodies[currentBody]->getChildJoint(i);
			jointOrder.push_back( getJointIndex(j->getName()) );
			bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild());
		}
		currentBody++;
	}

	reverseJointOrder.resize( jointOrder.size() );
	for( uint i=0; i < jointOrder.size(); ++i )
		reverseJointOrder[jointOrder[i]] = i;

}

void Skeleton::insertJoint(string parentJointName, Joint& newJoint) {

	_jointsNames.push_back(QString(newJoint.getName().c_str()));
	_joints[newJoint.getName()] = newJoint;

	//--- Inserting non-root, update children list from parent joint
	if (!parentJointName.empty()) {

		_joints[newJoint.getName()].setParentJointName(parentJointName);
		_joints[parentJointName].insertChildJointName(newJoint.getName());

	} else {
		//--- Just one root Joint
		_rootJointName = newJoint.getName();

	}

}

void PlaneRegistration::init_kdl_robot()
{
  std::string urdf;
  node_->param("/robot_description", urdf, std::string());
  
  urdf::Model urdf_model;
  urdf_model.initString(urdf);
  
  // get tree from urdf string
  KDL::Tree my_tree;
  if (!kdl_parser::treeFromString(urdf, my_tree)) {
    ROS_ERROR("Failed to construct kdl tree");
    return;
  }   
  std::string rootLink = "wam/base_link";
  std::string tipLink = "wam/cutter_tip_link";
  if (!my_tree.getChain(rootLink, tipLink, robot_))
    {   
      ROS_ERROR("Failed to get chain from kdl tree, check root/rip link");
      return;
    }   
  num_jnts_ = robot_.getNrOfJoints();
  
  // resize joint states
  jnt_pos_.resize(num_jnts_);
  jnt_vel_.resize(num_jnts_);
  //    jnt_cmd_.resize(num_jnts_);
  jnt_limit_min_.resize(num_jnts_);
  jnt_limit_max_.resize(num_jnts_);
  
  // get jnt limits from URDF model
  size_t i_jnt = 0;
  for (size_t i = 0; i < robot_.getNrOfSegments(); i++) {
    Joint jnt = robot_.getSegment(i).getJoint();
    if (jnt.getType() != Joint::None) {
      jnt_limit_min_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->lower;
      jnt_limit_max_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->upper;
      i_jnt++;
    }
  }       
  
  // print limit for debugging
  std::cout << "min: " << jnt_limit_min_.data.transpose() << std::endl;
  std::cout << "max: " << jnt_limit_max_.data.transpose() << std::endl;
  
  // KDL Solvers
  fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(robot_));
  ik_solver_.reset(
		   new KDL::ChainIkSolverPos_LMA(
						 robot_,
						 1E-5,
						 500,
						 1E-15));
}

int Skeleton::getJointIndex(const std::string& _name) const
{
    const int nJoints = getNumJoints();

    for(int i = 0; i < nJoints; i++)
    {
        Joint* node = getJoint(i);

        if (_name == node->getName())
            return i;
    }

    return -1;
}

//==============================================================================
void ReferentialSkeleton::unregisterJoint(BodyNode* _child)
{
  if(nullptr == _child)
  {
    dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister "
          << "a Joint from a nullptr BodyNode. This is most likely a bug. "
          << "Please report this!\n";
    assert(false);
    return;
  }

  Joint* joint = _child->getParentJoint();

  std::unordered_map<const BodyNode*, IndexMap>::iterator it =
      mIndexMap.find(_child);

  if( it == mIndexMap.end() || INVALID_INDEX == it->second.mJointIndex)
  {
    dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister "
          << "a Joint named [" << joint->getName() << "] (" << joint << "), "
          << "which is the parent Joint of BodyNode [" << _child->getName()
          << "] (" << _child << "), but the Joint is not currently in this "
          << "ReferentialSkeleton! This is most likely a bug. Please report "
          << "this!\n";
    assert(false);
    return;
  }

  std::size_t jointIndex = it->second.mJointIndex;
  mJoints.erase(mJoints.begin() + jointIndex);
  it->second.mJointIndex = INVALID_INDEX;

  for(std::size_t i = jointIndex; i < mJoints.size(); ++i)
  {
    // Re-index all of the Joints in this ReferentialSkeleton which came after
    // the Joint that was removed.
    JointPtr alteredJoint = mJoints[i];
    IndexMap& indexing = mIndexMap[alteredJoint.getBodyNodePtr()];
    indexing.mJointIndex = i;
  }

  if(it->second.isExpired())
    mIndexMap.erase(it);

  // Updating the caches isn't necessary after unregistering a joint right now,
  // but it might matter in the future, so it might be better to be safe than
  // sorry.
  updateCaches();
}

Joint * BlueprintInstance::detachJoint(unsigned int i)
{
    assert(i < mJointList.size());
    Joint * detached = mJointList[i];

    // detach from list
    mJointList.erase(mJointList.begin() + i);

    // detach from map
    map<string, Joint*>::iterator iter =
        mJointMap.find(detached->getName());
    mJointMap.erase(iter);

    return detached;
}

/* normal construction test */
void JointTest::normalConstruction() {

    int id = 1;
    std::string name = "test";
    float mass = 2;
    float length = 3;

    using namespace RoboticArm;

    Joint createdPart = Joint(id, name, mass, length);

    if (createdPart.getId() == id && createdPart.getName() == name &&
            createdPart.getLength() == length && createdPart.getMass() == mass) {
        CPPUNIT_ASSERT(true);
    } else {
        CPPUNIT_ASSERT(false);
    }

}

void JointTest::validParReadBack(){
    
    bool success = true;

    int id = 111;
    std::string name = "test";
    float mass = 123.54566;
    float length = 14676.54;

    using namespace RoboticArm;

    try {
        Joint createdPart = Joint(id, name, mass, length);

        if (id != createdPart.getId()) {
            std::cout << "ID is wrong." << std::endl;
            success = false;
        }

        if (name != createdPart.getName()) {
            std::cout << "Name is wrong." << std::endl;
            success = false;
        }

        if (mass != createdPart.getMass()) {
            std::cout << "Mass is wrong." << std::endl;
            success = false;
        }

        if (length != createdPart.getLength()) {
            std::cout << "Length is wrong." << std::endl;
            success = false;
        }


    } catch (const std::invalid_argument& ia) {
        success = false;
    }

    CPPUNIT_ASSERT(success);

}

//---Preorder
void Skeleton::printJoint(Joint& joint, int depth) {
	string logOutput = "";

	for (int i = 0; i < depth; i++) {
		logOutput += "\t";
	}
	logOutput += joint.getName();
	logOutput += "\n";
	for (int i = 0; i < depth; i++) {
		logOutput += "\t";
	}
	logDEBUG("%s(%d,%d,%d)",logOutput.c_str());

	vector<string> childrenJointsNames = joint.getChildrenJointsNames();

	for (unsigned int i = 0; i < childrenJointsNames.size(); i++) {
		printJoint(getJoint(childrenJointsNames.at(i)), depth + 1);
	}

}

/**
 * Creates skeleton object in XML.
 **/
void IO::saveSkeleton(TiXmlElement *parent, Skeleton *s, Mesh *m)
{
	vector<Joint *> *joints = s->getJoints();
	vector<Bone *> *bones = s->getBones();

	if (joints->empty() && bones->empty())
		return;

	TiXmlElement *skeletonXML = new TiXmlElement("skeleton");
	parent->LinkEndChild(skeletonXML);

	if (!joints->empty())
	{
		TiXmlElement *jointsXML = new TiXmlElement("joints");
		skeletonXML->LinkEndChild(jointsXML);

		vector<Joint *>::iterator i = joints->begin();
		for(; i < joints->end(); i++)
		{
			Joint *j = *i;
			TiXmlElement *jointXML = new TiXmlElement("joint");
			const char *name = j->getName();
			if (name[0] != 0) // save name only for named joints
				jointXML->SetAttribute("name", name);
			jointXML->SetDoubleAttribute("x", j->x);
			jointXML->SetDoubleAttribute("y", j->y);
			jointXML->SetAttribute("fixed", j->fixed);
			jointXML->SetAttribute("selected", j->selected);
			jointXML->SetAttribute("osc", j->osc);

			jointsXML->LinkEndChild(jointXML);
		}
	}

	if (!bones->empty())
	{
		saveBones(skeletonXML, bones, joints, m->getVertices());
	}
}

//==============================================================================
void Controller::printDebugInfo() const
{
  std::cout << "[ATLAS Robot]" << std::endl
            << " NUM NODES : " << mAtlasRobot->getNumBodyNodes() << std::endl
            << " NUM DOF   : " << mAtlasRobot->getNumDofs() << std::endl
            << " NUM JOINTS: " << mAtlasRobot->getNumBodyNodes() << std::endl;

  for (std::size_t i = 0; i < mAtlasRobot->getNumBodyNodes(); ++i)
  {
    Joint* joint = mAtlasRobot->getJoint(i);
    BodyNode* body = mAtlasRobot->getBodyNode(i);
    BodyNode* parentBody = mAtlasRobot->getBodyNode(i)->getParentBodyNode();

    std::cout << "  Joint [" << i << "]: " << joint->getName() << " ("
              << joint->getNumDofs() << ")" << std::endl;
    if (parentBody != nullptr)
    {
      std::cout << "    Parent body: " << parentBody->getName() << std::endl;
    }

    std::cout << "    Child body : " << body->getName() << std::endl;
  }
}

COLLADASW::Joint KDLColladaLibraryJointsExporter::makeColladaSWJoint(COLLADASW::StreamWriter* streamWriter, Joint& kdlJoint, string uniqueId)
{


    string jointName = kdlJoint.getName();

    if (jointName == kdlDefaultJointName)
        jointName = uniqueId;
    Joint::JointType jointType = kdlJoint.getType();
    Vector jointAxis = kdlJoint.JointAxis();


    COLLADAFW::JointPrimitive::Type type;
    switch (jointType)
    {
        case Joint::RotAxis :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotX :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotY :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }
        case Joint::RotZ :
        {
            type = COLLADAFW::JointPrimitive::REVOLUTE;
            break;
        }

        case Joint::TransAxis :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransX :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransY :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        case Joint::TransZ :
        {
            type = COLLADAFW::JointPrimitive::PRISMATIC;
            break;
        }

        default :
            LOG(ERROR) << "Unknown joint type: " << kdlJoint.getTypeName() << ", changing to revolute type with no axis.";
            type = COLLADAFW::JointPrimitive::REVOLUTE;
    }

    COLLADABU::Math::Vector3 axis(jointAxis.x(), jointAxis.y(), jointAxis.z());

    COLLADAFW::JointPrimitive jointPrimitive(COLLADAFW::UniqueId(uniqueId), type);
    jointPrimitive.setAxis(axis);

   // jointPrimitive.setHardLimitMax(10);
   // jointPrimitive.setHardLimitMin(-10);


    COLLADASW::Joint colladaJoint(streamWriter, jointPrimitive, uniqueId, jointName); //TODO make sure that ID is unique


//    kdlJoint.name = uniqueId;

    LOG(INFO) << "Joint id: " << colladaJoint.getJointId();
    LOG(INFO) << "Joint name: " << colladaJoint.getJointName();


    return colladaJoint;
}