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

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

}