/*
* Send a rpc command to ABM to obtain the kinematicStructure of part of the body (using the joint info)
* Send a rpc command to kinematicStructure if no kinematicStructure were provided
* input: joint of the bodypart to be moved (e.g. m_joint_number) + body part type (e.g. left_arm, right_arm, ...)
*/
Bottle proactiveTagging::assignKinematicStructureByJoint(int BPjoint, std::string sBodyPartType, bool forcingKS) {
//1. extract instance from singleJointAction for joint BPjoint, from ABM
Bottle bResult, bOutput;
ostringstream osRequest;
osRequest << "SELECT max(main.instance) FROM main, contentarg WHERE main.instance = contentarg.instance AND activityname = 'singleJointBabbling' AND main.begin = TRUE AND contentarg.role = 'limb' AND contentarg.argument = '" << BPjoint << "' ;";
yInfo() << "assignKinematicStructureByJoint request: " << osRequest.str();
bResult = iCub->getABMClient()->requestFromString(osRequest.str().c_str());
if (bResult.toString() == "NULL") {
yError() << " error in proactiveTagging::assignKinematicStructureByJoint | for joint " << BPjoint << " | No instance corresponding to singleJointBabbling for this part" ;
bOutput.addString("error");
bOutput.addString("No instance corresponding to singleJointBabbling for this part");
return bOutput;
}
int ksInstance = atoi(bResult.get(0).asList()->get(0).asString().c_str());
Bottle bResultCheckKS = checkForKinematicStructure(ksInstance, forcingKS);
if(bResultCheckKS.get(0).asString() == "error") {
return bResultCheckKS;
}
yInfo() << " [assignKinematicStructureByJoint] | for joint " << BPjoint << " | instance found : " << ksInstance;
//WRITE IN OPC
iCub->opc->checkout();
list<Entity*> lEntities = iCub->opc->EntitiesCache();
Bottle bListEntChanged;
for (auto& entity: lEntities) //go through all entity
{
yInfo() << "Checking if entity " << entity->name() << " has entitytype = bodypart : ----> " << entity->entity_type() ;
if (entity->entity_type() == "bodypart") //check bodypart entity
{
//pb with the casting: BPtemp is empty
Bodypart* BPtemp = dynamic_cast<Bodypart*>(entity);
if(!BPtemp) {
yDebug() << "Could not cast " << entity->name() << " to Bodypart";
continue;
}
if(BPtemp->m_joint_number == BPjoint) { //if corresponding joint : change it
//BPtemp->m_kinStruct_instance = ksInstance;
bListEntChanged.addString(BPtemp->name());
yInfo() << "Change" << BPtemp->name() << "to kinematic instance" << ksInstance;
iCub->opc->commit(BPtemp);
break;
}
}
}
yInfo() << "Out of the loop for checking entity in OPC, number of entityChanged : " << bListEntChanged.size() ;
if(bListEntChanged.isNull()){
yWarning() << "assignKinematicStructureByJoint | for joint " << BPjoint << " | no bodypart has been found with this joint!";
bOutput.addString("warning");
bOutput.addString("no bodypart has been found with this joint!");
return bOutput;
}
bOutput.addString("ack");
bOutput.addInt(ksInstance);
bOutput.addList() = bListEntChanged;
return bOutput;
}
/*
* Explore an unknown tactile entity (e.g. fingertips), when knowing the name
* @param: Bottle with (exploreTactileUnknownEntity entityType entityName) (eg: exploreUnknownEntity agent unknown_25)
* @return Bottle with the result (error or ack?)
*/
yarp::os::Bottle proactiveTagging::exploreTactileEntityWithName(Bottle bInput) {
Bottle bOutput;
if (bInput.size() != 3)
{
yInfo() << " proactiveTagging::exploreTactileEntityWithName | Problem in input size.";
bOutput.addString("Problem in input size");
iCub->say("Error in input size explore tactile with name");
return bOutput;
}
string sBodyPart = bInput.get(1).toString();
string sName = bInput.get(2).toString();
yInfo() << " EntityType : " << sBodyPart;
//1. search through opc for the bodypart entity
iCub->opc->checkout();
Bodypart* BPentity = dynamic_cast<Bodypart*>(iCub->opc->getEntity(sName, true));
if(!BPentity) {
iCub->say("Could not cast to bodypart in tactile");
yError() << "Could not cast to bodypart in tactile";
bOutput.addString("nack");
return bOutput;
}
//2.Ask human to touch
string sAsking = "I know how to move my " + sName + ", but how does it feel when I touch something? Can you touch my " + sName + " when I move it, please?";
yInfo() << " sAsking: " << sAsking;
iCub->lookAtPartner();
iCub->say(sAsking, false);
portFromTouchDetector.read(false); // clear buffer from previous readings
//2.b Move also the bodypart to show it has been learnt.
yInfo() << "Cast okay : name BP = " << BPentity->name();
int joint = BPentity->m_joint_number;
//send rpc command to bodySchema to move the corresponding part
yInfo() << "Start bodySchema";
double babbling_duration = 3.0;
iCub->babbling(joint, babblingArm, babbling_duration);
//3. Read until some tactile value are detected
//TODO: Here, instead we should check the saliency given by pasar!
bool gotTouch = false;
Bottle *bTactile;
int timeout = 0;
while(!gotTouch && timeout<10) {
bTactile = portFromTouchDetector.read(false);
if(bTactile != NULL) {
gotTouch = true;
break;
} else {
yarp::os::Time::delay(0.5);
timeout++;
}
}
if(!gotTouch) {
yError() << " error in proactiveTagging::exploreTactileEntityWithName | for " << sName << " | Touch not detected!" ;
bOutput.addString("error");
bOutput.addString("I did not feel any touch.");
iCub->say("I did not feel any touch.");
iCub->home();
return bOutput;
}
//4. Assign m_tactile_number
BPentity->m_tactile_number = bTactile->get(0).asInt();
bOutput.addString("ack");
bOutput.addInt(bTactile->get(0).asInt());
yDebug() << "Assigned" << BPentity->name() << "(id)" << BPentity->opc_id() << "tactile_number to" << bTactile->get(0).asInt();
iCub->opc->commit(BPentity);
//4.Ask human to touch
string sThank = " Thank you, now I know when I am touching object with my " + sName;
yInfo() << " sThank: " << sThank;
iCub->lookAtPartner();
iCub->say(sThank);
iCub->home();
return bOutput;
}