void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf)
{
//Initialise the gestures response
Bottle grpGesture = rf.findGroup("GESTURES");
Bottle *gestureStimulus = grpGesture.find("stimuli").asList();
if (gestureStimulus)
{
for(int d=0; d<gestureStimulus->size(); d++)
{
string gestureStimulusName = gestureStimulus->get(d).asString().c_str();
StimulusEmotionalResponse response;
Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList();
for(int s=0;s<bSentences->size();s++)
{
response.m_sentences.push_back(bSentences->get(s).asString().c_str());
}
std::string sGroupTemp = gestureStimulusName;
sGroupTemp += "-effect";
Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList();
for(int i=0; bEffects && i<bEffects->size(); i += 2)
{
response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble();
}
gestureEffects[gestureStimulusName] = response;
}
}
}
void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf)
{
//Initialise the tactile response
Bottle grpTactile = rf.findGroup("TACTILE");
Bottle *tactileStimulus = grpTactile.find("stimuli").asList();
if (tactileStimulus)
{
for (int d = 0; d<tactileStimulus->size(); d++)
{
string tactileStimulusName = tactileStimulus->get(d).asString().c_str();
StimulusEmotionalResponse response;
Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList();
for (int s = 0; s<bSentences->size(); s++)
{
response.m_sentences.push_back(bSentences->get(s).asString().c_str());
}
//choregraphies
Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList();
for (int sC = 0; bChore && sC<bChore->size(); sC++)
{
response.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
}
std::string sGroupTemp = tactileStimulusName;
sGroupTemp += "-effect";
Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList();
for (int i = 0; bEffects && i<bEffects->size(); i += 2)
{
response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
}
tactileEffects[tactileStimulusName] = response;
}
}
}
void ICubClient::LoadChoregraphies(yarp::os::ResourceFinder &rf)
{
choregraphiesKnown.clear();
int posCount = rf.check("choregraphiesCount", yarp::os::Value(0)).asInt();
cout<<"Loading Choregraphies: "<<endl;
for (int i = 0; i < posCount; i++)
{
std::stringstream ss;
ss<<"chore_" << i;
Bottle postureGroup = rf.findGroup(ss.str().c_str());
std::string name = postureGroup.find("name").asString().c_str();
std::cout<<"\t"<<name<<std::endl;
Bottle* sequence = postureGroup.find("sequence").asList();
std::list< std::pair<std::string, double> > seq;
for(int s=0; s<sequence->size(); s++)
{
Bottle* element = sequence->get(s).asList();
std::string elementName = element->get(0).asString().c_str();
double elementTime = element->get(1).asDouble();
seq.push_back(std::pair<std::string,double>(elementName,elementTime));
//std::cout<<"\t \t"<<elementName<< "\t" << elementTime << std::endl;
}
choregraphiesKnown[name] = seq;
}
}
bool Action::fromBottle(const Bottle &b)
{
if (!this->Entity::fromBottle(b))
return false;
if (!b.check("description")||!b.check("subactions"))
return false;
Bottle* bDesc = b.find("description").asList();
initialDescription.fromBottle(*bDesc);
this->subActions.clear();
Bottle* bSub = b.find("subactions").asList();
for(int i=0; i<bSub->size(); i++)
{
Action a;
a.fromBottle(*bSub->get(i).asList());
this->subActions.push_back(a);
}
this->estimatedDriveEffects.clear();
bSub = b.find("estimatedDriveEffects").asList();
for(int i=0; i<bSub->size(); i++)
{
string driveName = bSub->get(i).asList()->get(0).asString().c_str();
double driveEffect = bSub->get(i).asList()->get(1).asDouble();
this->estimatedDriveEffects[driveName] = driveEffect;
}
return true;
}
void ICubClient::LoadPostures(yarp::os::ResourceFinder &rf)
{
posturesKnown.clear();
int posCount = rf.check("posturesCount", yarp::os::Value(0)).asInt();
//cout<<"Loading posture: "<<endl;
for (int i = 0; i < posCount; i++)
{
std::stringstream ss;
ss<<"posture_" << i;
Bottle postureGroup = rf.findGroup(ss.str().c_str());
BodyPosture p;
std::string name = postureGroup.find("name").asString().c_str();
//std::cout<<"\t"<<name<<std::endl;
Bottle* bHead = postureGroup.find("head").asList();
Bottle* bLArm = postureGroup.find("left_arm").asList();
Bottle* bRArm = postureGroup.find("right_arm").asList();
Bottle* bTorso = postureGroup.find("torso").asList();
p.head.resize(6);
for(int i=0;i<6;i++)
p.head[i] = bHead->get(i).asDouble();
p.left_arm.resize(16);
for(int i=0;i<16;i++)
p.left_arm[i] = bLArm->get(i).asDouble();
p.right_arm.resize(16);
for(int i=0;i<16;i++)
p.right_arm[i] = bRArm->get(i).asDouble();
p.torso.resize(3);
for(int i=0;i<3;i++)
p.torso[i] = bTorso->get(i).asDouble();
posturesKnown[name] = p;
}
}
void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
;
//Initialise the gestures response
Bottle grpSocial = rf.findGroup("SOCIAL");
salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
Bottle *socialStimulus = grpSocial.find("stimuli").asList();
if (socialStimulus)
{
for (int d = 0; d<socialStimulus->size(); d++)
{
string socialStimulusName = socialStimulus->get(d).asString().c_str();
StimulusEmotionalResponse response;
Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
for (int s = 0; s<bSentences->size(); s++)
{
response.m_sentences.push_back(bSentences->get(s).asString().c_str());
}
std::string sGroupTemp = socialStimulusName;
sGroupTemp += "-effect";
Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
for (int i = 0; bEffects && i<bEffects->size(); i += 2)
{
response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
}
salutationEffects[socialStimulusName] = response;
}
}
//Add the relevant Entities for handling salutation
iCub->opc->addOrRetrieveAction("is");
iCub->opc->addOrRetrieveAdjective("saluted");
}
bool MapGrid2D::loadROSParams(string ros_yaml_filename, string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t )
{
std::string file_string;
std::ifstream file;
file.open(ros_yaml_filename.c_str());
if (!file.is_open())
{
yError() << "failed to open file" << ros_yaml_filename;
return false;
}
string line;
while (getline(file, line))
{
if (line.find("origin") != std::string::npos)
{
std::replace(line.begin(), line.end(), ',', ' ');
std::replace(line.begin(), line.end(), '[', '(');
std::replace(line.begin(), line.end(), ']', ')');
/*
auto it = line.find('[');
if (it != string::npos) line.replace(it, 1, "(");
it = line.find(']');
if(it != string::npos) line.replace(it, 1, ")");*/
}
file_string += (line + '\n');
}
file.close();
bool ret = true;
Bottle bbb;
bbb.fromString(file_string);
string debug_s = bbb.toString();
if (bbb.check("image:") == false) { yError() << "missing image"; ret = false; }
pgm_occ_filename = bbb.find("image:").asString();
//ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size()-4))+"_yarpflags"+".ppm";
if (bbb.check("resolution:") == false) { yError() << "missing resolution"; ret = false; }
resolution = bbb.find("resolution:").asDouble();
if (bbb.check("origin:") == false) { yError() << "missing origin"; ret = false; }
Bottle* b = bbb.find("origin:").asList();
if (b)
{
orig_x = b->get(0).asDouble();
orig_y = b->get(1).asDouble();
orig_t = b->get(2).asDouble();
}
if (bbb.check("occupied_thresh:"))
{m_occupied_thresh = bbb.find("occupied_thresh:").asDouble();}
if (bbb.check("free_thresh:"))
{m_free_thresh = bbb.find("free_thresh:").asDouble();}
return ret;
}
void getArmDependentOptions(Bottle &b, Vector &_gOrien, Vector &_gDisp,
Vector &_dOffs, Vector &_dLift, Vector &_home_x)
{
if (Bottle *pB=b.find("grasp_orientation").asList())
{
int sz=pB->size();
int len=_gOrien.length();
int l=len<sz?len:sz;
for (int i=0; i<l; i++)
_gOrien[i]=pB->get(i).asDouble();
}
if (Bottle *pB=b.find("grasp_displacement").asList())
{
int sz=pB->size();
int len=_gDisp.length();
int l=len<sz?len:sz;
for (int i=0; i<l; i++)
_gDisp[i]=pB->get(i).asDouble();
}
if (Bottle *pB=b.find("systematic_error_displacement").asList())
{
int sz=pB->size();
int len=_dOffs.length();
int l=len<sz?len:sz;
for (int i=0; i<l; i++)
_dOffs[i]=pB->get(i).asDouble();
}
if (Bottle *pB=b.find("lifting_displacement").asList())
{
int sz=pB->size();
int len=_dLift.length();
int l=len<sz?len:sz;
for (int i=0; i<l; i++)
_dLift[i]=pB->get(i).asDouble();
}
if (Bottle *pB=b.find("home_position").asList())
{
int sz=pB->size();
int len=_home_x.length();
int l=len<sz?len:sz;
for (int i=0; i<l; i++)
_home_x[i]=pB->get(i).asDouble();
}
}
bool homeostaticModule::configure(yarp::os::ResourceFinder &rf)
{
manager = homeostasisManager();
moduleName = rf.check("name",Value("homeostasis")).asString().c_str();
setName(moduleName.c_str());
cout<<moduleName<<": finding configuration files..."<<endl;
period = rf.check("period",Value(0.1)).asDouble();
cout << "Initializing drives";
Bottle grpHomeostatic = rf.findGroup("HOMEOSTATIC");
Bottle *drivesList = grpHomeostatic.find("drives").asList();
cout << "Initializing Drives... " << endl;
if (drivesList)
{
cout << "Configuration: Found " << drivesList->size() << " drives. " << endl;
for (int d = 0; d<drivesList->size(); d++)
{
cout << d << endl;
//Read Drive Configuration
string driveName = drivesList->get(d).asString().c_str();
addNewDrive(driveName, grpHomeostatic);
}
}
cout << "Opening RPC..."<< endl;
rpc.open ( ("/"+moduleName+"/rpc").c_str());
attach(rpc);
cout<<"Configuration done."<<endl;
return true;
}
//---------------------------------------------------------
void readBool(Bottle &rf, string name, bool &v, bool vdefault)
{
if(rf.check(name.c_str()))
{
if((rf.find(name.c_str()).asString()=="true")||(rf.find(name.c_str()).asString()=="on"))
v = true;
else
v = false;
}
else
{
v = vdefault;
cout<<"Could not find value true/false for "<<name<<". "
<<"Setting default "<<((vdefault==true)?"true":"false")<<endl;
}
displayNameValue(name,((v==true)?"true":"false"));
}
bool Agent::fromBottle(Bottle b)
{
if (!this->Object::fromBottle(b))
return false;
if (!b.check("belief")||!b.check("emotions"))
return false;
m_belief.clear();
Bottle* beliefs = b.find("belief").asList();
for(int i=0; i<beliefs->size() ; i++)
{
Bottle* bRelation = beliefs->get(i).asList();
Relation r(*bRelation);
m_belief.push_back(r);
}
m_emotions_intrinsic.clear();
Bottle* emotions = b.find("emotions").asList();
for(int i=0; i<emotions->size() ; i++)
{
Bottle* bEmo = emotions->get(i).asList();
string emotionName = bEmo->get(0).asString().c_str();
double emotionValue = bEmo->get(1).asDouble();
m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
}
m_drives.clear();
Bottle* drivesProperty = b.find("drives").asList();
string drivesDebug = drivesProperty->toString().c_str();
for(int i=0; i<drivesProperty->size() ; i++)
{
Bottle* bD = drivesProperty->get(i).asList();
string drivesDebug1 = bD->toString().c_str();
Drive currentDrive;
currentDrive.fromBottle(*bD);
m_drives[currentDrive.name] = currentDrive;
}
Bottle* bodyProperty = b.find("body").asList();
m_body.fromBottle(*bodyProperty);
return true;
}
bool Adjective::fromBottle(Bottle b)
{
if (!this->Entity::fromBottle(b))
return false;
if (!b.check("qualityType"))
return false;
m_quality = b.find("qualityType").asString().c_str();
return true;
}
bool getProjectionMatrix(Bottle b, Matrix &P)
{
double fx,fy,cx,cy;
if(b.check("fx"))
fx = b.find("fx").asDouble();
else
return 0;
if(b.check("fy"))
fy = b.find("fy").asDouble();
else
return 0;
// we suppose that the center distorsion is already compensated
if(b.check("w"))
cx = b.find("w").asDouble()/2.0;
else
return 0;
if(b.check("h"))
cy = b.find("h").asDouble()/2.0;
else
return 0;
double sth= 0.0; double sx = fx; double sy = fy;
double ox = cx; double oy = cy;
double f = 1.0;
Matrix K = eye(3,3);
K(0,0)=sx*f; K(1,1)=sy*f; K(0,1)=sth*f; K(0,2)=ox; K(1,2)=oy;
Matrix Pi = zeros(3,4);
Pi(0,0)=1.0; Pi(1,1)=1.0; Pi(2,2)=1.0;
P = K*Pi;
//fprintf(stderr, "Working with Projection %s\n", P.toString().c_str());
return 1;
}
void Localizer::setPidOptions(const Bottle &options)
{
mutex.lock();
pid->setOptions(options);
if (options.check("dominantEye"))
{
string domEye=options.find("dominantEye").asString().c_str();
if ((domEye=="left") || (domEye=="right"))
dominantEye=domEye;
}
mutex.unlock();
}
//---------------------------------------------------------
void readInt(Bottle &rf, string name, int &v, int vdefault)
{
if(rf.check(name.c_str()))
{
v = rf.find(name.c_str()).asInt();
}
else
{
v = vdefault;
cout<<"Could not find value for "<<name<<". "
<<"Setting default "<<vdefault<<endl;
}
displayNameValue(name,v);
}
void Reactable2OPC::loadObjectsDatabase(ResourceFinder& rf)
{
int objectsCount = rf.find("objectsCount").asInt();
for(int i=0;i<objectsCount;i++)
{
ostringstream oGrpName; oGrpName<<"object_"<<i;
Bottle cObject = rf.findGroup(oGrpName.str().c_str());
string oName = cObject.find("name").asString().c_str();
int id = cObject.find("id").asInt();
cout<<"Mapping ("<<id<<" , "<<oName<<")"<<endl;
idMap[id] = oName;
//Store the offset of the marker locally
Bottle* bOffset = cObject.find("marker-offset").asList();
idOffsets[id].resize(3);
idOffsets[id][0] = bOffset->get(0).asDouble();
idOffsets[id][1] = bOffset->get(1).asDouble();
idOffsets[id][2] = bOffset->get(2).asDouble();
//Push the object to OPC
RTObject* o = w->addOrRetrieveEntity<RTObject>(oName);
Bottle* bDim = cObject.find("dimensions").asList();
o->m_dimensions[0] = bDim->get(0).asDouble();
o->m_dimensions[1] = bDim->get(1).asDouble();
o->m_dimensions[2] = bDim->get(2).asDouble();
Bottle* bColor = cObject.find("color").asList();
o->m_color[0] = bColor->get(0).asDouble();
o->m_color[1] = bColor->get(1).asDouble();
o->m_color[2] = bColor->get(2).asDouble();
o->m_present = 0.0;
w->commit(o);
//cout<<o->toString()<<endl;
}
}
string IOL2OPCBridge::findName(const Bottle &scores,
const string &tag)
{
string retName=OBJECT_UNKNOWN;
double maxScore=0.0;
Bottle *blobScores=scores.find(tag.c_str()).asList();
if (blobScores==NULL)
return retName;
// first find the most likely object for the given blob
for (int i=0; i<blobScores->size(); i++)
{
Bottle *item=blobScores->get(i).asList();
if (item==NULL)
continue;
string name=item->get(0).asString().c_str();
double score=item->get(1).asDouble();
if (score>maxScore)
{
maxScore=score;
retName=name;
}
}
// then double-check that the found object remains the best
// prediction over the remaining blobs
if (retName!=OBJECT_UNKNOWN)
{
for (int i=0; i<scores.size(); i++)
{
if (Bottle *blob=scores.get(i).asList())
{
// skip the blob under examination
string name=blob->get(0).asString().c_str();
Bottle *blobScores=blob->get(1).asList();
if ((name==tag) || (blobScores==NULL))
continue;
if (blobScores->find(retName.c_str()).asDouble()>=maxScore)
return OBJECT_UNKNOWN;
}
}
}
return retName;
}
bool helperPID::getVectorFromOption(const Bottle &options, const char *key,
Vector &val, int &size)
{
if (options.check(key))
{
if (Bottle *b=options.find(key).asList())
{
int len=(int)val.length();
int bSize=b->size();
size=bSize<len?bSize:len;
for (int i=0; i<size; i++)
val[i]=b->get(i).asDouble();
return true;
}
}
return false;
}
bool CalibModule::getGazeParams(const string &eye, const string &type, Matrix &M)
{
if (((eye!="left") && (eye!="right")) ||
((type!="intrinsics") && (type!="extrinsics")))
return false;
Bottle info;
igaze->getInfo(info);
if (Bottle *pB=info.find(("camera_"+type+"_"+eye).c_str()).asList())
{
M.resize((type=="intrinsics")?3:4,4);
int cnt=0;
for (int r=0; r<M.rows(); r++)
for (int c=0; c<M.cols(); c++)
M(r,c)=pB->get(cnt++).asDouble();
return true;
}
else
return false;
}
bool yarp::os::impl::HttpCarrier::write(Protocol& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString body = b.find("web").toString();
if (body.length()!=0) {
ConstString header;
header += NetType::toHexString(body.length()).c_str();
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
} else {
ConstString txt = b.toString() + "\r\n";
ConstString header;
header += NetType::toHexString(txt.length()).c_str();
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
Bytes b3((char*)txt.c_str(),txt.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
}
proto.os().flush();
return proto.os().isOk();
}
void look()
{
Bottle cmd;
Bottle response;
cmd.addString("look");
port.write(cmd,response);
// pick out map part
world= response.findGroup("look").findGroup("map");
Bottle &users = response.findGroup("look").findGroup("players");
Bottle *player = users.get(1).asList();
if (player!=0)
{
Bottle &location = player->findGroup("location");
Value &life = player->find("life");
std::string playerName = player->get(0).asString();
myX=location.get(1).asInt32(),
myY=location.get(2).asInt32(),
myLife=life.asInt32();
}
}
Relation AdaptiveLayer::getRelationFromSemantic(Bottle b)
{
string s = "none";
string v = "none";
string o = "none";
string p = "none";
string t = "none";
string m = "none";
if (b.check("groupSubject"))
s = getEntityFromWordGroup(b.find("groupSubject").asList());
if (b.check("groupVerbal"))
v = getEntityFromWordGroup(b.find("groupVerbal").asList());
if (b.check("groupObject"))
o = getEntityFromWordGroup(b.find("groupObject").asList());
if (b.check("groupPlace"))
p = getEntityFromWordGroup(b.find("groupPlace").asList());
if (b.check("groupTime"))
t = getEntityFromWordGroup(b.find("groupTime").asList());
if (b.check("groupManner"))
m = getEntityFromWordGroup(b.find("groupManner").asList());
return Relation(s,v,o,p,t,m);
}
bool ClientCartesianController::open(Searchable &config)
{
ConstString remote, local, carrier;
if (config.check("remote"))
remote=config.find("remote").asString();
else
return false;
if (config.check("local"))
local=config.find("local").asString();
else
return false;
carrier=config.check("carrier",Value("udp")).asString();
if (config.check("timeout"))
timeout=config.find("timeout").asDouble();
portCmd.open((local+"/command:o").c_str());
portState.open((local+"/state:i").c_str());
portEvents.open((local+"/events:i").c_str());
portRpc.open((local+"/rpc:o").c_str());
bool ok=true;
ok&=Network::connect(portRpc.getName().c_str(),(remote+"/rpc:i").c_str());
if (ok)
{
Bottle info;
getInfoHelper(info);
if (info.check("server_version"))
{
double server_version=info.find("server_version").asDouble();
if (server_version!=CARTCTRL_CLIENT_VER)
{
yError("version mismatch => server(%g) != client(%g); please update accordingly",
server_version,CARTCTRL_CLIENT_VER);
return false;
}
}
else
yWarning("unable to retrieve server version; please update the server");
}
else
{
yError("unable to connect to the server rpc port!");
return false;
}
ok&=Network::connect(portCmd.getName().c_str(),(remote+"/command:i").c_str(),carrier.c_str());
ok&=Network::connect((remote+"/state:o").c_str(),portState.getName().c_str(),carrier.c_str());
ok&=Network::connect((remote+"/events:o").c_str(),portEvents.getName().c_str(),carrier.c_str());
// check whether the solver is alive and connected
if (ok)
{
Bottle command, reply;
command.addVocab(IKINCARTCTRL_VOCAB_CMD_GET);
command.addVocab(IKINCARTCTRL_VOCAB_OPT_ISSOLVERON);
if (!portRpc.write(command,reply))
{
yError("unable to get reply from server!");
close();
return false;
}
if (reply.get(0).asVocab()==IKINCARTCTRL_VOCAB_REP_ACK)
if (reply.size()>1)
if (reply.get(1).asVocab()==IKINCARTCTRL_VOCAB_VAL_TRUE)
return connected=true;
yError("unable to connect to solver!");
close();
return false;
}
else
{
yError("unable to connect to server!");
close();
return false;
}
}
bool BehaviorManager::configure(yarp::os::ResourceFinder &rf)
{
moduleName = rf.check("name",Value("BehaviorManager")).asString();
setName(moduleName.c_str());
yInfo()<<moduleName<<": finding configuration files...";//<<endl;
period = rf.check("period",Value(1.0)).asDouble();
Bottle grp = rf.findGroup("BEHAVIORS");
Bottle behaviorList = *grp.find("behaviors").asList();
rpc_in_port.open("/" + moduleName + "/trigger:i");
yInfo() << "RPC_IN : " << rpc_in_port.getName();
for (int i = 0; i<behaviorList.size(); i++)
{
behavior_name = behaviorList.get(i).asString();
if (behavior_name == "tagging") {
behaviors.push_back(new Tagging(&mut, rf, "tagging"));
} else if (behavior_name == "pointing") {
behaviors.push_back(new Pointing(&mut, rf, "pointing"));
} else if (behavior_name == "dummy") {
behaviors.push_back(new Dummy(&mut, rf, "dummy"));
} else if (behavior_name == "dummy2") {
behaviors.push_back(new Dummy(&mut, rf, "dummy2"));
} else if (behavior_name == "reactions") {
behaviors.push_back(new Reactions(&mut, rf, "reactions"));
} else if (behavior_name == "followingOrder") {
behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder"));
} else if (behavior_name == "narrate") {
behaviors.push_back(new Narrate(&mut, rf, "narrate"));
} else if (behavior_name == "recognitionOrder") {
behaviors.push_back(new RecognitionOrder(&mut, rf, "recognitionOrder"));
} else if (behavior_name == "greeting") {
behaviors.push_back(new Greeting(&mut, rf, "greeting"));
} else if (behavior_name == "ask") {
behaviors.push_back(new Ask(&mut, rf, "ask"));
} else if (behavior_name == "speech") {
behaviors.push_back(new Speech(&mut, rf, "speech"));
} else if (behavior_name == "moveObject") {
behaviors.push_back(new MoveObject(&mut, rf, "moveObject"));
// other behaviors here
} else {
yDebug() << "Behavior " + behavior_name + " not implemented";
return false;
}
}
//Create an iCub Client and check that all dependencies are here before starting
bool isRFVerbose = false;
iCub = new wysiwyd::wrdac::ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);
if (!iCub->connect())
{
yInfo() << "iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
if (rf.check("use_ears",Value("false")).asBool())
{
yDebug()<<"using ears";
while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
yWarning() << "Ears is not reachable";
yarp::os::Time::delay(0.5);
}
}else{
yDebug()<<"not using ears";
}
// id = 0;
for(auto& beh : behaviors) {
beh->configure();
beh->openPorts(moduleName);
beh->iCub = iCub;
if (beh->from_sensation_port_name != "None") {
while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " << beh->sensation_port_in.getName();// <<endl;
yarp::os::Time::delay(0.5);
}
}
if (beh->external_port_name != "None") {
while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) {
yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " << beh->external_port_name;// <<endl;
yarp::os::Time::delay(0.5);
}
}
}
attach(rpc_in_port);
yInfo("Init done");
return true;
}
bool embObjAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("verbose").asBool())
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
bool ret;
Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
if(groupTransceiver.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find TRANSCEIVER group in xml config files";
return false;
}
Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
if(groupProtocol.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find PROTOCOL group in xml config files";
return false;
}
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
_fId.pc104IPaddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
_fId.boardIPaddr.port = port;
sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);
// debug info
memset(info, 0x00, sizeof(info));
snprintf(info, sizeof(info), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address);
snprintf(_fId.name, sizeof(_fId.name), "%s", info); // Saving User Friendly Id
// Set dummy values
_fId.boardNumber = FEAT_boardnumber_dummy;
Value val = config.findGroup("ETH").check("Ems", Value(1), "Board number");
if(val.isInt())
_fId.boardNumber = val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "embObjAnalogSensor::open() cannot instantiate ethManager";
return false;
}
// N.B.: use a dynamic_cast<> to extract correct interface when using this pointer
_fId.interface = this;
_fId.endpoint = eoprot_endpoint_analogsensors;
if(AS_STRAIN == _as_type)
_fId.type = ethFeatType_AnalogStrain;
else if(AS_MAIS == _as_type)
_fId.type = ethFeatType_AnalogMais;
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boardNum to the ethManager in order to create the ethResource requested.
*/
res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
if(NULL == res)
{
yError() << "embObjAnalogSensor::open() fails because could not instantiate the ethResource board" << _fId.boardNumber << " ... unable to continue";
return false;
}
if(false == res->isEPmanaged(eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() detected that EMS "<< _fId.boardNumber << " does not support analog sensors";
cleanup();
return false;
}
if(false == res->verifyBoard(groupProtocol))
{
yError() << "embObjAnalogSensor::open() fails in function verifyBoard() for board " << _fId.boardNumber << ": CANNOT PROCEED ANY FURTHER";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " is communicating correctly";
}
}
if(!res->verifyEPprotocol(groupProtocol, eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() fails in function verifyEPprotocol() for board "<< _fId.boardNumber << ": either the board cannot be reached or it does not have the same eoprot_endpoint_management and/or eoprot_endpoint_analogsensors protocol version: DO A FW UPGRADE";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " has same protocol version for analogsensors as robotInterface";
}
}
// // marco.accame on 04 sept 2014: we could add a verification about the entities of analog ... MAYBE in the future
//
// uint8_t numberofmaisboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_mais);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_mais, numberofmaisboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE MAIS.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_mais: VERIFY their number in board, and in XML files";
// return false;
// }
// uint8_t numberofstrainboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_strain);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_strain, numberofstrainboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE STRAIN.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_strain: VERIFY their number in board, and in XML files";
// return false;
// }
data = new AnalogData(_channels, _channels+1);
scaleFactor = new double[_channels];
int i=0;
for (i=0; i<_channels; i++) scaleFactor[i]=1;
// Real values will be read from the sensor itself during its initalization hereafter
for(int i=0; i<_channels; i++)
{
scaleFactor[i]=1;
}
//#warning --> marco.accame on 04sept14: both embObjAnalogSensors and embObjMotionControl initialises the mais board. but potentially w/ different params (mc uses: datarate = 10 and mode = eoas_maismode_txdatacontinuously).
switch(_as_type)
{
case AS_MAIS:
{
ret = sendConfig2Mais();
} break;
case AS_STRAIN:
{
ret = sendConfig2Strain();
} break;
default:
{
//i should not be here. if AS_NONE then i should get error in fromConfig function
ret = false;
}
}
if(!ret)
{
cleanup();
return false;
}
// Set variable to be signalled
if(! init())
{
cleanup();
return false;
}
if(false == res->goToRun())
{
yError() << "embObjAnalogSensor::open() fails to start control loop of board" << _fId.boardNumber << ": cannot continue";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() correctly activated control loop of BOARD" << _fId.boardNumber;
}
}
opened = true;
return true;
}
bool homeostaticModule::respond(const Bottle& cmd, Bottle& reply)
{
if (cmd.get(0).asString() == "help" )
{ string help = "\n";
help += " ['par'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Assigns a value to a specific parameter \n";
help += " ['delta'] ['drive'] ['val'/'min'/'max'/'dec'] [value] : Adds a value to a specific parameter \n";
help += " ['add'] ['conf'] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n";
help += " ['add'] ['botl'] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n";
help += " ['add'] ['new'] [drive name] : Adds a default drive to the manager \n";
help += " ['rm'] [drive name] : removes a drive from the manager \n";
help += " ['names'] : returns an ordered list of the drives in the manager \n";
help += " : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n";
reply.addString(help);
/*cout << " [par] [drive] [val/min/max/dec] [value] : Assigns a value to a specific parameter \n"<<
" [delta] [drive] [val/min/max/dec] [value] : Adds a value to a specific parameter \n"<<
" [add] [conf] [drive Bottle] : Adds a drive to the manager as a drive directly read from conf-file \n"<<
" [add] [botl] [drive Bottle] : Adds a drive to the manager as a Bottle of values of shape \n"<<
" : (string name, double value, double homeo_min, double homeo_max, double decay = 0.05, bool gradient = true) \n"<<endl;
*/
}
else if (cmd.get(0).asString() == "par" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->setValue(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="min")
{
manager.drives[d]->setHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="max")
{
manager.drives[d]->setHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2).asString()=="dec")
{
manager.drives[d]->setDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]");
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
reply.addString("nack: wrong drive name");
}
else if (cmd.get(0).asString() == "delta" )
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
if (cmd.get(2).asString()=="val")
{
manager.drives[d]->deltaValue(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="min")
{
manager.drives[d]->deltaHomeostasisMin(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="max")
{
manager.drives[d]->deltaHomeostasisMax(cmd.get(3).asDouble());
}
else if (cmd.get(2)=="dec")
{
manager.drives[d]->deltaDecay(cmd.get(3).asDouble());
}
else
{
reply.addString("nack");
cout << "Format is: \n - ['par'] [drive_name] [val/min/max/dec] [value]"<<endl;
}
reply.addString("ack");
cout<<"Received a order"<<endl;
}
}
}
else if (cmd.get(0).asString()=="add")
{
if (cmd.get(1).asString()=="conf")
{
Bottle *ga = cmd.get(2).asList();
// cout <<"HOMEOSTATIC"<< ga->toString() << endl;
Bottle grpAllostatic = ga->findGroup("ALLOSTATIC");
cout <<"HOMEOSTATIC"<< grpAllostatic.toString() << endl;
cout << "Bottle allo check: " << grpAllostatic.find("test-decay").asDouble() << endl;
addNewDrive(cmd.get(2).check("name",yarp::os::Value("")).asString(), grpAllostatic);
reply.addString("add drive from config bottle: ack");
}
else if (cmd.get(1).asString()=="botl")
{
Drive d = bDrive(cmd.get(2).asList());
manager.addDrive(&d);
openPorts(d.name);
reply.addString("add drive from bottle: ack");
}
else if (cmd.get(1).asString()=="new")
{
string d_name = cmd.get(2).asString();
bool b = addNewDrive(d_name);
if (b)
reply.addString("add new drive: ack");
else
reply.addString("ack. drive not created");
}
else
{
reply.addString("nack");
}
}
else if (cmd.get(0).asString()=="rm")
{
for (unsigned int d = 0; d<manager.drives.size();d++)
{
if (cmd.get(1).asString() == manager.drives[d]->name)
{
bool b = removeDrive(d);
if (b)
reply.addString("ack: Successfully removed");
else
reply.addString("ack: Could not remove the drive");
}
}
reply.addString("nack");
}
else if (cmd.get(0).asString()=="names")
{
Bottle nms;
nms.clear();
for (unsigned int i=0;i<manager.drives.size();i++)
{
nms.addString(manager.drives[i]->name);
}
reply.addList()=nms;
}
else
{
reply.addString("nack");
}
return true;
}
bool yarp::os::impl::HttpCarrier::reply(Protocol& proto, SizedWriter& writer) {
DummyConnector con;
con.setTextMode(true);
for (size_t i=writer.headerLength(); i<writer.length(); i++) {
con.getWriter().appendBlock(writer.data(i),writer.length(i));
}
Bottle b;
b.read(con.getReader());
ConstString mime = b.check("mime",Value("text/html")).asString();
ConstString body;
bool using_json = false;
if (stream!=NULL) {
if (stream->useJson()) {
mime = "text/json";
asJson(body,&b,stream->typeHint());
using_json = true;
}
}
if (b.check("web")&&!using_json) {
body = b.find("web").toString();
}
if (b.check("stream")&&!using_json) {
String header("HTTP/1.1 200 OK\r\nContent-Type: ");
header += mime;
header += "\r\n";
header += "Transfer-Encoding: chunked\r\n";
header += "\r\n";
int N = 2*1024;
header += NetType::toHexString(body.length()+N);
header += "\r\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
// chrome etc won't render until enough chars are received.
for (int i=0; i<N; i++) {
proto.os().write(' ');
}
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
proto.os().write('\r');
proto.os().write('\n');
if (stream!=NULL) {
stream->flip();
}
return true;
}
if (stream!=NULL) {
stream->finish();
}
// Could check response codes, mime types here.
if (body.length()!=0 || using_json) {
ConstString mime = b.check("mime",Value("text/html")).asString();
String header("HTTP/1.1 200 OK\nContent-Type: ");
header += mime;
header += "\n";
header += "Access-Control-Allow-Origin: *\n";
header += "\n";
Bytes b2((char*)header.c_str(),header.length());
proto.os().write(b2);
//body = b.toString();
Bytes b3((char*)body.c_str(),body.length());
proto.os().write(b3);
} else {
writer.write(proto.os());
}
proto.os().flush();
return proto.os().isOk();
}
void AdaptiveLayer::configureOPC(yarp::os::ResourceFinder &rf)
{
//Populate the OPC if required
cout<<"Populating OPC";
Bottle grpOPC = rf.findGroup("OPC");
bool shouldPopulate = grpOPC.find("populateOPC").asInt() == 1;
if (shouldPopulate)
{
Bottle *agentList = grpOPC.find("agent").asList();
if (agentList)
{
for(int d=0; d<agentList->size(); d++)
{
string name = agentList->get(d).asString().c_str();
Agent* agent = iCub->opc->addOrRetrieveEntity<Agent>(name);
agent->m_present = false;
iCub->opc->commit(agent);
}
}
Bottle *objectList = grpOPC.find("object").asList();
if (objectList)
{
for(int d=0; d<objectList->size(); d++)
{
string name = objectList->get(d).asString().c_str();
Object* o = iCub->opc->addOrRetrieveEntity<Object>(name);
o->m_present = false;
iCub->opc->commit(o);
}
}
Bottle *rtobjectList = grpOPC.find("rtobject").asList();
if (rtobjectList)
{
for(int d=0; d<rtobjectList->size(); d++)
{
string name = rtobjectList->get(d).asString().c_str();
RTObject* o = iCub->opc->addOrRetrieveEntity<RTObject>(name);
o->m_present = false;
iCub->opc->commit(o);
}
}
Bottle *adjectiveList = grpOPC.find("adjective").asList();
if (adjectiveList)
{
for(int d=0; d<adjectiveList->size(); d++)
{
string name = adjectiveList->get(d).asString().c_str();
iCub->opc->addOrRetrieveEntity<Adjective>(name);
}
}
Bottle *actionList = grpOPC.find("action").asList();
if (actionList)
{
for(int d=0; d<actionList->size(); d++)
{
string name = actionList->get(d).asString().c_str();
iCub->opc->addOrRetrieveEntity<Action>(name);
}
}
}
cout<<"done"<<endl;
}
bool embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("Verbose").asInt())
_verbose = true;
if(_verbose)
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%s", parameter1.toString().c_str());
_fId.PC104ipAddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%s", parameter2.toString().c_str());
_fId.EMSipAddr.port = port;
sscanf(_fId.EMSipAddr.string,"\"%d.%d.%d.%d", &_fId.EMSipAddr.ip1, &_fId.EMSipAddr.ip2, &_fId.EMSipAddr.ip3, &_fId.EMSipAddr.ip4);
sscanf(_fId.PC104ipAddr.string,"\"%d.%d.%d.%d", &_fId.PC104ipAddr.ip1, &_fId.PC104ipAddr.ip2, &_fId.PC104ipAddr.ip3, &_fId.PC104ipAddr.ip4);
snprintf(_fId.EMSipAddr.string, sizeof(_fId.EMSipAddr.string), "%u.%u.%u.%u:%u", _fId.EMSipAddr.ip1, _fId.EMSipAddr.ip2, _fId.EMSipAddr.ip3, _fId.EMSipAddr.ip4, _fId.EMSipAddr.port);
snprintf(_fId.PC104ipAddr.string, sizeof(_fId.PC104ipAddr.string), "%u.%u.%u.%u:%u", _fId.PC104ipAddr.ip1, _fId.PC104ipAddr.ip2, _fId.PC104ipAddr.ip3, _fId.PC104ipAddr.ip4, _fId.PC104ipAddr.port);
// Debug info
snprintf(_fId.name, sizeof(_fId.name), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNum, address); // Saving User Friendly Id
// Set dummy values
_fId.boardNum = FEAT_boardnumber_dummy;
_fId.ep = eoprot_endpoint_none;
Value val =config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNum =val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.PC104ipAddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "Unable to instantiate ethManager";
return false;
}
//N.B.: use a dynamic_cast to extract correct interface when using this pointer
_fId.handle = (this);
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boradNum to the ethManagerin order to create the ethResource requested.
* I'll Get back the very same sturct filled with other data useful for future handling
* like the EPvector and EPhash_function */
res = ethManager->requestResource(&_fId);
if(NULL == res)
{
yError() << "EMS device not instantiated... unable to continue";
return false;
}
/*IMPORTANT: implement isEpManagedByBoard like every embObj obj when virtaulAnalogSensor will be exist in eo proto!!!!*/
// if(!isEpManagedByBoard())
// {
// yError() << "EMS "<< _fId.boardNum << "is not connected to virtual analog sensor";
// return false;
// }
yTrace() << "EmbObj Virtual Analog Sensor for board "<< _fId.boardNum << "instantiated correctly";
return true;
}
void seriesPID::setOptions(const Bottle &options)
{
Vector satLimVect(satLim.rows()*satLim.cols());
for (int r=0; r<satLim.rows(); r++)
for (int c=0; c<satLim.cols(); c++)
satLimVect[r*satLim.cols()+c]=satLim(r,c);
bool recomputeQuantities=false;
int size;
if (getVectorFromOption(options,"Kp",Kp,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"Ti",Ti,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"Kd",Kd,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"N",N,size))
recomputeQuantities=true;
if (getVectorFromOption(options,"satLim",satLimVect,size))
{
for (int r=0; r<satLim.rows(); r++)
for (int c=0; c<satLim.cols(); c++)
satLimVect[r*satLim.cols()+c]=satLim(r,c);
recomputeQuantities=true;
}
if (options.check("Ts"))
{
double _Ts=options.find("Ts").asDouble();
if (_Ts>0.0)
{
Ts=_Ts;
recomputeQuantities=true;
}
}
if (recomputeQuantities)
{
for (unsigned int i=0; i<dim; i++)
uSat[i]=PID_SAT(uSat[i],satLim(i,0),satLim(i,1));
Vector u0(1,0.0);
Vector num(2),den(2);
for (unsigned int i=0; i<dim; i++)
{
num[0]=Ts; num[1]=Ts;
den[0]=Ts+2.0*Ti[i]; den[1]=Ts-2.0*Ti[i];
Int[i]->adjustCoeffs(num,den);
Int[i]->init(u0);
if ((Kp[i]!=0.0) && (N[i]!=0.0))
{
double tau=Kd[i]/(Kp[i]*N[i]);
num[0]=2.0; num[1]=-2.0;
den[0]=Ts+2.0*tau; den[1]=Ts-2.0*tau;
}
else
{
num[0]=num[1]=den[1]=0.0;
den[0]=1.0;
}
Der[i]->adjustCoeffs(num,den);
Der[i]->init(u0);
}
}
if (options.check("reset"))
reset();
}