cimSLPService
getSLPData(cimomConfig cfg, const CMPIBroker *_broker,
const CMPIContext *ctx, const char *urlsyntax)
{
CMPIInstance **ci;
//CMPIStatus status;
//CMPIConstClass *ccls;
cimSLPService rs; // service which is going to be returned
// to the calling function
char *sn;
#ifdef SLP_HOSTNAME_LIB
static void *hostnameLib = NULL;
static getSlpHostname gethostname;
char *ln;
char dlName[512];
int err;
err = 1;
if (getControlChars("slpHostnamelib", &ln) == 0) {
libraryName(NULL, ln, dlName, 512);
if ((hostnameLib = dlopen(dlName, RTLD_LAZY))) {
gethostname = dlsym(hostnameLib, "_sfcGetSlpHostname");
if (gethostname)
err = 0;
}
}
if (err)
mlogf(M_ERROR, M_SHOW,
"--- SLP Hostname exit %s not found. Defaulting to system hostname.\n",
dlName);
#endif
_SFCB_ENTER(TRACE_SLP, "getSLPData");
memset(&rs, 0, sizeof(cimSLPService));
// first of all, get the interop namespace, needed for all further
// connections
// this call fills the array as well as sets the global interOpNS
// variable
rs.InteropSchemaNamespace = getInterOpNS();
// extract all relavant stuff for SLP out of CIM_ObjectManager
// construct the server string
ci = myGetInstances(_broker, ctx, interOpNS, "CIM_ObjectManager",
urlsyntax);
if (ci) {
sn = myGetProperty(ci[0], "SystemName");
#ifdef SLP_HOSTNAME_LIB
if (!err) {
char *tmp;
if ((err = gethostname(&tmp))) {
free(sn);
sn = tmp;
} else {
printf
("-#- SLP call to %s for hostname failed. Defaulting to system hostname.\n",
dlName);
}
}
#endif
rs.url_syntax = getUrlSyntax(sn, cfg.commScheme, cfg.port);
rs.service_hi_name = myGetProperty(ci[0], "ElementName");
rs.service_hi_description = myGetProperty(ci[0], "Description");
rs.service_id = myGetProperty(ci[0], "Name");
freeInstArr(ci);
}
// extract all relavant stuff for SLP out of
// CIM_ObjectManagerCommunicationMechanism
ci = myGetInstances(_broker, ctx, interOpNS,
"CIM_ObjectManagerCommunicationMechanism",
urlsyntax);
if (ci) {
rs.CommunicationMechanism =
myGetProperty(ci[0], "CommunicationMechanism");
rs.OtherCommunicationMechanismDescription =
myGetProperty(ci[0], "OtherCommunicationMechanism");
rs.ProtocolVersion = myGetProperty(ci[0], "Version");
rs.FunctionalProfilesSupported =
myGetPropertyArray(ci[0], "FunctionalProfilesSupported");
rs.FunctionalProfileDescriptions =
myGetPropertyArray(ci[0], "FunctionalProfileDescriptions");
rs.MultipleOperationsSupported =
myGetProperty(ci[0], "MultipleOperationsSupported");
rs.AuthenticationMechanismsSupported =
myGetPropertyArray(ci[0], "AuthenticationMechanismsSupported");
rs.AuthenticationMechansimDescriptions =
myGetPropertyArray(ci[0], "AuthenticationMechansimDescriptions");
// do the transformations from numbers to text via the qualifiers
//CMPIStatus myRc;
//CMPIObjectPath *myOp = CMGetObjectPath(ci[0], &myRc);
//CMPIData qd = CMGetPropertyQualifier(myOp, "CommunicationMechanism", "ValueMap", &myRc);
rs.CommunicationMechanism = transformValue(rs.CommunicationMechanism,
CMGetObjectPath(ci[0], NULL),
"CommunicationMechanism");
rs.FunctionalProfilesSupported =
transformValueArray(rs.FunctionalProfilesSupported,
CMGetObjectPath(ci[0], NULL),
"FunctionalProfilesSupported");
rs.AuthenticationMechanismsSupported =
transformValueArray(rs.AuthenticationMechanismsSupported,
CMGetObjectPath(ci[0], NULL),
"AuthenticationMechanismsSupported");
freeInstArr(ci);
}
// extract all relavant stuff for SLP out of CIM_Namespace
ci = myGetInstances(_broker, ctx, interOpNS, "CIM_Namespace", urlsyntax);
if (ci) {
rs.Namespace = myGetPropertyArrayFromArray(ci, "Name");
rs.Classinfo = myGetPropertyArrayFromArray(ci, "ClassInfo");
freeInstArr(ci);
}
// extract all relavant stuff for SLP out of CIM_RegisteredProfile
//CMPIContext *ctxLocal = prepareUpcall(ctx);
//ci = myGetInstances(_broker, ctxLocal, interOpNS, "CIM_RegisteredProfile", urlsyntax);
ci = myGetInstances(_broker, ctx, interOpNS, "CIM_RegisteredProfile", urlsyntax);
if (ci) {
rs.RegisteredProfilesSupported = myGetRegProfiles(_broker, ci, ctx);
//rs.RegisteredProfilesSupported = myGetRegProfiles(_broker, ci, ctxLocal);
freeInstArr(ci);
}
//CMRelease(ctxLocal);
_SFCB_RETURN(rs);
}
std::shared_ptr<drake::lcmt_qp_controller_input> encodeQPInputLCM(const mxArray *qp_input) {
// Take a matlab data structure corresponding to a QPInputConstantHeight object and parse it down to its representation as an equivalent LCM message.
std::shared_ptr<drake::lcmt_qp_controller_input> msg (new drake::lcmt_qp_controller_input());
msg->timestamp = (int64_t) (mxGetScalar(myGetProperty(qp_input, "timestamp")) * 1000000);
const mxArray* zmp_data = myGetProperty(qp_input, "zmp_data");
matlabToCArrayOfArrays<4, 4>(zmp_data, 0, "A", &msg->zmp_data.A[0][0]);
matlabToCArrayOfArrays<4, 2>(zmp_data, 0, "B", &msg->zmp_data.B[0][0]);
matlabToCArrayOfArrays<2, 4>(zmp_data, 0, "C", &msg->zmp_data.C[0][0]);
matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "D", &msg->zmp_data.D[0][0]);
matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "x0", &msg->zmp_data.x0[0][0]);
matlabToCArrayOfArrays<2, 1>(zmp_data, 0, "y0", &msg->zmp_data.y0[0][0]);
matlabToCArrayOfArrays<2, 1>(zmp_data, 0, "u0", &msg->zmp_data.u0[0][0]);
matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "R", &msg->zmp_data.R[0][0]);
matlabToCArrayOfArrays<2, 2>(zmp_data, 0, "Qy", &msg->zmp_data.Qy[0][0]);
matlabToCArrayOfArrays<4, 4>(zmp_data, 0, "S", &msg->zmp_data.S[0][0]);
matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "s1", &msg->zmp_data.s1[0][0]);
matlabToCArrayOfArrays<4, 1>(zmp_data, 0, "s1dot", &msg->zmp_data.s1dot[0][0]);
msg->zmp_data.s2 = mxGetScalar(myGetField(zmp_data, "s2"));
msg->zmp_data.s2dot = mxGetScalar(myGetField(zmp_data, "s2dot"));
msg->zmp_data.timestamp = msg->timestamp;
const mxArray* support_data = myGetProperty(qp_input, "support_data");
int nsupp = mxGetN(support_data);
msg->num_support_data = (int32_t) nsupp;
double double_logic_map[4][1];
msg->support_data.resize(nsupp);
if (nsupp > 0) {
if (mxGetM(support_data) != 1) {
mexErrMsgTxt("support data should be a struct array with M=1");
}
for (int i=0; i < nsupp; i++) {
msg->support_data[i].timestamp = msg->timestamp;
msg->support_data[i].body_id = (int32_t) mxGetScalar(myGetField(support_data, i, "body_id"));
const mxArray *contact_pts = myGetField(support_data, i, "contact_pts");
if (!contact_pts) mexErrMsgTxt("couldn't get points");
Map<MatrixXd>contact_pts_mat(mxGetPr(contact_pts), mxGetM(contact_pts), mxGetN(contact_pts));
msg->support_data[i].num_contact_pts = (int32_t) mxGetN(contact_pts);
msg->support_data[i].contact_pts.resize(3);
for (int j=0; j < 3; j++) {
msg->support_data[i].contact_pts[j].resize(msg->support_data[i].num_contact_pts);
for (int k=0; k < msg->support_data[i].num_contact_pts; k++) {
msg->support_data[i].contact_pts[j][k] = contact_pts_mat(j, k);
}
}
matlabToCArrayOfArrays<4, 1>(support_data, i, "support_logic_map", &double_logic_map[0][0]);
for (int j=0; j < 4; j++) {
msg->support_data[i].support_logic_map[j] = (double_logic_map[j][0] != 0);
}
msg->support_data[i].mu = mxGetScalar(myGetField(support_data, i, "mu"));
msg->support_data[i].contact_surfaces = (int32_t) mxGetScalar(myGetField(support_data, i, "contact_surfaces"));
}
}
const mxArray* body_motion_data = myGetProperty(qp_input, "body_motion_data");
const int nbod = mxGetN(body_motion_data);
msg->num_tracked_bodies = nbod;
msg->body_motion_data.resize(nbod);
if (nbod > 0) {
if (mxGetM(body_motion_data) != 1) {
mexErrMsgTxt("body motion data should be a 1xN struct array");
}
for (int i=0; i < nbod; i++) {
msg->body_motion_data[i].timestamp = msg->timestamp;
msg->body_motion_data[i].body_id = (int32_t) mxGetScalar(myGetField(body_motion_data, i, "body_id"));
memcpy(msg->body_motion_data[i].ts, mxGetPr(myGetField(body_motion_data, i, "ts")), 2*sizeof(double));
const mxArray* coefs = myGetField(body_motion_data, i, "coefs");
if (mxGetNumberOfDimensions(coefs) != 3) mexErrMsgTxt("coefs should be a dimension-3 array");
const mwSize* dim = mxGetDimensions(coefs);
if (dim[0] != 6 || dim[1] != 1 || dim[2] != 4) mexErrMsgTxt("coefs should be size 6x4");
matlabToCArrayOfArrays<6, 4>(body_motion_data, i, "coefs", &msg->body_motion_data[i].coefs[0][0]);
}
}
const mxArray* whole_body_data = myGetProperty(qp_input, "whole_body_data");
if (mxGetN(whole_body_data) != 1 || mxGetM(whole_body_data) != 1) mexErrMsgTxt("whole_body_data should be a 1x1 struct");
const mxArray* q_des = myGetField(whole_body_data, "q_des");
if (mxGetN(q_des) != 1) mexErrMsgTxt("q_des should be a column vector");
const int npos = mxGetM(q_des);
msg->whole_body_data.timestamp = msg->timestamp;
msg->whole_body_data.num_positions = npos;
Map<VectorXd>q_des_vec(mxGetPr(q_des), npos);
msg->whole_body_data.q_des.resize(npos);
for (int i=0; i < npos; i++) {
msg->whole_body_data.q_des[i] = q_des_vec(i);
}
const mxArray* condof = myGetField(whole_body_data, "constrained_dofs");
const int ncons = mxGetM(condof);
msg->whole_body_data.num_constrained_dofs = ncons;
msg->whole_body_data.constrained_dofs.resize(ncons);
if (ncons > 0) {
if (mxGetN(condof) != 1) mexErrMsgTxt("constrained dofs should be a column vector");
Map<VectorXd>condof_vec(mxGetPr(condof), ncons);
for (int i=0; i < ncons; i++) {
msg->whole_body_data.constrained_dofs[i] = condof_vec(i);
}
}
msg->param_set_name = mxArrayToString(myGetProperty(qp_input, "param_set_name"));
return msg;
}
