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;
break;
}
case Gesture::TYPE_KEY_TAP: {
KeyTapGesture tap = gesture;
std::cout << std::string(2, ' ')
<< "Key Tap id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", position: " << tap.position()
<< ", direction: " << tap.direction()<< std::endl;
break;
}
case Gesture::TYPE_SCREEN_TAP: {
ScreenTapGesture screentap = gesture;
std::cout << std::string(2, ' ')
<< "Screen Tap id: " << gesture.id()
<< ", state: " << stateNames[gesture.state()]
<< ", position: " << screentap.position()
<< ", direction: " << screentap.direction() << std::endl;
break;
}
default:
std::cout << std::string(2, ' ') << "Unknown gesture type." << std::endl;
break;
}
}
if (!frame.hands().isEmpty() || !gestures.isEmpty())
std::cout << 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);
}