Values AsebaNetworkInterface::GetVariable(const QString& node, const QString& variable, const QDBusMessage &message)
{
// make sure the node exists
NodesNamesMap::const_iterator nodeIt(nodesNames.find(node));
if (nodeIt == nodesNames.end())
{
DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("node %0 does not exists").arg(node)));
return Values();
}
const unsigned nodeId(nodeIt.value());
unsigned pos(unsigned(-1));
unsigned length(unsigned(-1));
// check whether variable is user-defined
const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(node));
if (userVarMapIt != userDefinedVariablesMap.end())
{
const VariablesMap& userVarMap(userVarMapIt.value());
const VariablesMap::const_iterator userVarIt(userVarMap.find(variable.toStdWString()));
if (userVarIt != userVarMap.end())
{
pos = userVarIt->second.first;
length = userVarIt->second.second;
}
}
// if variable is not user-defined, check whether it is provided by this node
if (pos == unsigned(-1))
{
bool ok1, ok2;
pos = getVariablePos(nodeId, variable.toStdWString(), &ok1);
length = getVariableSize(nodeId, variable.toStdWString(), &ok2);
if (!(ok1 && ok2))
{
DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("variable %0 does not exists in node %1").arg(variable).arg(node)));
return Values();
}
}
// send request to aseba network
{
GetVariables msg(nodeId, pos, length);
hub->sendMessage(msg);
}
// build bookkeeping for async reply
RequestData *request = new RequestData;
request->nodeId = nodeId;
request->pos = pos;
message.setDelayedReply(true);
request->reply = message.createReply();
pendingReads.push_back(request);
return Values();
}
void AsebaNetworkInterface::SetVariable(const QString& node, const QString& variable, const Values& data, const QDBusMessage &message) const
{
// make sure the node exists
NodesNamesMap::const_iterator nodeIt(nodesNames.find(node));
if (nodeIt == nodesNames.end())
{
DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("node %0 does not exists").arg(node)));
return;
}
const unsigned nodeId(nodeIt.value());
unsigned pos(unsigned(-1));
// check whether variable is user-defined
const UserDefinedVariablesMap::const_iterator userVarMapIt(userDefinedVariablesMap.find(node));
if (userVarMapIt != userDefinedVariablesMap.end())
{
const VariablesMap& userVarMap(userVarMapIt.value());
const VariablesMap::const_iterator userVarIt(userVarMap.find(variable.toStdWString()));
if (userVarIt != userVarMap.end())
{
pos = userVarIt->second.first;
}
}
// if variable is not user-defined, check whether it is provided by this node
if (pos == unsigned(-1))
{
bool ok;
pos = getVariablePos(nodeId, variable.toStdWString(), &ok);
if (!ok)
{
DBusConnectionBus().send(message.createErrorReply(QDBusError::InvalidArgs, QString("variable %0 does not exists in node %1").arg(variable).arg(node)));
return;
}
}
SetVariables msg(nodeId, pos, toAsebaVector(data));
hub->sendMessage(msg);
}
// Utility: find variable address
bool HttpInterface::getNodeAndVarPos(const string& nodeName, const string& variableName,
unsigned& nodeId, unsigned& pos)
{
// make sure the node exists
bool ok;
nodeId = getNodeId(UTF8ToWString(nodeName), 0, &ok);
if (!ok)
{
if (verbose)
wcerr << "invalid node name " << UTF8ToWString(nodeName) << endl;
return false;
}
pos = unsigned(-1);
// check whether variable is known from a compilation, if so, get position
const NodeNameVariablesMap::const_iterator allVarMapIt(allVariables.find(nodeName));
if (allVarMapIt != allVariables.end())
{
const VariablesMap& varMap(allVarMapIt->second);
const VariablesMap::const_iterator varIt(varMap.find(UTF8ToWString(variableName)));
if (varIt != varMap.end())
pos = varIt->second.first;
}
// if variable is not user-defined, check whether it is provided by this node
if (pos == unsigned(-1))
{
bool ok;
pos = getVariablePos(nodeId, UTF8ToWString(variableName), &ok);
if (!ok)
{
if (verbose)
wcerr << "no variable " << UTF8ToWString(variableName) << " in node " << UTF8ToWString(nodeName);
return false;
}
}
return true;
}
/*
* Use gradient-projection to solve Variable Placement with Separation Constraints problem.
*/
int
constrained_majorization_vpsc(CMajEnvVPSC * e, float *b, float *place,
int max_iterations)
{
int i, j, counter;
float *g, *old_place, *d;
/* for laplacian computation need number of real vars and those
* dummy vars included in lap
*/
int n = e->nv + e->nldv;
boolean converged = FALSE;
#ifdef CONMAJ_LOGGING
static int call_no = 0;
#endif /* CONMAJ_LOGGING */
if (max_iterations == 0)
return 0;
g = e->fArray1;
old_place = e->fArray2;
d = e->fArray3;
/* fprintf(stderr,"Entered: constrained_majorization_vpsc, #constraints=%d\n",e->m); */
if (e->m > 0) {
for (i = 0; i < n; i++) {
setVariableDesiredPos(e->vs[i], place[i]);
}
/* fprintf(stderr," calling satisfyVPSC...\n"); */
satisfyVPSC(e->vpsc);
for (i = 0; i < n; i++) {
place[i] = getVariablePos(e->vs[i]);
/* fprintf(stderr,"vs[%d]=%f\n",i,place[i]); */
}
/* fprintf(stderr," done.\n"); */
}
#ifdef CONMAJ_LOGGING
float prev_stress = 0;
for (i = 0; i < n; i++) {
prev_stress += 2 * b[i] * place[i];
for (j = 0; j < n; j++) {
prev_stress -= e->A[i][j] * place[j] * place[i];
}
}
FILE *logfile = fopen("constrained_majorization_log", "a");
/* fprintf(logfile,"grad proj %d: stress=%f\n",call_no,prev_stress); */
#endif
for (counter = 0; counter < max_iterations && !converged; counter++) {
float test = 0;
float alpha, beta;
float numerator = 0, denominator = 0, r;
/* fprintf(stderr,"."); */
converged = TRUE;
/* find steepest descent direction */
for (i = 0; i < n; i++) {
old_place[i] = place[i];
g[i] = 2 * b[i];
for (j = 0; j < n; j++) {
g[i] -= 2 * e->A[i][j] * place[j];
}
}
for (i = 0; i < n; i++) {
numerator += g[i] * g[i];
r = 0;
for (j = 0; j < n; j++) {
r += 2 * e->A[i][j] * g[j];
}
denominator -= r * g[i];
}
if (denominator != 0)
alpha = numerator / denominator;
else
alpha = 1.0;
for (i = 0; i < n; i++) {
place[i] -= alpha * g[i];
}
if (e->m > 0) {
/* project to constraint boundary */
for (i = 0; i < n; i++) {
setVariableDesiredPos(e->vs[i], place[i]);
}
satisfyVPSC(e->vpsc);
for (i = 0; i < n; i++) {
place[i] = getVariablePos(e->vs[i]);
}
}
/* set place to the intersection of old_place-g and boundary and
* compute d, the vector from intersection pnt to projection pnt
*/
for (i = 0; i < n; i++) {
d[i] = place[i] - old_place[i];
}
/* now compute beta */
numerator = 0, denominator = 0;
for (i = 0; i < n; i++) {
numerator += g[i] * d[i];
r = 0;
for (j = 0; j < n; j++) {
r += 2 * e->A[i][j] * d[j];
}
denominator += r * d[i];
}
if (denominator != 0.0)
beta = numerator / denominator;
else
beta = 1.0;
for (i = 0; i < n; i++) {
/* beta > 1.0 takes us back outside the feasible region
* beta < 0 clearly not useful and may happen due to numerical imp.
*/
if (beta > 0 && beta < 1.0) {
place[i] = old_place[i] + beta * d[i];
}
test += fabs(place[i] - old_place[i]);
}
#ifdef CONMAJ_LOGGING
float stress = 0;
for (i = 0; i < n; i++) {
stress += 2 * b[i] * place[i];
for (j = 0; j < n; j++) {
stress -= e->A[i][j] * place[j] * place[i];
}
}
fprintf(logfile, "%d: stress=%f, test=%f, %s\n", call_no, stress,
test, (stress >= prev_stress) ? "No Improvement" : "");
prev_stress = stress;
#endif
if (test > quad_prog_tol) {
converged = FALSE;
}
}
#ifdef CONMAJ_LOGGING
call_no++;
fclose(logfile);
#endif
return counter;
}
