bool updateModule() {
if (firstRun) {
init();
firstRun = false;
}
// get a target object position from a YARP port
Bottle *b = inPort.read();
if (b != NULL) {
Vector xd(3);
bool f;
xd[0] = b->get(0).asDouble();
xd[1] = b->get(1).asDouble();
xd[2] = b->get(2).asDouble();
// apply systematic offset
// due to uncalibrated kinematic
xd = xd + dOffs;
// safe thresholding
xd[0] = xd[0] > -0.1 ? -0.1 : xd[0];
// grasp (wait until it's done)
action->grasp(xd, graspOrien, graspDisp);
action->checkActionsDone(f, true);
action->areFingersInPosition(f);
// if fingers are not in position,
// it's likely that we've just grasped
// something, so lift it up!
if (!f) {
cout << "Wow, got something!" << endl;
// lift the object (wait until it's done)
action->pushAction(xd + dLift, graspOrien);
action->checkActionsDone(f, true);
}
else
cout << "Sorry :( ... nothing to grasp" << endl;
// release the object or just open the
// hand (wait until it's done)
action->pushAction("open_hand");
action->checkActionsDone(f, true);
// wait until it's done, since
// use two arms that shares the torso
action->pushAction(home_x);
action->checkActionsDone(f, true);
// let the hand wave a bit around home position
// the waving will be disabled before commencing
// a new action
action->enableArmWaving(home_x);
}
return true;
}
void init() {
bool f;
action->pushAction(home_x, "open_hand");
action->checkActionsDone(f, true);
action->enableArmWaving(home_x);
}