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);
}
bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
Property config; config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option(bGeneral.toString().c_str());
option.put("local",name.c_str());
option.put("part","left_arm");
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for left arm"<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
// check whether the grasp model is calibrated,
// otherwise calibrate it
Model *model; action->getGraspModel(model);
if (model!=NULL)
{
if (!model->isCalibrated())
{
Property prop;
prop.put("finger","all");
model->calibrate(prop);
}
}
return true;
}
bool interruptModule() {
// since a call to checkActionsDone() blocks
// the execution until it's done, we need to
// take control and exit from the waiting state
action->syncCheckInterrupt(true);
inPort.interrupt();
rpcPort.interrupt();
return true;
}
virtual bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
string partUsed=rf.find("part").asString().c_str();
if ((partUsed!="left_arm") && (partUsed!="right_arm"))
{
cout<<"Invalid part requested!"<<endl;
return false;
}
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option;
for (int i=1; i<bGeneral.size(); i++)
{
Bottle *pB=bGeneral.get(i).asList();
if (pB->size()==2)
option.put(pB->get(0).asString().c_str(),pB->get(1));
else
{
cout<<"Error: invalid option!"<<endl;
return false;
}
}
option.put("local",name.c_str());
option.put("part",rf.find("part").asString().c_str());
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for "<<partUsed<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
openPorts=true;
// check whether the grasp model is calibrated,
// otherwise calibrate it and save the results
Model *model;
action->getGraspModel(model);
if (!model->isCalibrated())
{
Property prop("(finger all)");
model->calibrate(prop);
ofstream fout;
fout.open(option.find("grasp_model_file").asString().c_str());
model->toStream(fout);
fout.close();
}
return true;
}