C++ Arm::elbowPosition方法代码示例

//--------------------------------------------------------------
void testApp::recordArmXML (Hand & hand){
	
	if (bRecordingThisFrame){
		Arm arm = hand.arm();
		if (arm.isValid()){
			
			float armWidth = arm.width();
			XML.setValue("AW", armWidth, lastTagNumber);
			
			ofPoint palmPt   = leap.getofPoint ( hand.palmPosition());
			ofPoint wristPt  = leap.getofPoint ( arm.wristPosition());
			ofPoint elbowPt  = leap.getofPoint ( arm.elbowPosition());
			ofPoint palmNorm = leap.getofPoint ( hand.palmNormal());
			
			int palmPtTagNum = XML.addTag("PM");
			XML.setValue("PM:X", palmPt.x, palmPtTagNum);
			XML.setValue("PM:Y", palmPt.y, palmPtTagNum);
			XML.setValue("PM:Z", palmPt.z, palmPtTagNum);
			
			int wristPtTagNum = XML.addTag("W");
			XML.setValue("W:X", wristPt.x, wristPtTagNum);
			XML.setValue("W:Y", wristPt.y, wristPtTagNum);
			XML.setValue("W:Z", wristPt.z, wristPtTagNum);
			
			int elbowPtTagNum = XML.addTag("E");
			XML.setValue("E:X", elbowPt.x, elbowPtTagNum);
			XML.setValue("E:Y", elbowPt.y, elbowPtTagNum);
			XML.setValue("E:Z", elbowPt.z, elbowPtTagNum);
			
			int palmNormTagNum = XML.addTag("PN");
			XML.setValue("PN:X", palmNorm.x, palmNormTagNum);
			XML.setValue("PN:Y", palmNorm.y, palmNormTagNum);
			XML.setValue("PN:Z", palmNorm.z, palmNormTagNum);
			
			
			//---------------
			// Export the hand basis matrix
			Leap::Matrix handMatrix = hand.basis();
			ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis);
			ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis);
			ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis);
			
			int handMatrixTagNum = XML.addTag("HM");
			if( XML.pushTag("HM", handMatrixTagNum) ){
				
				XML.setValue("XX", handBasisX.x, handMatrixTagNum);
				XML.setValue("XY", handBasisX.y, handMatrixTagNum);
				XML.setValue("XZ", handBasisX.z, handMatrixTagNum);
				
				XML.setValue("YX", handBasisY.x, handMatrixTagNum);
				XML.setValue("YY", handBasisY.y, handMatrixTagNum);
				XML.setValue("YZ", handBasisY.z, handMatrixTagNum);
				
				XML.setValue("ZX", handBasisZ.x, handMatrixTagNum);
				XML.setValue("ZY", handBasisZ.y, handMatrixTagNum);
				XML.setValue("ZZ", handBasisZ.z, handMatrixTagNum);
				
				XML.popTag(); // pop HM (Hand Matrix)
			}
			
			//---------------
			// Export the arm basis matrix
			Leap::Matrix armMatrix = arm.basis();
			ofPoint armBasisX = leap.getofPoint( armMatrix.xBasis);
			ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
			ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis);
			
			int armMatrixTagNum = XML.addTag("AM");
			if( XML.pushTag("AM", armMatrixTagNum) ){
				
				XML.setValue("XX", armBasisX.x, armMatrixTagNum);
				XML.setValue("XY", armBasisX.y, armMatrixTagNum);
				XML.setValue("XZ", armBasisX.z, armMatrixTagNum);
				
				XML.setValue("YX", armBasisY.x, armMatrixTagNum);
				XML.setValue("YY", armBasisY.y, armMatrixTagNum);
				XML.setValue("YZ", armBasisY.z, armMatrixTagNum);
				
				XML.setValue("ZX", armBasisZ.x, armMatrixTagNum);
				XML.setValue("ZY", armBasisZ.y, armMatrixTagNum);
				XML.setValue("ZZ", armBasisZ.z, armMatrixTagNum);
				
				XML.popTag(); // pop AM (Arm Matrix)
			}
			
		}
	}
}

