bool learnPrimitive::updatePrimitive(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bPrimitiveAction = rf.findGroup("Primitive_Action");
if (!bPrimitiveAction.isNull())
{
Bottle * bPrimitiveActionName = bPrimitiveAction.find("primitiveActionName").asList();
Bottle * bPrimitiveActionArg = bPrimitiveAction.find("primitiveActionArg").asList();
if(bPrimitiveActionName->isNull() || bPrimitiveActionArg->isNull()){
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf is null : primitiveActionName, primitiveActionArg" ;
return false ;
}
int primitiveActionSize = -1 ;
if(bPrimitiveActionName->size() == bPrimitiveActionArg->size()){
primitiveActionSize = bPrimitiveActionName->size() ;
} else {
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf has different size!" ;
return false ;
}
if(primitiveActionSize == 0) {
yWarning() << " [updatePrimitiveAction] : there is no primitiveAction defined at startup!" ;
} else {
for(int i = 0; i < primitiveActionSize ; i++) {
//insert protoaction even if already there
Bottle bPrimitive;
string currentPrimName = bPrimitiveActionName->get(i).asString();
string currentPrimArg = bPrimitiveActionArg->get(i).asString();
bPrimitive.addString(currentPrimName);
bPrimitive.addString(currentPrimArg);
string concat = currentPrimName + "_" + currentPrimArg;
Bottle bCurrentPrim = rf.findGroup(concat);
if(bCurrentPrim.isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListProtoAction = bCurrentPrim.find("actionList").asList();
if(bListProtoAction->isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bPrimitive.addList() = *bListProtoAction ;
yInfo() << "Primitive Action added : (" <<bPrimitive.get(0).asString() << ", " << bPrimitive.get(1).asString() << ", ( " << bPrimitive.get(2).asList()->toString()<< "))" ;
vPrimitiveActionBottle.push_back(bPrimitive);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool configure(ResourceFinder &rf)
{
string robot=rf.check("robot",Value("icub")).asString();
string arm=rf.check("arm",Value("left")).asString();
string eye=rf.check("eye",Value("left")).asString();
bool analog=(rf.check("analog",Value(robot=="icub"?"on":"off")).asString()=="on");
if ((arm!="left") && (arm!="right"))
{
yError()<<"Invalid arm requested";
return false;
}
if ((eye!="left") && (eye!="right"))
{
yError()<<"Invalid eye requested";
return false;
}
// open drivers
if (analog)
{
Property optionAnalog("(device analogsensorclient)");
optionAnalog.put("remote","/"+robot+"/"+arm+"_hand/analog:o");
optionAnalog.put("local","/show-fingers/analog");
if (!drvAnalog.open(optionAnalog))
{
yError()<<"Analog sensor not available";
terminate();
return false;
}
}
Property optionArm("(device remote_controlboard)");
optionArm.put("remote","/"+robot+"/"+arm+"_arm");
optionArm.put("local","/show-fingers/joints");
if (!drvArm.open(optionArm))
{
yError()<<"Joints arm controller not available";
terminate();
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote","/"+robot+"/cartesianController/"+arm+"_arm");
optionCart.put("local","/show-fingers/cartesian");
if (!drvCart.open(optionCart))
{
yError()<<"Cartesian arm controller not available";
terminate();
return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local","/show-fingers/gaze");
if (!drvGaze.open(optionGaze))
{
yError()<<"Gaze controller not available";
terminate();
return false;
}
if (analog)
drvAnalog.view(ianalog);
else
ianalog=NULL;
IControlLimits *ilim;
drvArm.view(iencs);
drvArm.view(ilim);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
finger[0]=iCubFinger(arm+"_thumb");
finger[1]=iCubFinger(arm+"_index");
finger[2]=iCubFinger(arm+"_middle");
deque<IControlLimits*> lim;
lim.push_back(ilim);
for (int i=0; i<3; i++)
finger[i].alignJointsBounds(lim);
yInfo()<<"Using arm="<<arm<<" and eye="<<eye;
imgInPort.open("/show-fingers/img:i");
imgOutPort.open("/show-fingers/img:o");
camSel=(eye=="left")?0:1;
return true;
}
void setInputPort(ResourceFinder *rf, int index, ConstString local)
{
ConstString **remote;
ConstString *localTmp;
bool connectionDone = false;
remote = new ConstString*[nPlots];
//local/remote port connection
if(rf->find("remote").isString())
{
nPortInputsInPlots[index] = 1;
remote[index] = new ConstString[nPortInputsInPlots[index]];
remote[index][0] = resFind->find("remote").toString();
plot[index]->setInputPortName(remote[index][0].c_str());
//connect
plot[index]->setPorts(&local, nPortInputsInPlots[index]);
yarpConnectRemoteLocal(remote[index][0], local);
connectionDone = true;
}
else if(rf->find("remote").isList())
{
Bottle *rrBot;
//if (VERBOSE) fprintf(stderr, "MESSAGE: Option remote is a list\n");
rrBot = resFind->find("remote").asList();
int listSize = rrBot->size();
if (rrBot->get(index).isString() && listSize==1)
{
nPortInputsInPlots[index] = 1;
remote[index] = new ConstString[nPortInputsInPlots[index]];
remote[index][0] = rrBot->get(0).toString();
plot[index]->setInputPortName(remote[index][0].c_str());
//connect
plot[index]->setPorts(&local, nPortInputsInPlots[index]);
yarpConnectRemoteLocal(remote[index][0], local);
connectionDone = true;
}
if (rrBot->get(index).isString() && listSize==nPlots)
{
nPortInputsInPlots[index] = 1;
remote[index] = new ConstString[nPortInputsInPlots[index]];
remote[index][0] = rrBot->get(index).toString();
plot[index]->setInputPortName(remote[index][0].c_str());
//connect
plot[index]->setPorts(&local, nPortInputsInPlots[index]);
yarpConnectRemoteLocal(remote[index][0], local);
connectionDone = true;
}
if (rrBot->get(index).isList() && listSize==nPlots)
{
//if (VERBOSE) fprintf(stderr, "MESSAGE: Getting a double list of remote ports! \n");
Bottle *rrrBot;
rrrBot = rrBot->get(index).asList();
nPortInputsInPlots[index] = rrrBot->size();
remote[index] = new ConstString[nPortInputsInPlots[index]];
localTmp = new ConstString[nPortInputsInPlots[index]];
for (int j = 0; j < nPortInputsInPlots[index]; j++)
{
char stringN[64];
sprintf(stringN, "/input%d", j);
ConstString sN(stringN);
remote[index][j] = rrrBot->get(j).toString();
localTmp[j] = sN;
localTmp[j] = localTmp[j] + local;
}
ConstString sumRemote = remote[index][0];
sumRemote = sumRemote + " ";
for (int j = 1; j < nPortInputsInPlots[index]; j++)
{
sumRemote = sumRemote + remote[index][j];
sumRemote = sumRemote + " ";
}
plot[index]->setInputPortName(sumRemote.c_str());
//connect
plot[index]->setPorts(localTmp, nPortInputsInPlots[index]);
for (int j = 0; j < nPortInputsInPlots[index]; j++)
yarpConnectRemoteLocal(remote[index][j], localTmp[j]);
connectionDone = true;
}
}
if (!connectionDone)
fprintf(stderr, "WARNING: no input port connected. Waiting explicit connection from user.\n");
}
virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
if (rf.check("name"))
name=string("/")+rf.find("name").asString().c_str();
else
name="/trajectoryPlayer";
rpcPort.open((name+"/rpc").c_str());
attach(rpcPort);
Property portProp;
portProp.put("robot", rf.find("robot"));
portProp.put("part", rf.find("part"));
//*** start the robot driver
if (!robot.configure(portProp))
{
yError() << "Error configuring position controller, check parameters";
return false;
}
if (!robot.init())
{
yError() << "Error cannot connect to remote ports" ;
return false;
}
//*** attach the robot driver to the thread and start it
w_thread.attachRobotDriver(&robot);
b_thread.attachRobotDriver(&robot);
if (!w_thread.start())
{
yError() << "Working thread did not start, queue will not work";
}
else
{
yInfo() << "Working thread started";
}
if (rf.check("execute")==true)
{
yInfo() << "Enablig iPid->setReference() controller";
w_thread.enable_execute_joint_command = true;
}
else
{
yInfo() << "Not using iPid->setReference() controller";
w_thread.enable_execute_joint_command = false;
}
if (rf.check("period")==true)
{
int period = rf.find("period").asInt();
yInfo() << "Thread period set to " << period << "ms";
w_thread.setRate(period);
}
yInfo() << "Using parameters:" << rf.toString();
//*** open the position file
yInfo() << "opening file...";
if (rf.check("filename")==true)
{
string filename = rf.find("filename").asString().c_str();
int req_joints = 0; //default value
if (rf.check("joints"))
{
req_joints = rf.find("joints").asInt();}
else
{
yError() << "Missing parameter 'joints' (number of joints to control)";
return false;
}
if (req_joints < robot.n_joints)
{
yWarning () << "changing n joints from" << robot.n_joints << "to" << req_joints;
robot.n_joints = req_joints;
}
else if (req_joints > robot.n_joints)
{
yError() << "Requested number of joints exceeds the number of available joints on the robot!";
return false;
}
if (!w_thread.actions.openFile(filename,robot.n_joints))
{
yError() << "Unable to parse file " << filename;
return false;
};
}
else
{
yWarning() << "no sequence files load.";
}
yInfo() << "module succesffully configured. ready.";
return true;
}
bool PlaybackLocomotion::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
//-- left leg --
std::string leftLegIni = rf.findFileByName("../locomotion/leftLeg.ini");
yarp::os::Property leftLegOptions;
if (! leftLegOptions.fromConfigFile(leftLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"leftLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured left leg from %s.\n",leftLegIni.c_str());
leftLegOptions.put("name","/teo/leftLeg");
leftLegOptions.put("device","CanBusControlboard");
leftLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) leftLegOptions.put("home",1);
if (rf.check("reset")) leftLegOptions.put("reset",1);
leftLegDevice.open(leftLegOptions);
if (!leftLegDevice.isValid()) {
CD_ERROR("leftLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- right leg --
std::string rightLegIni = rf.findFileByName("../locomotion/rightLeg.ini");
yarp::os::Property rightLegOptions;
if (! rightLegOptions.fromConfigFile(rightLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"rightLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured right leg from %s.\n",rightLegIni.c_str());
rightLegOptions.put("name","/teo/rightLeg");
rightLegOptions.put("device","CanBusControlboard");
rightLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) rightLegOptions.put("home",1);
if (rf.check("reset")) rightLegOptions.put("reset",1);
rightLegDevice.open(rightLegOptions);
if (!rightLegDevice.isValid()) {
CD_ERROR("rightLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftLegDevice.view( playbackThread.leftLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPos.\n");
if ( ! leftLegDevice.view( playbackThread.leftLegPosDirect ) ) {
CD_ERROR("Could not obtain leftLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPosDirect.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPos.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPosDirect ) ) {
CD_ERROR("Could not obtain rightLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPosDirect.\n");
//-- Do stuff.
playbackThread.leftLegPos->getAxes( &(playbackThread.leftLegNumMotors) );
playbackThread.rightLegPos->getAxes( &(playbackThread.rightLegNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftLegPosDirect->setPositionDirectMode();
playbackThread.rightLegPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
return true;
}
int main(int argc, char *argv[])
{
ResourceFinder rf;
rf.configure(argc,argv);
set<string> avFrames=HeadParameters::getTypes();
string types_helper("");
for (set<string>::iterator it=avFrames.begin(); it!=avFrames.end(); it++)
types_helper+=*it+"|";
types_helper.erase(types_helper.end()-1);
if (rf.check("help"))
{
cout<<"Options:"<<endl;
cout<<"--type "<<types_helper<<endl;
cout<<"--verbosity <int>"<<endl;
cout<<"--xd \"(0.0 1.0 2.0)\""<<endl;
cout<<"--q0 \"(0.0 1.0 ... 5.0)\""<<endl;
return 0;
}
string type=rf.check("type",Value("gaze")).asString();
int verbosity=rf.check("verbosity",Value(0)).asInt();
if (avFrames.find(type)==avFrames.end())
{
cerr<<"unrecognized type \""<<type<<"\""<<endl;
return 1;
}
Vector xd(3,0.0);
if (Bottle *b=rf.find("xd").asList())
{
size_t len=std::min(xd.length(),(size_t)b->size());
for (size_t i=0; i<len; i++)
xd[i]=b->get(i).asDouble();
}
Vector q0(6,0.0);
if (Bottle *b=rf.find("q0").asList())
{
size_t len=std::min(q0.length(),(size_t)b->size());
for (size_t i=0; i<len; i++)
q0[i]=b->get(i).asDouble();
}
HeadParameters headp(type);
HeadSolver solver(headp);
solver.setVerbosity(verbosity);
solver.setInitialGuess(q0);
Vector q;
solver.ikin(xd,q);
cout<<"head="<<solver.getHeadParameters().head.getType()<<endl;
cout<<"q0=("<<q0.toString(3,3)<<")"<<endl;
cout<<"xd=("<<xd.toString(3,3)<<")"<<endl;
cout<<"q=("<<q.toString(3,3)<<")"<<endl;
cout<<endl;
return 0;
}
virtual bool configure(ResourceFinder &rf)
{
string slash = "/";
string ctrlName;
string robotName;
// string remoteName;
string localName;
Time::turboBoost();
// get params from the RF
ctrlName = rf.check("local", Value("robotJoystickControl")).asString();
robotName = rf.check("robot", Value("cer")).asString();
localName = "/" + ctrlName;
//reads the configuration file
Property ctrl_options;
ConstString configFile = rf.findFile("from");
if (configFile == "") //--from torsoJoystickControl.ini
{
yWarning("Cannot find .ini configuration file. By default I'm searching for torsoJoystickControl.ini");
//return false;
}
else
{
ctrl_options.fromConfigFile(configFile.c_str());
}
ctrl_options.put("robot", robotName.c_str());
ctrl_options.put("local", localName.c_str());
//check for robotInterface availablity
yInfo("Checking for robotInterface availability");
Port startport;
startport.open(localName+"/robotInterfaceCheck:rpc");
Bottle cmd; cmd.addString("is_ready");
Bottle response;
int rc_count = 0;
int rp_count = 0;
int rf_count = 0;
double start_time = yarp::os::Time::now();
bool not_yet_connected = true;
bool skip_robot_interface_check = rf.check("skip_robot_interface_check");
if (skip_robot_interface_check)
{
yInfo("skipping robotInterface check");
}
else
{
do
{
if (not_yet_connected)
{
bool rc = yarp::os::Network::connect(localName + "/robotInterfaceCheck:rpc", "/" + robotName + "/robotInterface");
if (rc == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rc_count++);
yarp::os::Time::delay(1.0);
continue;
}
else
{
not_yet_connected = false;
yDebug("Connection established with robotInterface");
}
}
bool rp = startport.write(cmd, response);
if (rp == false)
{
yWarning("Problems trying to connect to RobotInterface %d", rp_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while trying to connect to robotInterface");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
if (response.get(0).asString() != "ok")
{
yWarning("RobotInterface is not ready yet, retrying... %d", rf_count++);
if (yarp::os::Time::now() - start_time > 30)
{
yError("Timeout expired while waiting for robotInterface availability");
return false;
}
yarp::os::Time::delay(1.0);
continue;
}
else
{
yInfo("RobotInterface is ready");
break;
}
}
} while (1);
}
//set the thread rate
int rate = rf.check("rate",Value(10)).asInt();
yInfo("baseCtrl thread rate: %d ms.",rate);
//start the control thread
control_thr = new ControlThread(rate, rf, ctrl_options);
if (!control_thr->start())
{
delete control_thr;
return false;
}
//check for debug mode
if (rf.check("no_motors"))
{
this->control_thr->motors_enabled = false;
yInfo() << "Motors disabled";
}
rpcPort.open((localName+"/rpc").c_str());
attach(rpcPort);
return true;
}
bool configure(ResourceFinder &rf)
{
string robot=rf.check("robot",Value("icub")).asString().c_str();
string name=rf.check("name",Value("karmaToolFinder")).asString().c_str();
arm=rf.check("arm",Value("right")).asString().c_str();
eye=rf.check("eye",Value("left")).asString().c_str();
if ((arm!="left") && (arm!="right"))
{
printf("Invalid arm requested!\n");
return false;
}
if ((eye!="left") && (eye!="right"))
{
printf("Invalid eye requested!\n");
return false;
}
Property optionArmL("(device cartesiancontrollerclient)");
optionArmL.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
optionArmL.put("local",("/"+name+"/left_arm").c_str());
if (!drvArmL.open(optionArmL))
{
printf("Cartesian left_arm controller not available!\n");
terminate();
return false;
}
Property optionArmR("(device cartesiancontrollerclient)");
optionArmR.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
optionArmR.put("local",("/"+name+"/right_arm").c_str());
if (!drvArmR.open(optionArmR))
{
printf("Cartesian right_arm controller not available!\n");
terminate();
return false;
}
if (arm=="left")
drvArmL.view(iarm);
else
drvArmR.view(iarm);
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (drvGaze.open(optionGaze))
drvGaze.view(igaze);
else
{
printf("Gaze controller not available!\n");
terminate();
return false;
}
Bottle info;
igaze->getInfo(info);
if (Bottle *pB=info.find(("camera_intrinsics_"+eye).c_str()).asList())
{
int cnt=0;
Prj.resize(3,4);
for (int r=0; r<Prj.rows(); r++)
for (int c=0; c<Prj.cols(); c++)
Prj(r,c)=pB->get(cnt++).asDouble();
}
else
{
printf("Camera intrinsic parameters not available!\n");
terminate();
return false;
}
imgInPort.open(("/"+name+"/img:i").c_str());
imgOutPort.open(("/"+name+"/img:o").c_str());
dataInPort.open(("/"+name+"/in").c_str());
logPort.open(("/"+name+"/log:o").c_str());
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
Vector min(3),max(3);
min[0]=-1.0; max[0]=1.0;
min[1]=-1.0; max[1]=1.0;
min[2]=-1.0; max[2]=1.0;
solver.setBounds(min,max);
solution.resize(3,0.0);
enabled=false;
dataInPort.setReader(*this);
return true;
}
bool scriptModule::configure(ResourceFinder &rf) {
Time::turboBoost();
this->rfCopy = rf;
if (rf.check("name"))
name=string("/")+rf.find("name").asString().c_str();
else
name="/walkPlayer";
contextPath=rf.getContextPath().c_str();
fprintf(stderr,"||| contextPath: %s\n", contextPath.c_str());
rpcPort.open((name+"/rpc").c_str());
attach(rpcPort);
Property portProp;
portProp.put("robot", rf.find("robot"));
//*** start the robot driver
if (!robot.configure(portProp))
{
cerr<<"Error configuring position controller, check parameters"<<endl;
return false;
}
else {
cout << "Configuration done." << endl;
}
if (!robot.init())
{
cerr<<"Error cannot connect to remote ports"<<endl;
return false;
}
else {
cout << "Initialization done." << endl;
}
//*** attach the robot driver to the thread and start it
thread.attachRobotDriver(&robot);
if (!thread.start())
{
cerr<<"ERROR: Thread did not start, queue will not work"<<endl;
}
else
{
cout<<"Thread started"<<endl;
}
if (rf.check("execute")==true)
{
cout << "Enablig iPid->setReference() controller"<< endl;
thread.enable_execute_joint_command = true;
}
else
{
cout << "Not using iPid->setReference() controller"<< endl;
thread.enable_execute_joint_command = false;
}
if (rf.check("period")==true)
{
int period = rf.find("period").asInt();
cout << "Thread period set to "<<period<< "ms" <<endl;
thread.setRate(period);
}
if (rf.check("speed")==true)
{
double fact = rf.find("speed").asDouble();
cout << "speed factor set to "<<fact<< endl;
thread.speed_factor= fact;
}
//*** open the position file
cout << "opening file..." << endl;
if (rf.check("filename")==true)
{
string filename = rf.find("filename").asString().c_str();
if (!thread.actions.openFile(filename, rf))
{
cout << "ERROR: Unable to parse file" << endl;
return false;
};
}
if (rf.check("minJerkLimit")==true )
{
int tmpLimit = rf.find("minJerkLimit").asInt();
thread.minJerkLimit = tmpLimit;
}
if (rf.check("refSpeedMinJerk")==true )
{
thread.refSpeedMinJerk = rf.find("refSpeedMinJerk").asDouble();
} else {
thread.refSpeedMinJerk = 0.0;
}
if (rf.check("torqueBalancingSequence")==true)
{
string filenamePrefix = rf.find("torqueBalancingSequence").asString().c_str();
// Overwrite the execute flag value. This option has higher priority and
// should simply stream trajectories use by the torqueBalancing module.
thread.enable_execute_joint_command = true;
if (!thread.actions.openTorqueBalancingSequence(filenamePrefix,rf))
{
cout << "ERROR: Unable to parse torque balancing sequence" << endl;
return false;
}
}
cout << "Using parameters:" << endl << rf.toString() << endl;
cout << "module successfully configured. ready." << endl;
return true;
}
int
main (int argc, char *argv[])
{
Network yarp;
if (!yarp.checkNetwork ())
return -1;
YARP_REGISTER_DEVICES(icubmod)
ResourceFinder rf;
rf.setVerbose (true);
rf.setDefaultContext ("actionPrimitivesExample/conf");
rf.setDefaultConfigFile ("config.ini");
rf.setDefault ("part", "left_arm");
rf.setDefault ("grasp_model_type", "tactile");
rf.setDefault ("grasp_model_file", "grasp_model.ini");
rf.setDefault ("hand_sequences_file", "hand_sequences.ini");
rf.setDefault ("name", "actionPrimitivesMod");
rf.setDefault ("sim", "off");
rf.configure ("ICUB_ROOT", argc, argv);
BehaviorModule mod;
return mod.runModule (rf);
}
bool LRH::configure(ResourceFinder &rf) {
bool bEveryThingisGood = true;
moduleName = rf.check("name",
Value("lrh"),
"module name (string)").asString();
sKeyWord = rf.check("keyword", Value("grammar")).toString().c_str();
cout << "**************Context path for grammars: " << rf.getContextPath() << endl;
/* Mode Action Performer => Meaning*/
scorpusFileAP = rf.getContextPath().c_str();
scorpusFileAP += rf.check("corpusFileAP", Value("/Corpus/corpus_AP.txt")).toString().c_str();
scorpusFileSD = rf.getContextPath().c_str();
scorpusFileSD += rf.check("corpusFileSD", Value("/Corpus/corpus_SD.txt")).toString().c_str();
sfileResult = rf.getContextPath().c_str();
sfileResult += rf.check("fileResult", Value("/Corpus/output.txt")).toString().c_str();
stemporaryCorpus = rf.getContextPath().c_str();
stemporaryCorpus += rf.check("temporaryCorpus", Value("/Corpus/temporaryCorpus.txt")).toString().c_str();
sreservoirAP = rf.getContextPath().c_str();
sreservoirAP += rf.check("fileAP", Value("/action_performer_PAOR.py")).toString().c_str();
cout << "sreservoirAP : " << sreservoirAP << endl;
sreservoirSD = rf.getContextPath().c_str();
sreservoirSD += rf.check("fileAP", Value("/spatial_relation.py")).toString().c_str();
sclosed_class_wordsAP = rf.check("closed_class_wordsSD", Value("after")).toString().c_str();
sclosed_class_wordsSD = rf.check("closed_class_wordsSD", Value("after")).toString().c_str();
smax_nr_ocw = rf.check("max_nr_ocw", Value("10")).toString().c_str();
smax_nr_actionrelation = rf.check("max_nr_actionrelation", Value("4")).toString().c_str();
selt_pred = rf.check("elt_pred", Value("P A O R V")).toString().c_str();
sHand = rf.check("hand", Value("right")).toString().c_str();
offsetGrasp = rf.check("offsetGrasp", Value("0.02")).asDouble();
setName(moduleName.c_str());
// Open handler port
string sName = getName();
handlerPortName = "/" + sName + "/rpc";
if (!handlerPort.open(handlerPortName.c_str())) {
cout << getName() << ": Unable to open port " << handlerPortName << endl;
bEveryThingisGood = false;
}
attach(handlerPort); // attach to port
//------------------------//
// iCub Client
//------------------------//
// string ttsSystem = SUBSYSTEM_SPEECH;
iCub = new ICubClient(moduleName.c_str(), "lrh", "client.ini", true);
iCub->opc->isVerbose = false;
char rep = 'n';
while (rep != 'y'&&!iCub->connect())
{
cout << "iCubClient : Some dependencies are not running..." << endl;
break; //to debug
Time::delay(1.0);
}
cout << "Connections done" << endl;
iCub->opc->checkout();
cout << "Checkout done" << endl;
//populateOPC();
return true;
}
int main()
{
Network::init();
// open an rpc interface to receive user's commands
Port speaker;
speaker.setRpcMode(true);
int i=0;
while(!speaker.open("/IITdemo/rpc"))
{
if(i>=4) {cout << "Error in creating rpc port" << endl; Network::fini(); return -1;}
Time::delay(0.25);
i++;
}
// open a face interface to dialog with expressions
Port face;
while(!face.open("/IITdemo/face:o"))
{
if(i>=4) {cout << "Error in creating face port" << endl; speaker.close(); Network::fini(); return -1;}
Time::delay(0.25);
i++;
}
// open a planner instance that executes user's commands
Demo1Module * planner;
ResourceFinder rf;
rf.setVerbose(true); // print to a console conf info
rf.setDefaultConfigFile("C:/DARWIN/ConXML/conf/Demo1Configuration.ini");
rf.setDefaultContext("../conf"); // dove sono i file .ini
rf.configure("ICUB_ROOT",0,NULL);
planner = new Demo1Module(rf,20);
if(!planner->threadInit())
{
fprintf(stderr, "Error configuring module returning\n");
face.close();
return -1;
}
if(!planner->start())
{
planner->threadRelease();
fprintf(stderr, "Error configuring module returning\n");
face.close();
return -1;
}
// wait for the port to be connected
//send a welcome message:
Network::connect(face.getName().c_str(),"/icub/face/emotions/in");
Bottle faceCmd,test,reply;
test.clear();
faceCmd.append("set mou sur");
face.write(faceCmd,test);
Bottle welcome;
welcome.addString("--> Hello! Are you ready to play with me?");
speaker.read(test,true);
reply.append(welcome);
speaker.reply(reply);
test.clear();
speaker.read(test,true);
if (test.get(0).asString() == "no")
{
faceCmd.clear();
faceCmd.append("set mou sad");
face.write(faceCmd,test);
welcome.clear();
welcome.addString("--> Bye");
speaker.replyAndDrop(welcome);
planner->stop();
speaker.close();
face.close();
Network::fini();
return 0;
}
faceCmd.clear();
faceCmd.append("set all hap");
face.write(faceCmd,test);
// mettere icub in posizione relax
planner->Relax();
if(!planner->startPMP())
{
welcome.clear();
welcome.addString("--> Did you connect my ports?");
speaker.reply(welcome);
}
planner->enableTouch();
planner->suspend();
welcome.clear();
welcome.addString("--> What should I do?");
speaker.reply(welcome);
Bottle cmd;
while(true)
{
reply.clear();
speaker.read(cmd,true);
if (!planner->arePortsConnected() && cmd.get(0).asString() != "quit")
{
reply.append("--> Have you connected all my ports?");
speaker.reply(reply);
}
else
{
string first = cmd.get(0).asString().c_str();
if (first == "stop")
{
if(planner->stopMotion()) reply.addString("--> OK, I'm freezed");
else reply.addString("--> Sorry, I'm not freezed");
speaker.reply(reply);
}
else if(first == "save")
{
int done;
if(cmd.size() != 5 || !cmd.get(4).isString())
done = -1;
else
done = planner->add2map(cmd.get(4).asString().c_str(),cmd.get(1).asDouble(),cmd.get(2).asDouble(),cmd.get(3).asDouble());
switch(done)
{
case 1: reply.addString("--> Ok, I got it! But it was too low, I have saved it higher");break;
case 0: reply.addString("--> Ok, now I know this object!"); break;
case -1: reply.addString("--> Sorry, I can't understand");break;
case -2: reply.addString("--> Hey, I already knew this object!"); break;
}
speaker.reply(reply);
}
else if(first == "userRecord")
{
int done;
bool left = (cmd.get(1).asString() == "left");
// record using user input to stop
if (cmd.size() == 2)
{
if(!left)
done = planner->userRecordStart(false);
else
done = planner->userRecordStart();
}
if (done == 0)
{
reply.addString("--> Ok, you can guide me!");
speaker.reply(reply);
Bottle stop;
speaker.read(stop,true);
if(!left)
done = planner->userRecordStop(false);
else
done = planner->userRecordStop();
}
switch(done)
{
case 1:
reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
break;
case 0:
reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
break;
case -3: reply.addString("--> Sorry, I did not understand what you said");break;
case -4: reply.addString("--> Sorry, I could not stop my motion");break;
case -5: reply.addString("--> Sorry, I could not start recording");break;
case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
}
}
else if(first == "record")
{
int done;
// record using user input to stop
if (cmd.size() == 2)
{
if(cmd.get(1).asString() == "right")
done = planner->Record(false);
else if(cmd.get(1).asString() == "left")
done = planner->Record();
else
done = -3;
}
else
done = planner->Record();
switch(done)
{
case 1:
// wait for the point to be recorded: if z<=SAFETY_Z, z = z+0.1
// once it has been recorded ask for the object label
planner->pointRecorded.wait();
Time::delay(0.5);
//planner->Relax();
planner->Prepare();
reply.addString("--> Ok, I got it! But it was too low, I have saved it higher. How is this object called (label <name>)?");
break;
case 0:
// wait for the point to be recorded
// once it has been recorded ask for the object label
planner->pointRecorded.wait();
Time::delay(5);
//planner->Relax();
planner->Prepare();
reply.addString("--> Ok, I got it! How is this object called (label <name>)?");
break;
case -3: reply.addString("--> Sorry, I did not understand what you said");break;
case -4: reply.addString("--> Sorry, I could not stop my motion");break;
case -5: reply.addString("--> Sorry, I could not start recording");break;
case -6: reply.addString("--> Sorry, I don't know where my arm is");break;
}
speaker.reply(reply);
}
else if (first == "label")
{
if (cmd.size() != 2)
reply.addString("--> Sorry, I did not understand the label");
else
{
int done = planner->Label(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, now I know this object!"); break;
case -1: reply.addString("--> Sorry, I don't have any point to label"); break;
case -2: reply.addString("--> Hey, I already knew this object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "forget")
{
if (cmd.size() != 2)
reply.addString("--> Sorry, I did not understand the label");
else
{
int done = planner->Forget(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, object forgotten!"); break;
case -1: reply.addString("--> Sorry, I don't remember this object"); break;
}
}
speaker.reply(reply);
}
else if (first == "recall")
{
reply = planner->Recall();
if (reply.size() == 0)
reply.addString("--> Sorry, tabula rasa");
speaker.reply(reply);
}
else if (first == "relax")
{
int done = planner->Relax();
switch(done)
{
case 0: reply.addString("--> Ok, I relax!"); break;
case -1: reply.addString("--> Sorry, I can't relax"); break;
}
speaker.reply(reply);
}
else if (first == "prepare")
{
int done = planner->Prepare();
switch(done)
{
case 0: reply.addString("--> Ok, I'm ready!"); break;
case -1: reply.addString("--> Sorry, something went wrong"); break;
}
speaker.reply(reply);
}
// sequence reaching
else if (first == "reach")
{
if(cmd.size() == 1)
reply.addString("--> Sorry, I don't know what I should reach with my finger");
else
{
int done = planner->SequenceReach(cmd.tail());
switch(done)
{
case 0: reply.addString("--> Ok, I have reached the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
}
}
speaker.reply(reply);
}
// finger reaching
else if (first == "reach1")
{
if(cmd.size() != 3 && cmd.size() != 2)
reply.addString("--> Sorry, I don't know what I should reach with my finger");
else
{
int done = planner->Prepare();
if(done != 0) reply.addString("--> Sorry, I'm lazy");
else if (cmd.size() == 3)
{
if (cmd.get(2).asString() == "top" || cmd.get(2).asString() == "side")
{
done = planner->FingerReach(cmd.get(1).asString().c_str(),"",cmd.get(2).asString().c_str(),false);
if (done == 0)
{
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
}
else
done = -9;
}
else
{
done = planner->FingerReach(cmd.get(1).asString().c_str(),"","top",false);
if (done == 0)
{
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
}
switch(done)
{
case 0:
reply.addString("--> Ok, I have reached the object!");
//done = planner->Lift(planner->activeChain);
//if (done !=0 )
//{reply.addString("But I could not lift my hand"); cout << "done: " << done << endl;break;}
done = planner->goFar(planner->activeChain);
if (done !=0 )
{reply.addString("But I could not go farer"); cout << "done: " << done << endl;}
planner->Relax();
break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
}
}
speaker.reply(reply);
}
// reach to grasp function
else if (first == "grasp")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should grasp");
}
else
{
cout << "prepare for grasping" << endl;
// this function will return 1 if no reaching is needed, 0 otherwise
int done = planner->prepare4Grasping(cmd.get(1).asString().c_str());
if (done == 0)
{
// check that motion is finished successfully
cout << "check reaching performance" << endl;
done = planner->checkReachingSuccess(cmd.get(1).asString().c_str());
}
if(done == 0 || done == 1)
{
cout << "grasp" << endl;
done = (planner->Grasp()).get(0).asInt();
}
if(done == 0)
{
done = planner->Lift(planner->activeChain,true);
// check if contact is mantained
if(done == 0)
{
done = planner->Release(planner->activeChain);
if (done == 0) // reinitialize
done = planner->Relax();
}
}
switch(done)
{
case 0:
case 1: reply.addString("--> Ok, I grasped the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected, I can't grasp"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "release")
{
if(cmd.size() != 2)
reply.addString("--> Sorry, I don't know what hand to open");
else
{
int done = planner->Release(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I released the object!"); break;
case -1: reply.addString("--> Sorry, I can't release the object"); break;
}
if (done == 0) planner->Relax();//planner->initIcubUp();
}
speaker.reply(reply);
}
else if (first == "lift")
{
if(cmd.size() != 2)
reply.addString("--> Sorry, I don't know what hand to lift");
else
{
int done = planner->Lift(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I have lifted my hand!"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -9: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "carry")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should grasp");
}
else
{
}
speaker.reply(reply);
}
else if (first == "unstack")
{
int done = planner->Unstack();
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
case -11: reply.addString("--> Hey, I do not see a stack!"); break;
case -12: reply.addString("--> Sorry, points are not consistent!"); break;
case -13: reply.addString("--> Sorry, I can't see any object!"); break;
}
speaker.reply(reply);
planner->Prepare();
}
else if (first == "recycle")
{
if(cmd.size() != 2)
{
reply.addString("--> Sorry, I don't know what I should throw in the garbage");
}
else
{
int done = planner->Recycle(cmd.get(1).asString().c_str());
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
}
planner->Relax();
}
speaker.reply(reply);
}
else if (first == "geometry")
{
if(cmd.size() < 2)
reply.addString("--> Sorry, I don't understand");
if(cmd.get(1).asString() == "get")
reply.append(planner->getObjectGeometry());
else if (cmd.get(1).asString() == "set" && cmd.size() == 4)
{
planner->setObjectGeometry(cmd.get(2).asDouble(),cmd.get(3).asDouble());
reply.addString("--> OK, now I have new geometry values");
}
else
reply.addString("--> Sorry, I don't understand");
speaker.reply(reply);
}
else if (first == "bimanual")
{
if(cmd.size() < 2)
reply.addString("--> Sorry, I don't know what I should do with my hands");
else
{
int done;
if (cmd.get(1).asString() == "reach")
{
switch(cmd.size())
{
case 3: // use the geometrical info abt the object to reach it
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
planner->BimanualRelease();
break;
case 4: // reach from top two different objects
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(3).asString().c_str());
break;
}
// get the label of the object/s to reach: if one
}
else if (cmd.get(1).asString() == "grasp" && cmd.size() == 3)
{
done = planner->initBimanualReach(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
if (done == 0)
{
done = planner->checkBimanualReachingSuccess(cmd.get(2).asString().c_str(),cmd.get(2).asString().c_str());
if (done == 0)
{
done = planner->BimanualGrasp();
if (done == 0)
done = planner->BimanualLift(false);
}
}
Time::delay(1);
planner->BimanualRelease();
}
else if(cmd.get(1).asString() == "carry")
{
}
switch(done)
{
case 0: reply.addString("--> Ok, I have recycled the object!"); break;
case -1: reply.addString("--> Sorry, I don't know this object"); break;
case 1 : reply.addString("--> Sorry, I don't know where the litter is (label = bin)"); break;
case -2: reply.addString("--> Sorry, I don't know where my hands are"); break;
case -3: reply.addString("--> Sorry, I did not understand what you said"); break;
case -4: reply.addString("--> Sorry, my ports are not connected"); break;
case -5: reply.addString("--> Sorry, I don't know this command"); break;
case -6: reply.addString("--> Sorry, I don't have this hand"); break;
case -7: reply.addString("--> Sorry, I couldn't grasp the object"); break;
case -8: reply.addString("--> Sorry, I couldn't reach the object"); break;
case -9: reply.addString("--> Sorry, my wrist orientation is wrong"); break;
case -10: reply.addString("--> Hey, I have lost the object!"); break;
}
}
speaker.reply(reply);
}
else if (first == "quit")
{
planner->Relax();
//if(planner->isSuspended()) planner->resume();
planner->stop();
reply.addString("--> Bye");
speaker.replyAndDrop(reply);
break;
}
else
{
reply.addString("--> I did not understand what you said");
speaker.reply(reply);
}
}
cmd.clear();
reply.clear();
}
face.interrupt();
face.close();
speaker.close();
Network::fini();
return 0;
}
int Drivers::yarpdev(int argc, char *argv[]) {
std::signal(SIGINT, handler);
std::signal(SIGTERM, handler);
// get command line options
ResourceFinder rf;
rf.configure(argc, argv); // this will process --from FILE if present
Property options;
// yarpdev will by default try to pass its thread on to the device.
// this is because some libraries need configuration and all
// access methods done in the same thread (e.g. opencv_grabber
// apparently).
options.put("single_threaded", 1);
// interpret command line options as a set of flags
//options.fromCommand(argc,argv,true,false);
options.fromString(rf.toString().c_str(), false);
// check if we're being asked to read the options from file
Value *val;
if (options.check("file",val))
{
// FIXME use argv[0]
yError("*** yarpdev --file is deprecated, please use --from\n");
yError("*** yarpdev --file will be removed in a future version of YARP\n");
std::string fname = val->toString();
options.unput("file");
yDebug("yarpdev: working with config file %s\n", fname.c_str());
options.fromConfigFile(fname,false);
// interpret command line options as a set of flags again
// (just in case we need to override something)
options.fromCommand(argc,argv,true,false);
}
// check if we want to use nested options (less ambiguous)
if (options.check("nested",val)||options.check("lispy",val))
{
std::string lispy = val->toString();
yDebug("yarpdev: working with config %s\n", lispy.c_str());
options.fromString(lispy);
}
if (!options.check("device"))
{
// no device mentioned - maybe user needs help
if (options.check("list"))
{
yInfo("Here are devices listed for your system:");
for (auto& s : split(Drivers::factory().toString(), '\n')) {
yInfo("%s", s.c_str());
}
}
else
{
yInfo("Welcome to yarpdev, a program to create YARP devices\n");
yInfo("To see the devices available, try:\n");
yInfo(" yarpdev --list\n");
yInfo("To create a device whose name you know, call yarpdev like this:\n");
yInfo(" yarpdev --device DEVICENAME --OPTION VALUE ...\n");
yInfo(" For example:\n");
yInfo(" yarpdev --device test_grabber --width 32 --height 16 --name /grabber\n");
yInfo("You can always move options to a configuration file:\n");
yInfo(" yarpdev [--device DEVICENAME] --from CONFIG_FILENAME\n");
yInfo("If you have problems, you can add the \"verbose\" flag to get more information\n");
yInfo(" yarpdev --verbose --device ffmpeg_grabber\n");
if (options.check ("from"))
{
yError("Unable to find --device option in file %s. Closing.", options.find("from").asString().c_str());
}
else
{
yWarning("--device option not specified. Closing.");
}
}
return 0;
}
// ask for a wrapped, remotable device rather than raw device
options.put("wrapped","1");
//YarpDevMonitor monitor;
bool verbose = false;
if (options.check("verbose")) {
verbose = true;
//options.setMonitor(&monitor,"top-level");
}
// we now need network
bool ret=Network::checkNetwork();
if (!ret)
{
yError("YARP network not available, check if yarp server is reachable\n");
return -1;
}
//
// yarpdev initializes the clock only before starting to do real thing.
// This way yarpdev --lish/help will not be affected by network clock.
//
// Shall other devices be affected by network clock ??
// Hereafter the device may need to use the SystemClock or the NetworkClock
// depending by the device, a real or a fake / simulated one.
// Using the YARP_CLOCK_DEFAULT the behaviour will be determined by the
// environment variable.
//
yarp::os::NetworkBase::yarpClockInit(yarp::os::YARP_CLOCK_DEFAULT);
PolyDriver dd(options);
if (verbose) {
toDox(dd,stdout);
}
if (!dd.isValid()) {
yError("yarpdev: ***ERROR*** device not available.\n");
if (argc==1)
{
yInfo("Here are the known devices:\n");
yInfo("%s", Drivers::factory().toString().c_str());
}
else
{
yInfo("Suggestions:\n");
yInfo("+ Do \"yarpdev --list\" to see list of supported devices.\n");
if (!options.check("verbose"))
{
yInfo("+ Or append \"--verbose\" option to get more information.\n");
}
}
return 1;
}
Terminee *terminee = nullptr;
if (dd.isValid()) {
Value *v;
std::string s("/yarpdev/quit");
if (options.check("device", v)) {
if (v->isString()) {
s = "";
s += "/";
s += v->toString();
s += "/quit";
}
}
if (options.check("name", v)) {
s = "";
s += v->toString();
s += "/quit";
}
if (s.find("=") == std::string::npos &&
s.find("@") == std::string::npos) {
terminee = new Terminee(s.c_str());
terminatorKey = s.c_str();
if (terminee == nullptr) {
yError("Can't allocate terminator port\n");
terminatorKey = "";
dd.close();
return 1;
}
if (!terminee->isOk()) {
yError("Failed to create terminator port\n");
terminatorKey = "";
delete terminee;
terminee = nullptr;
dd.close();
return 1;
}
}
}
double dnow = 3;
double startTime = Time::now()-dnow;
IService *service = nullptr;
dd.view(service);
if (service!=nullptr) {
bool backgrounded = service->startService();
if (backgrounded) {
// we don't need to poll this, so forget about the
// service interface
yDebug("yarpdev: service backgrounded\n");
service = nullptr;
}
}
while (dd.isValid() && !(terminated||(terminee&&terminee->mustQuit()))) {
if (service!=nullptr) {
double now = Time::now();
if (now-startTime>dnow) {
yInfo("device active...");
startTime += dnow;
}
// we requested single threading, so need to
// give the device its chance
if(!service->updateService()) {
if(!service->stopService()) {
yWarning("Error while stopping device");
}
terminated = true;
}
} else {
// we don't need to do anything
yInfo("device active in background...");
SystemClock::delaySystem(dnow);
}
}
if (terminee) {
delete terminee;
terminee = nullptr;
}
dd.close();
yInfo("yarpdev is finished.");
return 0;
}
bool learnPrimitive::updateAction(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bAction = rf.findGroup("Action");
if (!bAction.isNull())
{
Bottle * bActionName = bAction.find("ActionName").asList();
Bottle * bActionArg = bAction.find("ActionArg").asList();
if(bActionName->isNull() || bActionArg->isNull()){
yError() << " [updateAction] : one of the primitiveAction conf is null : ActionName, ActionArg" ;
return false ;
}
int actionSize = -1 ;
if(bActionName->size() == bActionArg->size()){
actionSize = bActionName->size() ;
} else {
yError() << " [updateAction] : one of the Action conf has different size!" ;
return false ;
}
if(actionSize == 0) {
yWarning() << " [updateAction] : there is no Action defined at startup!" ;
} else {
for(int i = 0; i < actionSize ; i++) {
//insert protoaction even if already there
Bottle bSubAction;
string currentActionName = bActionName->get(i).asString();
string currentActionArg = bActionArg->get(i).asString();
bSubAction.addString(currentActionName);
bSubAction.addString(currentActionArg);
string concat = currentActionName + "_" + currentActionArg;
// yInfo() << " Looking for " << concat ;
Bottle bCurrent = rf.findGroup(concat);
if(bCurrent.isNull()){
yError() << " [updateAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListSubAction = bCurrent.find("actionList").asList();
//yInfo() << "Subaction found : " << bListSubAction->toString() ;
if(bListSubAction->isNull()){
yError() << " [updateAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bSubAction.addList() = *bListSubAction ;
yInfo() << "Complex Action added : (" <<bSubAction.get(0).asString() << ", " << bSubAction.get(1).asString() << ", ( " << bSubAction.get(2).asList()->toString()<< "))" ;
vActionBottle.push_back(bSubAction);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
int main(int argc, char *argv[])
{
Network yarp;
ResourceFinder rf;
BufferedPort<Bottle> inPort, outPort;
rf.setVerbose(true);
rf.configure(argc,argv);
if (rf.check("help"))
{
usage();
return 0;
}
std::string name=rf.check("name",Value("com2gui")).asString();
std::string wholebody=rf.check("wholebody",Value("wholeBodyDynamics")).asString();
bool ok=inPort.open("/"+name+"/COM:i");
ok=ok && outPort.open("/"+name+"/objects:o");
if (!ok)
{
yError() << "Could not open yarp ports - is yarpserver running happily ?";
return EXIT_FAILURE;
}
yarp.connect(outPort.getName(),"/iCubGui/objects","udp");
yarp.connect("/"+wholebody+"/com:o",inPort.getName(),"udp");
yInfo() << "Configured: ";
yInfo() << " name= " << name;
yInfo() << " wholebody= " << wholebody << " (reading from /"+wholebody+"/com:o)";
yInfo() << " writing to /iCubGui/objects";
int i=0;
while (true)
{
Bottle *in = inPort.read();
if (in==NULL) {
yError() << "Failed to read message";
return EXIT_FAILURE;
}
double x=in->get(0).asDouble();
double y=in->get(1).asDouble();
double z=in->get(2).asDouble();
Bottle &obj=outPort.prepare();
obj.clear();
obj.addString("object"); // command to add/update an object
obj.addString("COM");
// object dimensions in mm
// (it will be displayed as an ellipsoid with the tag "COM")
obj.addDouble(50);
obj.addDouble(50);
obj.addDouble(50);
// object position in millimiters
// reference frame: X=fwd, Y=left, Z=up
obj.addDouble(x);
obj.addDouble(y);
obj.addDouble(z);
// object orientation (roll, pitch, yaw) in degrees
obj.addDouble(0);
obj.addDouble(0);
obj.addDouble(0);
// object color (0-255)
obj.addInt(255);
obj.addInt(128);
obj.addInt(128);
// transparency (0.0=invisible 1.0=solid)
obj.addDouble(1.0);
outPort.write(true);
}
return EXIT_SUCCESS;
}
int main(int argc, char *argv[])
{
ResourceFinder rf;
rf.configure(argc,argv);
double sim_time=rf.check("sim-time",Value(10.0)).asDouble();
double motor_tau=rf.check("motor-tau",Value(0.0)).asDouble();
string avoidance_type=rf.check("avoidance-type",Value("tactile")).asString();
iCubArm arm("left");
iKinChain &chain=*arm.asChain();
chain.releaseLink(0);
chain.releaseLink(1);
chain.releaseLink(2);
Vector q0(chain.getDOF(),0.0);
q0[3]=-25.0; q0[4]=20.0; q0[6]=50.0;
chain.setAng(CTRL_DEG2RAD*q0);
Matrix lim(chain.getDOF(),2);
Matrix v_lim(chain.getDOF(),2);
for (size_t r=0; r<chain.getDOF(); r++)
{
lim(r,0)=CTRL_RAD2DEG*chain(r).getMin();
lim(r,1)=CTRL_RAD2DEG*chain(r).getMax();
v_lim(r,0)=-50.0;
v_lim(r,1)=+50.0;
}
v_lim(1,0)=v_lim(1,1)=0.0; // disable torso roll
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",1e-6);
app->Options()->SetStringValue("mu_strategy","adaptive");
app->Options()->SetIntegerValue("max_iter",10000);
app->Options()->SetNumericValue("max_cpu_time",0.05);
app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
app->Options()->SetStringValue("hessian_approximation","limited-memory");
app->Options()->SetStringValue("derivative_test","none");
app->Options()->SetIntegerValue("print_level",0);
app->Initialize();
Ipopt::SmartPtr<ControllerNLP> nlp=new ControllerNLP(chain);
AvoidanceHandlerAbstract *avhdl;
if (avoidance_type=="none")
avhdl=new AvoidanceHandlerAbstract(arm);
else if (avoidance_type=="visuo")
avhdl=new AvoidanceHandlerVisuo(arm);
else if (avoidance_type=="tactile")
avhdl=new AvoidanceHandlerTactile(arm);
else if (avoidance_type=="visuo-tactile")
avhdl=new AvoidanceHandlerVisuoTactile(arm);
else
{
yError()<<"unrecognized avoidance type! exiting ...";
return 1;
}
yInfo()<<"Avoidance-Handler="<<avhdl->getType();
yInfo()<<"Avoidance Parameters="<<avhdl->getParameters().toString();
double dt=0.01;
double T=1.0;
nlp->set_dt(dt);
Motor motor(q0,lim,motor_tau,dt);
Vector v(chain.getDOF(),0.0);
Vector xee=chain.EndEffPosition();
minJerkTrajGen target(xee,dt,T);
Vector xc(3);
xc[0]=-0.35;
xc[1]=0.0;
xc[2]=0.1;
double rt=0.1;
Vector xo(3);
xo[0]=-0.3;
xo[1]=0.0;
xo[2]=0.4;
Vector vo(3,0.0);
vo[2]=-0.1;
Obstacle obstacle(xo,0.07,vo,dt);
ofstream fout;
fout.open("data.log");
std::signal(SIGINT,signal_handler);
for (double t=0.0; t<sim_time; t+=dt)
{
Vector xd=xc;
xd[1]+=rt*cos(2.0*M_PI*0.3*t);
xd[2]+=rt*sin(2.0*M_PI*0.3*t);
target.computeNextValues(xd);
Vector xr=target.getPos();
xo=obstacle.move();
avhdl->updateCtrlPoints();
Matrix VLIM=avhdl->getVLIM(obstacle,v_lim);
nlp->set_xr(xr);
nlp->set_v_lim(VLIM);
nlp->set_v0(v);
nlp->init();
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
v=nlp->get_result();
xee=chain.EndEffPosition(CTRL_DEG2RAD*motor.move(v));
yInfo()<<" t [s] = "<<t;
yInfo()<<" v [deg/s] = ("<<v.toString(3,3)<<")";
yInfo()<<" |xr-xee| [m] = "<<norm(xr-xee);
yInfo()<<"";
ostringstream strCtrlPoints;
deque<Vector> ctrlPoints=avhdl->getCtrlPointsPosition();
for (size_t i=0; i<ctrlPoints.size(); i++)
strCtrlPoints<<ctrlPoints[i].toString(3,3)<<" ";
fout<<t<<" "<<
xr.toString(3,3)<<" "<<
obstacle.toString()<<" "<<
v.toString(3,3)<<" "<<
(CTRL_RAD2DEG*chain.getAng()).toString(3,3)<<" "<<
strCtrlPoints.str()<<
endl;
if (gSignalStatus==SIGINT)
{
yWarning("SIGINT detected: exiting ...");
break;
}
}
fout.close();
delete avhdl;
return 0;
}
// Initializes the application loading the modules, setting the
// graphics mode, loading the configuration and resources, etc.
App::App(int argc, const char* argv[])
: m_modules(NULL)
, m_legacy(NULL)
, m_isGui(false)
, m_isShell(false)
, m_exporter(NULL)
{
ASSERT(m_instance == NULL);
m_instance = this;
AppOptions options(argc, argv);
m_modules = new Modules(!options.startUI(), options.verbose());
m_isGui = options.startUI();
m_isShell = options.startShell();
m_legacy = new LegacyModules(isGui() ? REQUIRE_INTERFACE: 0);
m_files = options.files();
if (options.hasExporterParams()) {
m_exporter.reset(new DocumentExporter);
m_exporter->setDataFilename(options.data());
m_exporter->setTextureFilename(options.sheet());
m_exporter->setScale(options.scale());
}
// Register well-known image file types.
FileFormatsManager::instance()->registerAllFormats();
// init editor cursor
Editor::editor_cursor_init();
// Load RenderEngine configuration
RenderEngine::loadConfig();
if (isPortable())
PRINTF("Running in portable mode\n");
// Default palette.
std::string palFile(!options.paletteFileName().empty() ?
options.paletteFileName():
std::string(get_config_string("GfxMode", "Palette", "")));
if (palFile.empty()) {
// Try to use a default pixel art palette.
ResourceFinder rf;
rf.includeDataDir("palettes/db32.gpl");
if (rf.findFirst())
palFile = rf.filename();
}
if (!palFile.empty()) {
PRINTF("Loading custom palette file: %s\n", palFile.c_str());
base::UniquePtr<Palette> pal(load_palette(palFile.c_str()));
if (pal.get() != NULL) {
set_default_palette(pal.get());
}
else {
PRINTF("Error loading custom palette file\n");
}
}
// Set system palette to the default one.
set_current_palette(NULL, true);
}
void TouchDetectorModule::initializeParameters(ResourceFinder &rf)
{
moduleName = rf.check("name", Value("touchDetector"), "Module name (string)").asString();
setName(moduleName.c_str());
period = rf.check("period", Value(1000 / 30), "Thread period (string)").asInt();
threshold = rf.check("threshold", Value(50), "Activation threshold (int)").asInt();
rf.setDefault("clustersConfFile", Value("clustersConfig.ini"));
clustersConfFilepath = rf.findFile("clustersConfFile");
// get the name of the input and output ports, automatically prefixing the module name by using getName()
torsoPortName = "/";
torsoPortName += getName(rf.check("torsoPort", Value("/torso:i"), "Torso input port (string)").asString());
leftArmPortName = "/";
leftArmPortName += getName(rf.check("leftArmPort", Value("/left_arm:i"), "Left arm input port (string)").asString());
rightArmPortName = "/";
rightArmPortName += getName(rf.check("rightArmPort", Value("/right_arm:i"), "Right arm input port (string)").asString());
leftForearmPortName = "/";
leftForearmPortName += getName(rf.check("leftForearmPort", Value("/left_forearm:i"), "Left forearm input port (string)").asString());
rightForearmPortName = "/";
rightForearmPortName += getName(rf.check("rightForearmPort", Value("/right_forearm:i"), "Right forearm input port (string)").asString());
leftHandPortName = "/";
leftHandPortName += getName(rf.check("leftHandPort", Value("/left_hand:i"), "Left hand input port (string)").asString());
rightHandPortName = "/";
rightHandPortName += getName(rf.check("rightHandPort", Value("/right_hand:i"), "Right hand input port (string)").asString());
touchPortName = "/";
touchPortName += getName(rf.check("touchPort", Value("/touch:o"), "Touch output port (string)").asString());
}
void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData)
{
//ask for confirmation
if (!dialog_question("Do you really want to recalibrate the joint?"))
{
return;
}
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "opening file \n");
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_calib"));
if (!fnd->findGroup(buffer2).isNull())
{
bool ok = true;
Bottle xtmp;
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%d ", type);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration1");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param1 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param1);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration2");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param2 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param2);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration3");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param3 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f \n", param3);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group", buffer2, true);
else
ical->calibrate2(*joint, type, param1, param2, param3);
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:", buffer2, true);
return;
}
//********************************************
bool configure(ResourceFinder &rf)
{
if (rf.check("noLeftArm") && rf.check("noRightArm"))
{
yError("[demoAvoidance] no arm has been selected. Closing.");
return false;
}
useLeftArm = false;
useRightArm = false;
string name=rf.check("name",Value("avoidance")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
motionGain = -1.0;
if (rf.check("catching"))
{
motionGain = 1.0;
}
if (motionGain!=-1.0)
{
yWarning("[demoAvoidance] motionGain set to catching");
}
bool autoConnect=rf.check("autoConnect");
if (autoConnect)
{
yWarning("[demoAvoidance] Autoconnect mode set to ON");
}
bool stiff=rf.check("stiff");
if (stiff)
{
yInfo("[demoAvoidance] Stiff Mode enabled.");
}
Matrix R(4,4);
R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0;
if (!rf.check("noLeftArm"))
{
useLeftArm=true;
data["left"]=Data();
data["left"].home_x[0]=-0.30;
data["left"].home_x[1]=-0.20;
data["left"].home_x[2]=+0.05;
data["left"].home_o=dcm2axis(R);
Property optionCartL;
optionCartL.put("device","cartesiancontrollerclient");
optionCartL.put("remote","/"+robot+"/cartesianController/left_arm");
optionCartL.put("local",("/"+name+"/cart/left_arm").c_str());
if (!driverCartL.open(optionCartL))
{
close();
return false;
}
Property optionJointL;
optionJointL.put("device","remote_controlboard");
optionJointL.put("remote","/"+robot+"/left_arm");
optionJointL.put("local",("/"+name+"/joint/left_arm").c_str());
if (!driverJointL.open(optionJointL))
{
close();
return false;
}
driverCartL.view(data["left"].iarm);
data["left"].iarm->storeContext(&contextL);
Vector dof;
data["left"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["left"].iarm->setDOF(dof,dof);
data["left"].iarm->setTrajTime(0.9);
data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o);
data["left"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointL.view(imode);
IImpedanceControl *iimp; driverJointL.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
if (!rf.check("noRightArm"))
{
useRightArm = true;
data["right"]=Data();
data["right"].home_x[0]=-0.30;
data["right"].home_x[1]=+0.20;
data["right"].home_x[2]=+0.05;
data["right"].home_o=dcm2axis(R);
Property optionCartR;
optionCartR.put("device","cartesiancontrollerclient");
optionCartR.put("remote","/"+robot+"/cartesianController/right_arm");
optionCartR.put("local",("/"+name+"/cart/right_arm").c_str());
if (!driverCartR.open(optionCartR))
{
close();
return false;
}
Property optionJointR;
optionJointR.put("device","remote_controlboard");
optionJointR.put("remote","/"+robot+"/right_arm");
optionJointR.put("local",("/"+name+"/joint/right_arm").c_str());
if (!driverJointR.open(optionJointR))
{
close();
return false;
}
driverCartR.view(data["right"].iarm);
data["right"].iarm->storeContext(&contextR);
Vector dof;
data["right"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["right"].iarm->setDOF(dof,dof);
data["right"].iarm->setTrajTime(0.9);
data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o);
data["right"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointR.view(imode);
IImpedanceControl *iimp; driverJointR.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
dataPort.open(("/"+name+"/data:i").c_str());
dataPort.setReader(*this);
if (autoConnect)
{
Network::connect("/visuoTactileRF/skin_events:o",("/"+name+"/data:i").c_str());
}
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
fprintf(stderr, "Getting projections\n");
Matrix PiRight;
Bottle b;
b = rf.findGroup("CAMERA_CALIBRATION_RIGHT");
//fprintf(stderr, "CAMERA_CALIBRATION_RIGHT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiRight) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_RIGHT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with RightProjection \n");
for (int i=0; i < PiRight.rows(); i++)
fprintf(stderr, "%s\n", PiRight.getRow(i).toString().c_str());
}
Matrix PiLeft;
b = rf.findGroup("CAMERA_CALIBRATION_LEFT");
//fprintf(stderr, "CAMERA_CALIBRATION_LEFT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiLeft) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_LEFT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with LeftProjection \n");
for (int i=0; i < PiLeft.rows(); i++)
fprintf(stderr, "%s\n", PiLeft.getRow(i).toString().c_str());
}
int period=50;
if (rf.check("period"))
period=rf.find("period").asInt();
bool enableKalman=false;
if (rf.check("kalman"))
enableKalman=true;
string ctrlName=rf.find("name").asString().c_str();
string robotName=rf.find("robot").asString().c_str();
fprintf(stderr, "Initializing eT\n");
eT=new eyeTriangulation(rf, PiLeft, PiRight, enableKalman, period, ctrlName, robotName);
Vector xr(3); xr(0)=PiRight(0,2); xr(1)=PiRight(1,2); xr(2)=1.0;
Vector xl(3); xl(0)=PiLeft(0,2); xl(1)=PiLeft(1,2); xl(2)=1.0;
eT->xInit(xl, xr);
if (rf.check("const"))
eT->xSetConstant(xl, xr);
eT->start();
string rpcPortName="/"+ctrlName+"/rpc";
rpcPort.open(rpcPortName.c_str());
attach(rpcPort);
return true;
}
int main(int argc, char *argv[])
{
Network yarp; //initialize network, this goes before everything
if (!yarp.checkNetwork())
{
fprintf(stderr, "Sorry YARP network does not seem to be available, is the yarp server available?\n");
return -1;
}
yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
Time::turboBoost();
//for compatibility with old usage of iCubInterface, the use of the ResourceFinder
//here is merely functional and should NOT be taken as an example
ResourceFinder rf;
rf.setVerbose();
rf.setDefaultConfigFile("iCubInterface.ini");
rf.configure(argc, argv);
ConstString configFile=rf.findFile("config");
ConstString cartRightArm=rf.findFile("cartRightArm");
ConstString cartLeftArm=rf.findFile("cartLeftArm");
std::string filename;
bool remap=false;
if (configFile!="")
{
configFile=rf.findFile("config");
filename=configFile.c_str();
remap=true;
}
else
{
configFile=rf.find("file").asString();
if (configFile!="")
{
const char *conf = yarp::os::getenv("ICUB_ROOT");
filename+=conf;
filename+="/conf/";
filename+=configFile.c_str();
printf("Read robot description from %s\n", filename.c_str());
}
else
{
printf("\n");
printf("Error: iCubInterface was not able to find a valid configuration file ");
printf("(since September 2009 we changed a bit how iCubInterface locates configuratrion files).\n");
printf("== Old possibilities:\n");
printf("--config <CONFIG_FILE> read config file CONFIG_FILE.\n iCubInterface now ");
printf("uses the ResourceFinder class to search for CONFIG_FILE. ");
printf("Make sure you understand how the ResourceFinder works. In particular ");
printf("you most likely need to set the YARP_ROBOT_NAME environment variable ");
printf("to tell the RF to add robots/$YARP_ROBOT_NAME to the search path.\n");
printf("--file <CONFIG_FILE> read config file from $ICUB_ROOT/conf: old style ");
printf("initialization method, obsolete. Still here for compatibility reasons.\n");
printf("== New possibilities:\n");
printf("Place a file called iCubInterface.ini in robots/$YARP_ROBOT_NAME that contains ");
printf("the line \"config icubSafe.ini\" (or anything of your choice), and run iCubInterface ");
printf("without parameters\n.");
printf("== Preventing default behaviors:\n");
printf("Use full path in <CONFIG_FILE> (e.g. --config ./icubSafe.ini).\n");
printf("Use --from: change config file (e.g. --from iCubInterfaceCustom.ini).\n");
return -1;
}
}
// printf("--file <CONFIG_FILE> read robot config file CONFIG_FILE\n");
// printf("Note: this files are searched in $ICUB_ROOT/conf (obsolete)\n");
// printf("--config <CONFIG_FILE> full path to robot config file\n");
// printf("-Examples:\n");
// printf("%s --file icub.ini (obsolete, not standard)\n", argv[0]);
// printf("%s --config $ICUB_ROOT/app/default/conf/icub.ini\n)", argv[0]);
// return -1;
Property robotOptions;
bool ok=robotOptions.fromConfigFile(filename.c_str());
if (!ok)
{
fprintf(stderr, "Sorry could not open %s\n", filename.c_str());
return -1;
}
printf("Config file loaded successfully\n");
std::string quitPortName;
if (robotOptions.check("TERMINATEPORT"))
{
Value &v=robotOptions.findGroup("TERMINATEPORT").find("Name");
quitPortName=std::string(v.toString().c_str());
}
else
quitPortName=std::string("/iCubInterface/quit");
fprintf(stderr, "Quit will listen on %s\n", quitPortName.c_str());
// LATER: must use a specific robot name to make the port unique.
Terminee terminee(quitPortName.c_str());
// Terminee terminee("/iCubInterface/quit");
if (!terminee.isOk()) {
printf("Failed to create proper quit socket\n");
return 1;
}
IRobotInterface *i;
if (remap)
{
i=new RobotInterfaceRemap;
}
else
{
i=new RobotInterface;
}
ri=i; //set pointer to RobotInterface object (used in the handlers above)
ok = i->initialize(filename);
if (!ok)
return 0;
char c = 0;
printf("Driver instantiated and running (silently)\n");
printf("Checking if you have requested instantiation of cartesian controller\n");
bool someCartesian=false;
if (cartRightArm!="")
{
i->initCart(cartRightArm.c_str());
someCartesian=true;
}
if (cartLeftArm!="")
{
i->initCart(cartLeftArm.c_str());
someCartesian=true;
}
if (!someCartesian)
{
printf("Nothing found\n");
}
while (!terminee.mustQuit()&&!terminated)
{
Time::delay(2);
}
printf("Received a quit message\n");
if (someCartesian)
{
i->finiCart();
}
i->detachWrappers();
i->park(); //default behavior is blocking (safer)
i->closeNetworks();
ri=0; //tell signal handler interface is not longer valid (do this before you destroy i ;)
delete i;
i=0;
return 0;
}
bool configure(ResourceFinder &rf)
{
bool noLegs=rf.check("no_legs");
Property optTorso("(device remote_controlboard)");
Property optArmR("(device remote_controlboard)");
Property optArmL("(device remote_controlboard)");
Property optLegR("(device remote_controlboard)");
Property optLegL("(device remote_controlboard)");
string robot=rf.find("robot").asString().c_str();
optTorso.put("remote",("/"+robot+"/torso").c_str());
optArmR.put("remote",("/"+robot+"/right_arm").c_str());
optArmL.put("remote",("/"+robot+"/left_arm").c_str());
optLegR.put("remote",("/"+robot+"/right_leg").c_str());
optLegL.put("remote",("/"+robot+"/left_leg").c_str());
string local=rf.find("local").asString().c_str();
optTorso.put("local",("/"+local+"/torso").c_str());
optArmR.put("local",("/"+local+"/right_arm").c_str());
optArmL.put("local",("/"+local+"/left_arm").c_str());
optLegR.put("local",("/"+local+"/right_leg").c_str());
optLegL.put("local",("/"+local+"/left_leg").c_str());
optTorso.put("writeStrict","on");
optArmR.put("writeStrict","on");
optArmL.put("writeStrict","on");
optLegR.put("writeStrict","on");
optLegL.put("writeStrict","on");
if (!torso.open(optTorso) || !armR.open(optArmR) || !armL.open(optArmL))
{
cout<<"Device drivers not available!"<<endl;
close();
return false;
}
if (!noLegs)
{
if (!legR.open(optLegR) || !legL.open(optLegL))
{
cout<<"Device drivers not available!"<<endl;
close();
return false;
}
}
PolyDriverList listArmR, listArmL, listLegR, listLegL;
listArmR.push(&torso,"torso");
listArmR.push(&armR,"right_arm");
listArmL.push(&torso,"torso");
listArmL.push(&armL,"left_arm");
listLegR.push(&legR,"right_leg");
listLegL.push(&legL,"left_leg");
Property optServerArmR("(device cartesiancontrollerserver)");
Property optServerArmL("(device cartesiancontrollerserver)");
Property optServerLegR("(device cartesiancontrollerserver)");
Property optServerLegL("(device cartesiancontrollerserver)");
optServerArmR.fromConfigFile(rf.findFile("right_arm_file"),false);
optServerArmL.fromConfigFile(rf.findFile("left_arm_file"),false);
optServerLegR.fromConfigFile(rf.findFile("right_leg_file"),false);
optServerLegL.fromConfigFile(rf.findFile("left_leg_file"),false);
if (!serverArmR.open(optServerArmR) || !serverArmL.open(optServerArmL))
{
close();
return false;
}
if (!noLegs)
{
if (!serverLegR.open(optServerLegR) || !serverLegL.open(optServerLegL))
{
close();
return false;
}
}
IMultipleWrapper *wrapperArmR, *wrapperArmL, *wrapperLegR, *wrapperLegL;
serverArmR.view(wrapperArmR);
serverArmL.view(wrapperArmL);
serverLegR.view(wrapperLegR);
serverLegL.view(wrapperLegL);
if (!wrapperArmR->attachAll(listArmR) || !wrapperArmL->attachAll(listArmL))
{
close();
return false;
}
if (!noLegs)
{
if (!wrapperLegR->attachAll(listLegR) || !wrapperLegL->attachAll(listLegL))
{
close();
return false;
}
}
return true;
}
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
//YARP_REGISTER_DEVICES(icubmod)
Network yarp;
if (!yarp.checkNetwork())
{
LOG_ERROR("Error initializing yarp network (is yarpserver running?)\n");
QMessageBox::critical(0,"Error","Error initializing yarp network (is yarpserver running?)");
return 0;
}
ResourceFinder *finder;
Property p, q;
finder = new ResourceFinder;
//retrieve information for the list of parts
finder->setVerbose();
finder->setDefaultConfigFile("yarpmotorgui.ini");
finder->setDefault("name", "icub");
finder->configure(argc,argv);
qRegisterMetaType<Pid>("Pid");
qRegisterMetaType<SequenceItem>("SequenceItem");
qRegisterMetaType<QList<SequenceItem> >("QList<SequenceItem>");
if (finder->check("calib")){
LOG("Calibrate buttons on\n");
enable_calib_all = true;
}
if (finder->check("admin")){
LOG("Admin mode on.\n");
enable_calib_all = true;
debug_param_enabled = false;
position_direct_enabled = true;
openloop_enabled = true;
old_impedance_enabled = true;
}
if (finder->check("debug")){
LOG("Debug interface requested.\n");
debug_param_enabled = false;
}
if (finder->check("speed")){
LOG("Speed view requested.\n");
speedview_param_enabled = true;
}
if (finder->check("direct")){
LOG("Position direct requested.\n");
position_direct_enabled = true;
}
if (finder->check("openloop")){
LOG("Openloop requested.\n");
openloop_enabled = true;
}
bool deleteParts=false;
std::string robotName=finder->find("name").asString().c_str();
yDebug("Robot name: %s\n",robotName.data());
Bottle *pParts=finder->find("parts").asList();
if (pParts==NULL){
printf("Setting default parts.\n");
pParts=new Bottle("head torso left_arm right_arm left_leg right_leg");
deleteParts=true;
}
NUMBER_OF_AVAILABLE_PARTS=pParts->size();
if (NUMBER_OF_AVAILABLE_PARTS > MAX_NUMBER_ACTIVATED){
LOG_ERROR("The number of parts exceeds the maximum! \n");
return 0;
}
if (NUMBER_OF_AVAILABLE_PARTS<=0){
LOG_ERROR("Invalid number of parts, check config file \n");
return 0;
}
for(int n=0; n < MAX_NUMBER_ACTIVATED; n++){
//ENA = 0: part available
//ENA = -1: part unavailable
//ENA = 1: part used
ENA.append(-1);
}
//Check 1 in the panel
for(int n=0;n<NUMBER_OF_AVAILABLE_PARTS;n++){
QString part = QString("%1").arg(pParts->get(n).asString().c_str());
yDebug("Appending %s",part.toUtf8().constData());
partsName.append(part);
if(n < ENA.count()){
ENA.replace(n,1);
}else{
ENA.append(1);
}
}
QString newRobotName = robotName.data();
if (!finder->check("skip"))
{
StartDlg dlg;
dlg.init(QString(robotName.data()),partsName,ENA);
if(dlg.exec() == QDialog::Accepted){
ENA.clear();
ENA = dlg.getEnabledParts();
newRobotName = dlg.getRobotName();
}else{
yInfo("Cancel Button pressed. Closing..");
return 0;
}
}
QStringList enabledParts;
for(int i=0; i<partsName.count();i++){
if(ENA.at(i) == 1){
enabledParts.append(partsName.at(i));
}
}
yarp::os::signal(yarp::os::YARP_SIGINT, sighandler);
yarp::os::signal(yarp::os::YARP_SIGTERM, sighandler);
MainWindow w;
mainW = &w;
int appRet = 0;
bool ret = w.init(newRobotName,enabledParts,finder,debug_param_enabled,speedview_param_enabled,enable_calib_all,position_direct_enabled,openloop_enabled);
if(ret){
w.show();
appRet = a.exec();
}
delete finder;
if(deleteParts){
delete pParts;
}
return appRet;
}
bool TactileLearning::getFingersAndJoints(ResourceFinder& rf)
{
/**
* There are two mutual exclusive ways to specify which fingers and joints are involved in the problem.
* 1. Checking which fingers are active and setting the joints properly.
* 2. Setting by hand the joint vector.
* In both cases checks for consistency have to be done, which higher priority to the fingers vector.
*/
string tmp;
int jointIndex;
if(!rf.findGroup("FINGERS").isNull())
{
// check which fingers are specified in the configuration file and set the activeFingers vector accordingly
for(int i = 0; i < NUM_FINGERS; i++)
{
tmp = rf.findGroup("FINGERS").find(FINGER_TYPES[i].c_str()).asString();
if(tmp.compare("on") == 0) activeFingers[i] = true;
else activeFingers[i] = false;
}
/**
* Set the joints vector so that it is consistent with the fingers vector values. This step requires
* knowledge about hardware implementation of the robot hand.
*/
jointIndex = 0;
//------------------------------------------------------------------------------------------------------------
// WARNING! The joint of the thumb to be ignored is the 9-th in the simulator, but the 8-th in the real robot!
//------------------------------------------------------------------------------------------------------------
// thumb
if(activeFingers[0] == true)
{
for(int i = 0; i < 3; i++)
{
// for simulator: ignore second joint (9-th for the whole arm)
// for real robot: ignore first joint (8-th for the whole arm)
if(i != 0) activeJoints[jointIndex++] = true;
else activeJoints[jointIndex++] = false;
}
}
else
{
for(int i = 0; i < 3; i++) activeJoints[jointIndex++] = false;
}
// index
if(activeFingers[1] == true)
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
}
else
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
}
// middle
if(activeFingers[2] == true)
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
}
else
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
}
// ring + pinky
if(activeFingers[3] == true) activeJoints[jointIndex] = true;
else activeJoints[jointIndex] = false;
}
else if(rf.check("joints"))
{
Bottle joints = rf.findGroup("joints").tail();
// check which joints are specified in the configuration file and set the activeJoints vector accordingly.
// brute force but who cares
for(int i = 0; i < NUM_HAND_JOINTS; i++)
{
activeJoints[i] = false;
for(int j = 0; j < joints.size(); j++)
{
if(i == joints.get(j).asInt() - HAND_JOINTS_OFFSET)
{
activeJoints[i] = true;
break;
}
}
}
/**
* activate each finger if at least a joint related to it has been activated.
* This step requires knowledge about hardware implementation of the robot hand.
*/
// thumb
if(activeJoints[0] || activeJoints[1] || activeJoints[2]) activeFingers[0] = true;
else activeFingers[0] = false;
// index
if(activeJoints[3] || activeJoints[4]) activeFingers[1] = true;
else activeFingers[1] = false;
// middle
if(activeJoints[5] || activeJoints[6]) activeFingers[2] = true;
else activeFingers[2] = false;
// pinky
if(activeJoints[7]) activeFingers[3] = true;
else activeFingers[3] = false;
}
else
{
printf("ERROR: no fingers were set, please check configuration file.\n");
return false;
}
// check for at least one active finger and in this case print both active fingers and active joints
for(int i = 0; i < NUM_FINGERS; i++)
{
if(activeFingers[i])
{
printFingersAndJoints();
return true;
}
}
// if no fingers are active the initialization is not successful
printf("ERROR: no fingers were set, please check configuration file.\n");
return false;
}
Reactable2OPC::Reactable2OPC()
{
H2ICUB = eye(4);
H2RT = eye(4);
isCalibrated = false;
portCalibration.open("/reactable2opc/calibration:rpc");
w = new OPCClient("reactable2opc");
w->isVerbose = false;
while ( !w->connect("OPC") || !Network::connect("/reactable2opc/calibration:rpc","/referenceFrameHandler/rpc"))
{
cout<<"Waiting for connection to OPC or rfh..."<<endl;
Time::delay(1.0);
}
//
//Load the id/name mapping from a config file
ResourceFinder rf;
rf.setVerbose(false);
rf.setDefaultContext("reactable2opc");
rf.setDefaultConfigFile("mapping.ini");
rf.configure(0,NULL);
bool invertYaxis = rf.check("invertYaxis",Value(0)).asInt() == 1;
bool invertXaxis = rf.check("invertXaxis",Value(0)).asInt() == 1;
if (invertYaxis)
{
cout<<"Using inverted Y axis"<<endl;
YaxisFactor = -1;
}
else
YaxisFactor = 1;
if (invertXaxis)
{
cout<<"Using inverted X axis"<<endl;
XaxisFactor = -1;
}
else
XaxisFactor = 1;
loadObjectsDatabase(rf);
//Open the OSC socket for reception
OSC_INPUT_TABLE_PORT = rf.check("OSC_TABLE_INPUT_PORT", Value(3333)).asInt();
int oscReceiverPort = rf.check("OSC_INPUT_PORT",Value(3334)).asInt();
oscThreadReceiver = new OscThread(w, oscReceiverPort);
oscThreadReceiver->start();
//Open the OSC socket for writing
int oscEmitterPort = rf.check("OSC_OUTPUT_PORT",Value(3335)).asInt();
string oscEmitterEndPoint = rf.check("OSC_OUTPUT_ADRESS",Value("10.0.0.106")).asString().c_str();
oscEmitter = new UdpTransmitSocket(IpEndpointName( oscEmitterEndPoint.c_str(), oscEmitterPort ));
yarp2osc = new DataPort(oscEmitter);
yarp2osc->useCallback();
yarp2osc->open("/reactable2opc/command:i");
cout<<"Targeting OSC to" <<oscEmitterEndPoint<<":"<<oscEmitterPort<< endl;
cout<<"OSC & TUIO listening..." << endl;
checkCalibration();
}
MainWindow(ResourceFinder *rf)
{
//if (VERBOSE) fprintf(stderr, "Passing RF by address ...");
resFind = rf;
//if (VERBOSE) fprintf(stderr, "done \n");
Property options;
options.fromString(resFind->toString());
Value &robot = options.find("robot");
Value &part = options.find("part");
Value &rows = options.find("rows");
Value &cols = options.find("cols");
if (!options.check("robot"))
printf("Missing --robot option. Using icub.\n");
if (!options.check("part"))
printf("Missing --part option. Using head.\n");
if (!options.check("local"))
printf("Missing --name option. Using /portScope/vector:i.\n");
if (!options.check("remote"))
printf("Missing --remote option. Will wait for the connection...\n");
if (!options.check("rows"))
printf("Missing --rows option. Disabling subplotting.\n");
//if (VERBOSE) fprintf(stderr, "Start plotting the GUI\n");
QToolBar *toolBar = new QToolBar(this);
toolBar->setFixedHeight(80);
#if QT_VERSION < 0x040000
setDockEnabled(TornOff, true);
setRightJustification(true);
#else
toolBar->setAllowedAreas(Qt::TopToolBarArea | Qt::BottomToolBarArea);
#endif
QWidget *hBox = new QWidget(toolBar);
QLabel *label = new QLabel("Timer Interval", hBox);
QwtCounter *counter = new QwtCounter(hBox);
QLabel *labelActions = new QLabel("Actions", hBox);
QPushButton *toggleAcquireButton = new QPushButton("stop", hBox, "stop");
counter->setRange(-1.0, 1000.0, 1.0);
QHBoxLayout *layout = new QHBoxLayout(hBox);
layout->addWidget(label);
layout->addWidget(counter);
layout->addWidget(labelActions);
layout->addWidget(toggleAcquireButton);
layout->addWidget(new QWidget(hBox), 10); // spacer);
#if QT_VERSION >= 0x040000
toolBar->addWidget(hBox);
#endif
addToolBar(toolBar);
if (options.check("rows"))
nRows = rows.asInt();
else
nRows = 1;
if (options.check("cols"))
nCols = cols.asInt();
else
nCols = nRows;
nPlots = nRows*nCols;
nPortInputsInPlots = new int[nPlots];
QGrid *grid = new QGrid( nRows, Qt::Vertical, this );
plot = new DataPlot*[nPlots];
setCentralWidget(grid);
for( int r = 0 ; r < nRows ; r++ )
{
for( int c = 0 ; c < nCols ; c++ )
{
char rcS[256];
sprintf(rcS, "%d%d", r, c);
ConstString rS(rcS);
ConstString local;
local = resFind->find("local").toString();
//local port name
local = local + rcS;
//dataPlot creation
plot[r+nRows*c] = new DataPlot(grid);
//dataPlot set plot to read from
setInputPort(resFind, r+nRows*c, local);
//dataPlot set indeces of ports to plot
setIndexMask(resFind, r+nRows*c, nPortInputsInPlots[r+nRows*c]);
}
}
for( int r = 0 ; r < nRows ; r++ )
{
for( int c = 0 ; c < nCols ; c++ )
{
//stop acquisition button
connect( toggleAcquireButton, SIGNAL(clicked()), plot[r+nRows*c], SLOT(toggleAcquire()) );
plot[r+nRows*c]->initSignalDimensions();
//counter button
connect(counter, SIGNAL(valueChanged(double)),
plot[r+nRows*c], SLOT(setTimerInterval(double)) );
counter->setValue(50.0);
QSizePolicy sizePolicy2(QSizePolicy::Maximum, QSizePolicy::Maximum);
grid->setSizePolicy(sizePolicy2);
QSizePolicy sizePolicy(QSizePolicy::Maximum, QSizePolicy::Maximum);
sizePolicy.setHeightForWidth(plot[r+nRows*c]->sizePolicy().hasHeightForWidth());
plot[r+nRows*c]->setSizePolicy(sizePolicy);
//QSize gridSize= grid->sizeHint();
//gridSize.setWidth(500);
//gridSize.setHeight(500);
plot[r+nRows*c]->setMinimumWidth(10);
plot[r+nRows*c]->setMaximumWidth(800);
plot[r+nRows*c]->setMinimumHeight(10);
plot[r+nRows*c]->setMaximumHeight(800);
//if (VERBOSE) fprintf(stderr, "Grid max is: hInt=%d, vInt=%d\n", grid->maximumHeight(), grid->maximumWidth());
//if (VERBOSE) fprintf(stderr, "Plot max is: hInt=%d, vInt=%d\n", plot[r+nRows*c]->maximumHeight(), plot[r+nRows*c]->maximumWidth());
//QSize plotSize= plot[r+nRows*c]->sizeHint();
//if (VERBOSE) fprintf(stderr, "Plot Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width());
}
}
}
bool configure(ResourceFinder &rf)
{
name = "visuoTactileRF";
robot = "icub";
modality = "1D";
verbosity = 0; // verbosity
rate = 20; // rate of the vtRFThread
//******************************************************
//********************** CONFIGS ***********************
//******************* NAME ******************
if (rf.check("name"))
{
name = rf.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (rf.check("robot"))
{
robot = rf.find("robot").asString();
yInfo("Robot is %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//***************** MODALITY *****************
if (rf.check("modality"))
{
modality = rf.find("modality").asString();
yInfo("modality is %s", modality.c_str());
}
else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());
//******************* VERBOSE ******************
if (rf.check("verbosity"))
{
verbosity = rf.find("verbosity").asInt();
yInfo("vtRFThread verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);
//****************** rate ******************
if (rf.check("rate"))
{
rate = rf.find("rate").asInt();
yInfo("vtRFThread working at %i ms", rate);
}
else yInfo("Could not find rate in the config file; using %i ms as default",rate);
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
vector<string> filenames;
int partNum=4;
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
{
if (rf.check("leftHand"))
{
string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("leftForeArm"))
{
string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightHand"))
{
string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightForeArm"))
{
string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
filenames.push_back(filePath);
}
}
}
else
{
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
filenames.push_back(filePath);
}
}
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
//*************** eyes' Resource finder ****************
ResourceFinder gazeRF;
gazeRF.setVerbose(verbosity!=0);
gazeRF.setDefaultContext("iKinGazeCtrl");
robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
gazeRF.configure(0,NULL);
double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();
ResourceFinder eyeAlignRF;
Bottle &camerasGroup = gazeRF.findGroup("cameras");
if(!camerasGroup.isNull())
{
eyeAlignRF.setVerbose(verbosity!=0);
camerasGroup.check("context")?
eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
eyeAlignRF.configure(0,NULL);
}
else
{
yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");
}
//******************************************************
//*********************** THREAD **********************
if( filenames.size() > 0 )
{
vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
filenames, head_version, eyeAlignRF);
if (!vtRFThrd -> start())
{
delete vtRFThrd;
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated!!");
return false;
}
yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
}
else {
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated. No filenames have been given!");
return false;
}
//******************************************************
//************************ PORTS ***********************
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
void setIndexMask(ResourceFinder *rf, int index, int numberOfInputs)
{
int **indexToPlot = NULL;
indexToPlot = new int*[numberOfInputs];
int *sizeIndexToPlot = NULL;
sizeIndexToPlot = new int[numberOfInputs];
if (rf->find("index").isList())
{
Bottle *rrBot;
//if (VERBOSE) fprintf(stderr, "Option remote is a list\n");
rrBot = resFind->find("index").asList();
if (rrBot->size() == nRows*nCols)
{
if (rrBot->get(index).isInt())
{
if (numberOfInputs==1)
{
indexToPlot[0] = new int;
indexToPlot[0][0] = rrBot->get(index).asInt();
sizeIndexToPlot[0] = 1;
}
else
fprintf(stderr, "ERROR: a single index was supplied but more than one input port was given!\n");
}
else if (rrBot->get(index).isList())
{
Bottle *rrrBot;
rrrBot = rrBot->get(index).asList();
if(rrrBot->get(0).isInt() && numberOfInputs==1)
{
indexToPlot[0] = new int[rrrBot->size()];
sizeIndexToPlot[0] = rrrBot->size();
}
for(int j = 0; j < rrrBot->size(); j++)
{
if(rrrBot->get(j).isInt())
{
if (numberOfInputs==1)
{
indexToPlot[0][j] = rrrBot->get(j).asInt();
//if (VERBOSE) fprintf(stderr, "MESSAGE: getting a list of indeces for a plot with a single input. Index(%d)=%d\n", j, indexToPlot[0][j]);
}
}
else if (rrrBot->get(j).isList())
{
if (rrrBot->size() == numberOfInputs)
{
//if (VERBOSE) fprintf(stderr, "MESSAGE: Getting a double list in the --index parameter! \n");
Bottle *rrrrBot;
rrrrBot = rrrBot->get(j).asList();
indexToPlot[j] = new int[rrrrBot->size()];
sizeIndexToPlot[j] = rrrrBot->size();
//if (VERBOSE) fprintf(stderr, "MESSAGE: Plot %d has %d indeces to plot! %s \n", index, sizeIndexToPlot[j], rrrrBot->toString().c_str());
for(int k = 0; k < rrrrBot->size(); k++)
{
if(rrrrBot->get(k).isInt())
indexToPlot[j][k] = rrrrBot->get(k).asInt();
}
//if (VERBOSE) fprintf(stderr, "MESSAGE: the index to plot for input %d was set! \n", j);
}
else
fprintf(stderr, "ERROR: a list of indeces was supplied but its dimension differs from the number of input ports given!\n");
}
}
}
}
else if(rrBot->size() > 1 && rrBot->get(0).isInt())
{
sizeIndexToPlot[index] = rrBot->size();
indexToPlot[index] = new int[sizeIndexToPlot[0]];
for (int i = 0; i < rrBot->size(); i++)
indexToPlot[index][i] = rrBot->get(i).asInt();
}
else
{
sizeIndexToPlot[0] = 1;
indexToPlot[0] = new int;
indexToPlot[0][0] = index;
fprintf(stderr, "WARNING: Ignoring option index since dimensions do not match\n");
}
}
else if (rf->find("index").isInt())
{
//if (VERBOSE) fprintf(stderr, "MESSAGE: was not a list. Initializing with 0\n");
sizeIndexToPlot[0] = 1;
indexToPlot[0] = new int;
indexToPlot[0][0] = rf->find("index").asInt();
}
else
{
//if (VERBOSE) fprintf(stderr, "MESSAGE: was not a list. Initializing with 0\n");
sizeIndexToPlot[0] = 1;
indexToPlot[0] = new int;
indexToPlot[0][0] = index;
}
if (indexToPlot == NULL)
{
//if (VERBOSE) fprintf(stderr, "MESSAGE: was not initialized. Initializing with 0\n");
sizeIndexToPlot[0] = 1;
indexToPlot[0] = new int;
indexToPlot[0][0] = 0;
}
//if (VERBOSE) fprintf(stderr, "Plot %d has %d input/s and the associated indeces to plot are: \n", index, numberOfInputs);
//for (int k = 0; k < numberOfInputs; k++)
// {
// if (VERBOSE) fprintf(stderr, "Input (%d) index/indeces:", k);
// for (int i = 0; i < sizeIndexToPlot[k]; i++)
// if (VERBOSE) fprintf(stderr, " %d ", indexToPlot[k][i]);
// if (VERBOSE) fprintf(stderr, "\n");
// }
plot[index]->setInputPortIndex((int**)indexToPlot, (int*)sizeIndexToPlot, (int)numberOfInputs);
}
bool learnPrimitive::updateProtoAction(ResourceFinder &rf){
Bottle bOutput;
//1. Protoaction
Bottle bProtoAction = rf.findGroup("Proto_Action");
if (!bProtoAction.isNull())
{
Bottle * bProtoActionName = bProtoAction.find("protoActionName").asList();
Bottle * bProtoActionEnd = bProtoAction.find("protoActionEnd").asList();
Bottle * bProtoActionSpeed = bProtoAction.find("protoActionSpeed").asList();
if(bProtoActionName->isNull() || bProtoActionEnd->isNull() || bProtoActionSpeed->isNull()){
yError() << " [updateProtoAction] : one of the protoAction conf is null : protoActionName, protoActionEnd, protoActionSpeed" ;
return false ;
}
int protoActionSize = -1 ;
if(bProtoActionName->size() == bProtoActionEnd->size() && bProtoActionEnd->size() == bProtoActionSpeed->size()){
protoActionSize = bProtoActionName->size() ;
} else {
yError() << " [updateProtoAction] : one of the protoAction conf has different size!" ;
return false ;
}
if(protoActionSize == 0) {
yWarning() << " [updateProtoAction] : there is no protoaction defined at startup!" ;
} else {
for(int i = 0; i < protoActionSize ; i++) {
//insert protoaction even if already there
yInfo() << "ProtoAction added : (" << bProtoActionName->get(i).asString() << ", " << bProtoActionEnd->get(i).asInt() << ", " << bProtoActionSpeed->get(i).asDouble() << ")" ;
mProtoActionEnd[bProtoActionName->get(i).asString()] = bProtoActionEnd->get(i).asInt();
mProtoActionSpeed[bProtoActionName->get(i).asString()] = bProtoActionSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | Proto_Action is NOT defined in the learnPrimitive.ini";
return false;
}
//2. Effect of bodypart
Bottle bBodyPart = rf.findGroup("BodyPart");
if (!bBodyPart.isNull())
{
Bottle * bBodyPartName = bBodyPart.find("bodyPartName").asList();
Bottle * bBodyPartEnd = bBodyPart.find("bodyPartProtoEnd").asList();
Bottle * bBodyPartSpeed = bBodyPart.find("bodyPartProtoSpeed").asList();
//Crash if no match : isnull is not good for that
if(bBodyPartName->isNull() || bBodyPartEnd->isNull() || bBodyPartSpeed->isNull()){
yError() << "[updateProtoAction] : one of the bodyPartProto conf is null : bodyPartName, bodyPartProtoEnd, bodyPartProtoSpeed" ;
return false ;
}
int bodyPartSize = -1 ;
if(bBodyPartName->size() == bBodyPartEnd->size() && bBodyPartEnd->size() == bBodyPartSpeed->size()){
bodyPartSize = bBodyPartName->size() ;
} else {
yError() << "[updateProtoAction] : one of the bodyPartProto conf has different size!" ;
return false ;
}
if(bodyPartSize == 0) {
yWarning() << "[updateProtoAction] : there is no bodyPartProto defined at startup!" ;
} else {
for(int i = 0; i < bodyPartSize ; i++) {
//insert protoaction even if already there
yInfo() << "bodyPartProto added : (" << bBodyPartName->get(i).asString() << ", " << bBodyPartEnd->get(i).asInt() << ", " << bBodyPartSpeed->get(i).asDouble() << ")" ;
mBodyPartEnd[bBodyPartName->get(i).asString()] = bBodyPartEnd->get(i).asInt();
mBodyPartSpeed[bBodyPartName->get(i).asString()] = bBodyPartSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | BodyPart is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}