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

int w_Joint_getType(lua_State *L)
{
	Joint *t = luax_checkjoint(L, 1);
	const char *type = "";
	Joint::getConstant(t->getType(), type);
	lua_pushstring(L, type);
	return 1;
}

void Skeleton::setJoint(Joint joint) {
	string jointType = joint.getType();

	if (jointType == "ThumbRight") {
		setThumbRight(joint);
	} else if (jointType == "SpineBase") {
		setSpineBase(joint);
	} else if (jointType == "SpineMid") {
		setSpineMid(joint);
	} else if (jointType == "Neck") {
		setNeck(joint);
	} else if (jointType == "Head") {
		setHead(joint);
	} else if (jointType == "ShoulderLeft") {
		setShoulderLeft(joint);
	} else if (jointType == "ElbowLeft") {
		setElbowLeft(joint);
	} else if (jointType == "WristLeft") {
		setWristLeft(joint);
	} else if (jointType == "HandLeft") {
		setHandLeft(joint);
	} else if (jointType == "ShoulderRight") {
		setShoulderRight(joint);
	} else if (jointType == "ElbowRight") {
		setElbowRight(joint);
	} else if (jointType == "WristRight") {
		setWristRight(joint);
	} else if (jointType == "HandRight") {
		setHandRight(joint);
	} else if (jointType == "HipLeft") {
		setHipLeft(joint);
	} else if (jointType == "KneeLeft") {
		setKneeLeft(joint);
	} else if (jointType == "AnkleLeft") {
		setAnkleLeft(joint);
	} else if (jointType == "FootLeft") {
		setFootLeft(joint);
	} else if (jointType == "HipRight") {
		setHipRight(joint);
	} else if (jointType == "KneeRight") {
		setKneeRight(joint);
	} else if (jointType == "AnkleRight") {
		setAnkleRight(joint);
	} else if (jointType == "FootRight") {
		setFootRight(joint);
	} else if (jointType == "SpineShoulder") {
		setSpineShoulder(joint);
	} else if (jointType == "HandTipLeft") {
		setHandTipLeft(joint);
	} else if (jointType == "ThumbLeft") {
		setThumbLeft(joint);
	} else if (jointType == "HandTipRight") {
		setHandTipRight(joint);
	}
}

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

void KinFamTest::JointTest()
{
    double q;
    Joint j;
    j=Joint("Joint 1", Joint::None);
    CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
    random(q);
    j=Joint("Joint 2", Joint::RotX);
    CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
    random(q);
    j=Joint("Joint 3", Joint::RotY);
    CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
    random(q);
    j=Joint("Joint 4", Joint::RotZ);
    CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
    random(q);
    j=Joint("Joint 5", Joint::TransX);
    CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
    random(q);
    j=Joint("Joint 6", Joint::TransY);
    CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
    random(q);
    j=Joint("Joint 7", Joint::TransZ);
    CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
    CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
    random(q);
    CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));

}

/*!
  Computes the jacobian for the base frame of link \a l on finger \a f
  relative to the base frame of the finger. This is wrt THE FRAME OF LINK!!!
  
  Distances USED TO BE converted to meters (check why this is...). Now they are
  kept in MILLIMETERS!
*/
double *
Grasp::getLinkJacobian(int f, int l)
{
  int j,col;
  Joint *jointPtr;
  int numDOF = hand->getNumDOF();
  double *jac = new double[6*numDOF];
  double k;
  mat3 m;
  vec3 p;
  transf T;
  double db0 = 0.0;

  dcopy(6*numDOF,&db0,0,jac,1);
  
  //I use f=-1 on virtual contacts to show that a contact is on the palm
  if (f < 0) return jac;

  for (j=hand->getFinger(f)->getLastJoint(l);j>=0;j--) {
    jointPtr = hand->getFinger(f)->getJoint(j);
    col = jointPtr->getDOFNum();
    
	k = hand->getDOF(jointPtr->getDOFNum())->getStaticRatio(jointPtr);
	T = T * jointPtr->getDH()->getTran();
    m = T.affine();
    p = T.translation();
    
    if (jointPtr->getType() == REVOLUTE) {
      jac[col*6]   += k*(-m.element(0,0)*p.y() + m.element(0,1)*p.x());
      jac[col*6+1] += k*(-m.element(1,0)*p.y() + m.element(1,1)*p.x());
      jac[col*6+2] += k*(-m.element(2,0)*p.y() + m.element(2,1)*p.x());
      jac[col*6+3] += k*m.element(0,2);
      jac[col*6+4] += k*m.element(1,2);
      jac[col*6+5] += k*m.element(2,2);
    } else {
      jac[col*6]   += k*m.element(0,2);
      jac[col*6+1] += k*m.element(1,2);
      jac[col*6+2] += k*m.element(2,2);
      jac[col*6+3] += 0.0;
      jac[col*6+4] += 0.0;
      jac[col*6+5] += 0.0;
    }
  }
  return jac;
}

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

//.........这里部分代码省略.........

				// Find out whether the Simulator has static-to-sleeping
				// contacts disabled.
				bool ignoreStaticSleepingContacts =
				    !sim->areStaticSleepingContactsEnabled();

				// Get pointers to the geoms' Solids.
				Solid* solid0 = geomData0->solid;
				Solid* solid1 = geomData1->solid;

				// Get pointers to the two Solids' CollisionEventHandlers.
				// These will be NULL if the Solids don't use
				// CollisionEventHandlers.
				CollisionEventHandler* handler0 =
				    solid0->getCollisionEventHandler();
				CollisionEventHandler* handler1 =
				    solid1->getCollisionEventHandler();

				bool neitherHasCollisionHandler = !(handler0 || handler1);

				bool noGlobalCollisionHandlers = 
					sim->getNumGlobalCollisionEventHandlers() == 0;

				// Now do the actual tests to see if we should return early.
				// It is important here that we don't call dBodyIsEnabled on
				// a static body because that crashes ODE.

				bool case1 = neitherHasCollisionHandler && 
					noGlobalCollisionHandlers && solid0Static && solid1Static;
				//bool case2= o0BodyID == o1BodyID;
				bool case3 = bothHaveBodies && !dBodyIsEnabled(o0BodyID)
					&& !dBodyIsEnabled(o1BodyID);
				bool case4 = commonJoint &&
					commonJoint->getType() == FIXED_JOINT;
				bool case5 = commonJoint
					&& !commonJoint->areContactsEnabled();
				bool case6 = solid0Static && 0 != o1BodyID
					&& !dBodyIsEnabled(o1BodyID)
					&& ignoreStaticSleepingContacts
					&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
				bool case7 = solid1Static && 0 != o0BodyID
					&& !dBodyIsEnabled(o0BodyID)
					&& ignoreStaticSleepingContacts
					&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
				bool case8 = !makeContacts && neitherHasCollisionHandler
					&& noGlobalCollisionHandlers;

				if (case1 || case3 || case4 || case5 || case6 || case7 || case8)
				{
					return;
				}

				// Now actually test for collision between the two geoms.
				// This is one of the more expensive operations.
				dWorldID theWorldID = sim->internal_getWorldID();
				dJointGroupID theJointGroupID =
				    sim->internal_getJointGroupID();
				dContactGeom contactArray[globals::maxMaxContacts];
				int numContacts = dCollide(o0, o1, sim->getMaxContacts(),
					contactArray, sizeof(dContactGeom));

				// If the two objects didn't make any contacts, they weren't
				// touching, so just return.
				if (0 == numContacts)
				{
					return ;

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