void LeapListener::onFrame(const Controller & controller) {
    // Get the most recent frame and report some basic information
    const Frame frame = controller.frame();
    std::cout << "Frame id: " << frame.id()
    << ", timestamp: " << frame.timestamp()
    << ", hands: " << frame.hands().count()
    << ", extended fingers: " << frame.fingers().extended().count()
    << ", tools: " << frame.tools().count()
    << ", gestures: " << frame.gestures().count() << std::endl;

    HandList hands = frame.hands();
    for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
        // Get the first hand
        const Hand hand = *hl;
        std::string handType = hand.isLeft() ? "Left hand" : "Right hand";
        std::cout << std::string(2, ' ') << handType << ", id: " << hand.id()
        << ", palm position: " << hand.palmPosition() << std::endl;
        // Get the hand's normal vector and direction
        const Vector normal = hand.palmNormal();
        const Vector direction = hand.direction();

        // Calculate the hand's pitch, roll, and yaw angles
        std::cout << std::string(2, ' ') <<  "pitch: " << direction.pitch() * RAD_TO_DEG << " degrees, "
        << "roll: " << normal.roll() * RAD_TO_DEG << " degrees, "
        << "yaw: " << direction.yaw() * RAD_TO_DEG << " degrees" << std::endl;

        // Get the Arm bone
        Arm arm = hand.arm();
        std::cout << std::string(2, ' ') <<  "Arm direction: " << arm.direction()
        << " wrist position: " << arm.wristPosition()
        << " elbow position: " << arm.elbowPosition() << std::endl;

        // Get fingers
        const FingerList fingers = hand.fingers();
        for (FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) {
            const Finger finger = *fl;
            std::cout << std::string(4, ' ') <<  fingerNames[finger.type()]
            << " finger, id: " << finger.id()
            << ", length: " << finger.length()
            << "mm, width: " << finger.width() << std::endl;

            // Get finger bones
            for (int b = 0; b < 4; ++b) {
                Bone::Type boneType = static_cast<Bone::Type>(b);
                Bone bone = finger.bone(boneType);
                std::cout << std::string(6, ' ') <<  boneNames[boneType]
                << " bone, start: " << bone.prevJoint()
                << ", end: " << bone.nextJoint()
                << ", direction: " << bone.direction() << std::endl;
            }
        }
    }

    // Get tools
    const ToolList tools = frame.tools();
    for (ToolList::const_iterator tl = tools.begin(); tl != tools.end(); ++tl) {
        const Tool tool = *tl;
        std::cout << std::string(2, ' ') <<  "Tool, id: " << tool.id()
        << ", position: " << tool.tipPosition()
        << ", direction: " << tool.direction() << std::endl;
    }

    // Get gestures
    const GestureList gestures = frame.gestures();
    for (int g = 0; g < gestures.count(); ++g) {
        Gesture gesture = gestures[g];

        switch (gesture.type()) {
            case Gesture::TYPE_CIRCLE: {
                CircleGesture circle = gesture;
                std::string clockwiseness;

                if (circle.pointable().direction().angleTo(circle.normal()) <= PI/2) {
                    clockwiseness = "clockwise";
                } else {
                    clockwiseness = "counterclockwise";
                }

                // Calculate angle swept since last frame
                float sweptAngle = 0;
                if (circle.state() != Gesture::STATE_START) {
                    CircleGesture previousUpdate = CircleGesture(controller.frame(1).gesture(circle.id()));
                    sweptAngle = (circle.progress() - previousUpdate.progress()) * 2 * PI;
                }
                std::cout << std::string(2, ' ')
                << "Circle id: " << gesture.id()
                << ", state: " << stateNames[gesture.state()]
                << ", progress: " << circle.progress()
                << ", radius: " << circle.radius()
                << ", angle " << sweptAngle * RAD_TO_DEG
                <<  ", " << clockwiseness << std::endl;
                break;
            }
            case Gesture::TYPE_SWIPE: {
                SwipeGesture swipe = gesture;
                std::cout << std::string(2, ' ')
                << "Swipe id: " << gesture.id()
                << ", state: " << stateNames[gesture.state()]
                << ", direction: " << swipe.direction()
                << ", speed: " << swipe.speed() << std::endl;
//.........这里部分代码省略.........

//--------------------------------------------------------------
void LeapVisualizer::drawArm (Hand & hand,ofxLeapMotion & leap){
	
	// Draw the wrist and elbow points.
	Arm arm = hand.arm();
	if (arm.isValid()){
		
		ofPoint handPt   = leap.getofPoint ( hand.palmPosition());
		ofPoint handNorm = leap.getofPoint ( hand.palmNormal());
		ofPoint wristPt  = leap.getofPoint ( arm.wristPosition());
		ofPoint elbowPt  = leap.getofPoint ( arm.elbowPosition());
		
		float basisLen = 50.0;
		
		
		if (bDrawSimple){
			ofSetColor(ofColor::white);
			ofDrawSphere(handPt,  8.0);
			ofDrawSphere(wristPt, 8.0);
			ofDrawSphere(elbowPt, 8.0);
			
			ofLine(handPt, wristPt);
			ofLine(wristPt, elbowPt);
			ofLine(handPt, handPt+ basisLen*handNorm);
			ofDrawSphere(handPt+ basisLen*handNorm, 2.0);
			
			// draw the rotation vectors of the hand.
			{
				Leap::Matrix handMatrix = hand.basis();
				ofPoint handBasisX = leap.getofPoint( handMatrix.xBasis);
				ofPoint handBasisY = leap.getofPoint( handMatrix.yBasis);
				ofPoint handBasisZ = leap.getofPoint( handMatrix.zBasis);
				
				glLineWidth(2.0);
				ofSetColor(ofColor::red  );	ofLine(handPt, handPt + basisLen*handBasisX);
				ofSetColor(ofColor::green);	ofLine(handPt, handPt + basisLen*handBasisY);
				ofSetColor(ofColor::blue );	ofLine(handPt, handPt + basisLen*handBasisZ);
				glLineWidth(1.0);
				
				// draw the identity of the hand (left or right)
				string whichHandString = "RIGHT";
				if (hand.isLeft()){
					whichHandString = "LEFT";
				}
				// float handConfidence = hand.confidence();
				// whichHandString += " " + ofToString(handConfidence);
				
				ofSetColor(ofColor::white);
				ofDrawBitmapString(whichHandString, (handPt + (basisLen*1.2)*handBasisY));
			}
			
			// draw the rotation vectors of the arm.
			{
				Leap::Matrix armMatrix = arm.basis();
				ofPoint armBasisX = leap.getofPoint( armMatrix.xBasis);
				ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
				ofPoint armBasisZ = leap.getofPoint( armMatrix.zBasis);
				
				glLineWidth(2.0);
				ofSetColor(ofColor::red  );	ofLine(wristPt, wristPt + basisLen*armBasisX);
				ofSetColor(ofColor::green);	ofLine(wristPt, wristPt + basisLen*armBasisY);
				ofSetColor(ofColor::blue );	ofLine(wristPt, wristPt + basisLen*armBasisZ);
				glLineWidth(1.0);
			}
			
		} else {
			
			// Draw a cylinder between two points, properly oriented in space.
			float armWidth = arm.width();
			float dx = wristPt.x - elbowPt.x;
			float dy = wristPt.y - elbowPt.y;
			float dz = wristPt.z - elbowPt.z;
			float dh = sqrt(dx*dx + dy*dy + dz*dz);
			
			ofPushMatrix();
			{
				ofTranslate( (elbowPt.x+wristPt.x)/2, (elbowPt.y+wristPt.y)/2, (elbowPt.z+wristPt.z)/2 );
				
				float theta =   90 - RAD_TO_DEG * asin(dz/dh);
				float phi   =        RAD_TO_DEG * atan2(dy,dx);
				ofRotate(phi,   0,0,1);
				ofRotate(theta, 0,1,0);
				ofRotate(90,	1,0,0);
				
				// Get the arm Matrix, which provides its orthogonal basis vectors.
				Leap::Matrix armMatrix = arm.basis();
				ofPoint armBasisY = leap.getofPoint( armMatrix.yBasis);
				float ax = armBasisY.x;
				float ay = armBasisY.y;
				float az = armBasisY.z;
				
				// Compute the longitudinal rotation of the arm.
				// Sheesh, I really need to learn 3D matrix math.
				ofNode armBasisYNode;
				armBasisYNode.setPosition(armBasisY);
				armBasisYNode.rotateAround(0-   phi, ofVec3f(0,0,1), ofVec3f(0,0,0));
				armBasisYNode.rotateAround(0- theta, ofVec3f(0,1,0), ofVec3f(0,0,0));
				armBasisYNode.rotateAround(0-    90, ofVec3f(1,0,0), ofVec3f(0,0,0));
				ofPoint newArmBasisY = armBasisYNode.getPosition();
				float armRotation = RAD_TO_DEG * atan2f(newArmBasisY.z, newArmBasisY.x);
//.........这里部分代码省略.........

/* ----------------------------------------------------------------------------------------- */
void CListener::onFrame(const Controller& controller) {
  usleep(timeout);        // timeout to move the arm close to real time

  // Get the most recent frame and report some basic information
  const Frame frame = controller.frame();

  if(frame.hands().count() == 0)    // do nothing if no hand is involved
    return;

  if(frame.hands().count() > 1) {     // do nothing if more than one hand is involved
    //std::cout << "More than one hand detected, returning to center position" << std::endl;
    //rarm.set_to_mid();
    return;
  }
  
  // ------------------------ MAIN LOGIC
  // ------------------------
  // GRIPPER
  new_pos[GRIPPER] = Robot_arm::ranges[GRIPPER][MAX] - frame.fingers().extended().count() * Robot_arm::gripper_offset;  // move GRIPPER in safe ranges
  //std::cout <<  "new_pos[GRIPPER]: " << new_pos[GRIPPER] << std::endl;

  HandList hands = frame.hands();
  for (HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) {
    // Get the first hand
    const Hand hand = *hl;
    Arm arm = hand.arm();
    const Vector direction = hand.direction();
    const Vector normal = hand.palmNormal();

    // ------------------------------
    // ELBOW
    new_pos[ELBOW] = Robot_arm::ranges[ELBOW][MAX] - int(Robot_arm::elbow_offset * hand.palmPosition()[Y]);
    //std::cout <<  "new_pos[ELBOW]: " << new_pos[ELBOW] << std::endl;


    // ------------------------------
    // BASE
    if(arm.direction()[X] < 0)
      new_pos[BASE] = Robot_arm::ranges[BASE][CENTER] - int(Robot_arm::base_offset * fabs(arm.direction()[X]));
    else
      new_pos[BASE] = Robot_arm::ranges[BASE][CENTER] + int(Robot_arm::base_offset * fabs(arm.direction()[X]));
    //std::cout <<  "new_pos[BASE]: " << new_pos[BASE] << std::endl;
 

    // ------------------------------
    // WRIST ROTATE
    if(direction.pitch() * RAD_TO_DEG < 0)
      new_pos[WRIST_ROTATE] = Robot_arm::ranges[WRIST_ROTATE][CENTER] - int(Robot_arm::wrist_rotate_offset * fabs(direction.pitch() * RAD_TO_DEG));
    else
      new_pos[WRIST_ROTATE] = Robot_arm::ranges[WRIST_ROTATE][CENTER] + int(Robot_arm::wrist_rotate_offset * fabs(direction.pitch() * RAD_TO_DEG));
    //std::cout <<  "new_pos[WRIST_ROTATE]: " << new_pos[WRIST_ROTATE] << std::endl;


    // ------------------------------
    // WRIST
    if(normal.roll() * RAD_TO_DEG < 0)
      new_pos[WRIST] = Robot_arm::ranges[WRIST][CENTER] + int(Robot_arm::wrist_offset * fabs(normal.roll() * RAD_TO_DEG));
    else
      new_pos[WRIST] = Robot_arm::ranges[WRIST][CENTER] - int(Robot_arm::wrist_offset * fabs(normal.roll() * RAD_TO_DEG));
    //std::cout <<  "new_pos[WRIST]: " << new_pos[WRIST] << std::endl;


    // ------------------------------
    // SHOULDER
    new_pos[SHOULDER] = Robot_arm::ranges[SHOULDER][MIN] + int(Robot_arm::shoulder_offset * arm.elbowPosition()[Z]);
    //std::cout <<  "new_pos[SHOULDER]: " << new_pos[SHOULDER] << std::endl;
  }

  // filtration
  if(filter != 0) {
    int * tmp_pos = rarm.get_pos();

    for(int i = 0; i < Robot_arm::num_servos; i++) {
      if(fabs((tmp_pos[i] - new_pos[i]) / (double)filter_const) > filter) {      // move only if greater than filter
        rarm.move(new_pos);
        break;
      }
    }
    return;
  }

  rarm.move(new_pos);

}