void makeTrajVariablesAndBounds(int n_steps, RobotAndDOFPtr manip, OptProb& prob_out, VarArray& vars_out) { int n_dof = manip->GetDOF(); DblVec lower, upper; manip->GetDOFLimits(lower, upper); IPI_LOG_INFO("Dof limits: %s, %s", Str(lower), Str(upper)); vector<double> vlower, vupper; vector<string> names; for (int i=0; i < n_steps; ++i) { vlower.insert(vlower.end(), lower.data(), lower.data()+lower.size()); vupper.insert(vupper.end(), upper.data(), upper.data()+upper.size()); for (unsigned j=0; j < n_dof; ++j) { names.push_back( (boost::format("j_%i_%i")%i%j).str() ); } } prob_out.createVariables(names, vlower, vupper); vars_out = VarArray(n_steps, n_dof, prob_out.getVars().data()); }
KS_RESULT IFBS_DIR(KscServerBase* Server, FbDirParams &Pars, PltList<ObjProps> &Res) /******************************************************************************/ { KsGetEPParams params; /* Parameter für getPP-Service */ KsGetEPResult result; //char help[256]; KS_RESULT err = 0; PltString log(""); if(!Server) { err = KS_ERR_SERVERUNKNOWN; iFBS_SetLastError(1, err, log); return err; } if( !Pars.path.len() ) { log = "\"%s\" \"\""; err = KS_ERR_BADNAME; iFBS_SetLastError(1, err, log); return err; } iFBS_SetLastError(1, err, log); //strcpy(help, (const char*)Pars.path); params.path = Pars.path; if(Pars.name_mask != "") params.name_mask = Pars.name_mask; else params.name_mask = "*"; //cout << "\n Name-Mask ist >" << params.name_mask << "<\n"; params.type_mask = 0; if( Pars.type_mask == FB_OBJ_ANY ) { params.type_mask = KS_OT_ANY; } else { if( Pars.type_mask & FB_OBJ_HISTORY ) params.type_mask |= KS_OT_HISTORY; if( Pars.type_mask & FB_OBJ_VARIABLE ) params.type_mask |= KS_OT_VARIABLE; if( Pars.type_mask & FB_OBJ_LINK ) params.type_mask |= KS_OT_LINK; if( (Pars.type_mask & FB_OBJ_INSTANCE) || (Pars.type_mask & FB_OBJ_TASK) || (Pars.type_mask & FB_OBJ_LIBRARY) || (Pars.type_mask & FB_OBJ_CLASS) || (Pars.type_mask & FB_OBJ_CONNECTION) || (Pars.type_mask & FB_OBJ_CONTAINER) ) params.type_mask |= KS_OT_DOMAIN; } if(params.type_mask == 0) params.type_mask = KS_OT_ANY; params.scope_flags = Pars.scope_flags; bool ok = Server->getEP(0, params, result); if ( !ok ) { log = "\"%s\" \""; log += Pars.path; log += "\""; err = Server->getLastResult(); if(err == KS_ERR_OK) err = KS_ERR_GENERIC; iFBS_SetLastError(1, err, log); return KS_ERR_GENERIC; } if ( result.result ) { err = result.result; log = "\"%s\" \""; log += Pars.path; log += "\""; iFBS_SetLastError(1, err, log); return err; } /* Sonst ist getPP-Dienst ohne Fehler ausgefuehrt worden und die Rueckgabe- */ /* Parameter koennen generiert werden : */ PltList<KsEngPropsHandle> ListVars; size_t i = 0; /* Laufvariable */ size_t Anz ; /* Merker : Anzahl gefundenen Objekten */ Anz = result.items.size(); if(!Anz) { // Keine Objekten gefungen return KS_ERR_OK; } PltArray<KscVariable*> VarArray(Anz); if(VarArray.size() != Anz) { err = OV_ERR_HEAPOUTOFMEMORY; log = ""; iFBS_SetLastError(1, err, log); return err; } KsString root = Server->getHostAndName(); KsString Var; KscPackage *Vpkg = new KscPackage; if(!Vpkg) { err = OV_ERR_HEAPOUTOFMEMORY; iFBS_SetLastError(1, err, log); return err; } while ( result.items.size() ) { ObjProps item; KsEngPropsHandle hpp = result.items.removeFirst(); if(!hpp) { delete Vpkg; err = KS_ERR_GENERIC; iFBS_SetLastError(1, err, log); return err; } if(hpp->xdrTypeCode() == KS_OT_HISTORY) { item.obj_type = FB_OBJ_HISTORY; item.identifier = (const char*)hpp->identifier; item.creation_time = ((KsHistoryEngProps &)(*hpp)).creation_time; item.comment = (const char*)((KsHistoryEngProps &)(*hpp)).comment; item.access = ((KsHistoryEngProps &)(*hpp)).access_mode; item.flags = ((KsHistoryEngProps &)(*hpp)).semantic_flags; item.class_ident = (const char*)((KsHistoryEngProps &)(*hpp)).type_identifier; item.value = ""; item.tech_unit = ""; item.var_type = 0; item.state = 0; item.len = 1; Res.addLast(item); continue; } if(hpp->xdrTypeCode() == KS_OT_DOMAIN) { if( (Pars.type_mask & FB_OBJ_INSTANCE ) && ( ((KsDomainEngProps &)(*hpp)).class_identifier != CONTAINER_CLASS_PATH ) && ( ((KsDomainEngProps &)(*hpp)).class_identifier != TASK_CLASS_PATH ) && ( ((KsDomainEngProps &)(*hpp)).class_identifier != LIBRARY_FACTORY_PATH ) && ( ((KsDomainEngProps &)(*hpp)).class_identifier != CLASS_CLASS_PATH ) && ( ((KsDomainEngProps &)(*hpp)).class_identifier != CONNECTION_CLASS_PATH ) ) { item.obj_type = FB_OBJ_INSTANCE; } else { if( ( ((KsDomainEngProps &)(*hpp)).class_identifier == CONTAINER_CLASS_PATH ) && (Pars.type_mask & FB_OBJ_CONTAINER) ) { item.obj_type = FB_OBJ_CONTAINER; } else { if( ( ((KsDomainEngProps &)(*hpp)).class_identifier == TASK_CLASS_PATH ) && (Pars.type_mask & FB_OBJ_TASK) ) { item.obj_type = FB_OBJ_TASK; } else { if( ( ((KsDomainEngProps &)(*hpp)).class_identifier == LIBRARY_FACTORY_PATH ) && (Pars.type_mask & FB_OBJ_LIBRARY) ) { item.obj_type = FB_OBJ_LIBRARY; } else { if( ( ((KsDomainEngProps &)(*hpp)).class_identifier == CLASS_CLASS_PATH ) && (Pars.type_mask & FB_OBJ_CLASS) ) { item.obj_type = FB_OBJ_CLASS; } else { if( ( ((KsDomainEngProps &)(*hpp)).class_identifier == CONNECTION_CLASS_PATH ) && (Pars.type_mask & FB_OBJ_CONNECTION) ) { item.obj_type = FB_OBJ_CONNECTION; } else { continue; } } } } } } item.identifier = (const char*)hpp->identifier; item.creation_time = ((KsDomainEngProps &)(*hpp)).creation_time; item.comment = (const char*)((KsDomainEngProps &)(*hpp)).comment; item.access = ((KsDomainEngProps &)(*hpp)).access_mode; item.flags = ((KsDomainEngProps &)(*hpp)).semantic_flags; item.class_ident = (const char*)((KsDomainEngProps &)(*hpp)).class_identifier; item.value = ""; item.tech_unit = ""; item.var_type = 0; item.state = 0; item.len = 1; Res.addLast(item); /* if DOMAIN */ } else { if( (hpp->xdrTypeCode() == KS_OT_VARIABLE) || (hpp->xdrTypeCode() == KS_OT_LINK) ) { ListVars.addLast(hpp); Var = params.path; if( Pars.path.substr(0,7) == "/vendor" ) Var += "/"; else Var += "."; Var += hpp->identifier; /* /. */ VarArray[i] = new KscVariable(root+Var); if(!VarArray[i]) { delete Vpkg; err = OV_ERR_HEAPOUTOFMEMORY; iFBS_SetLastError(1, err, log); return err; } if(!Vpkg->add(KscVariableHandle(VarArray[i], PltOsNew)) ){ delete Vpkg; err = 1; log = ""; iFBS_SetLastError(1, err, log); return err; } i++; // Naechste Variable } } /* else (also keine DOMAIN) */ } /* while */ if(!ListVars.isEmpty() ) { if(!Vpkg->getUpdate() ) { err = Vpkg->getLastResult(); if(err == KS_ERR_OK) err = KS_ERR_GENERIC; delete Vpkg; log = ""; iFBS_SetLastError(1, err, log); return err; } } i = 0; while(!ListVars.isEmpty() ) { ObjProps itemv; KsEngPropsHandle pv = ListVars.removeFirst(); if(!pv) { err = KS_ERR_GENERIC; iFBS_SetLastError(1, err, log); return err; } const KsVarCurrProps *cp = (VarArray[i])->getCurrProps(); if(!cp) { err = KS_ERR_GENERIC; log = ""; iFBS_SetLastError(1, err, log); delete Vpkg; return err; } GetPortProps( pv, cp, itemv); Res.addLast(itemv); i++; // Naechste Variable } /* Speicher freigeben */ delete Vpkg; return KS_ERR_OK; }