CallbackData * CBDCreate(const mxArray *taskObj,
CallbackTypeEnum callbackTypeEnum,
const char *callbackFuncProp,
mxArray *eventData,
void *callbackTypeData)
{
CallbackData *cbd = (CallbackData*)mxCalloc(1,sizeof(CallbackData));
mexMakeMemoryPersistent(cbd);
//Pack callbackData structure
cbd->taskObjHandle = mxDuplicateArray(taskObj);
mexMakeArrayPersistent(cbd->taskObjHandle);
mxArray *mxTaskID = mxGetProperty(taskObj,0,"taskID");
TaskHandle *taskIDPtr = (TaskHandle*)mxGetData(mxTaskID);
cbd->taskHandle = *taskIDPtr;
mxDestroyArray(mxTaskID);
mxArray *callbackFuncs = mxGetProperty(taskObj,0,callbackFuncProp);
size_t numCallbacks = mxGetNumberOfElements(callbackFuncs);
if (numCallbacks > MAXNUMCALLBACKS) {
mexErrMsgTxt("Exceeded the maximum allowed number of callback functions."); /// leak a bunch of memory, no biggie (see doc for fcn)
}
for (size_t i=0;i<numCallbacks;i++) {
cbd->callbackFuncHandles[i] = mxDuplicateArray(mxGetCell(callbackFuncs,i));
mexMakeArrayPersistent(cbd->callbackFuncHandles[i]);
}
mxDestroyArray(callbackFuncs);
for (size_t i=numCallbacks;i<MAXNUMCALLBACKS;++i) {
cbd->callbackFuncHandles[i] = NULL;
}
cbd->numCallbacks = numCallbacks;
cbd->callbackType = CallbackTypeStrings[callbackTypeEnum];
if (eventData==NULL) {
// use default empty event array
mxArray *eventArray = mxCreateStructMatrix(0, 0, 0, 0);
mexMakeArrayPersistent(eventArray);
cbd->eventData = eventArray;
} else {
cbd->eventData = eventData;
}
cbd->callbackTypeData = callbackTypeData;
#if DEBUG_MEX
mexPrintf("At end of CBDCreate.\n");
CBDDebug(cbd);
#endif
return cbd;
}
int
CFAEMisc::getIntScalarPropFromMX(const mxArray *a,const char *pname)
{
assert(a!=NULL);
mxArray *tmp = mxGetProperty(a,0,pname);
assert(tmp!=NULL);
int retval = (int)mxGetScalar(tmp);
mxDestroyArray(tmp);
return retval;
}
matwrap get_property(const char* property) const {
safe_assert(array != NULL, "dereferenced null mxArray");
mxArray* result(mxGetProperty(array, 0, property));
if(result == NULL) {
char buffer[256];
sprintf(buffer, "Invalid property %s\n", property);
mexErrMsgTxt(buffer);
}
return matwrap(result);
} // end of get property
/*************************************************************************
* Matlab structure access methods, get the field from joint i
*************************************************************************/
static mxArray *
mstruct_get_element(mxArray *m, int j, char *field)
{
mxArray *e;
if ((e = mxGetProperty(m, (mwIndex)j, field)) != NULL)
return e;
else {
error("No such field as %s", field);
return NULL;
}
}
void parseGeneralInputs(int nrhs, const mxArray *prhs[], bool *registerTF, TaskHandle *taskID)
{
assert(nrhs>=1);
const mxArray *task = prhs[0];
TaskHandle *taskIDPtr;
//Get TaskHandle
taskIDPtr = (TaskHandle*)mxGetData(mxGetProperty(prhs[0],0, "taskID"));
*taskID = (TaskHandle)*taskIDPtr;
//Process argument 2 - registerTF
if ((nrhs < 2) || mxIsEmpty(prhs[1])) {
*registerTF = true;
} else {
mxLogical *registerTFTemp = mxGetLogicals(prhs[1]);
*registerTF = (bool) *registerTFTemp;
}
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
UNUSED(nlhs);
UNUSED(plhs);
if (nrhs > 0) {
double v = mxGetScalar(prhs[0]);
char buf[100];
mxGetString(prhs[1], buf, 100);
mxArray* C = mxGetProperty(prhs[2], 0, "C");
if (!C) mexErrMsgIdAndTxt("debugMexTest", "failed to get property C");
double* pC = mxGetPrSafe(C);
mexPrintf("%f,%s, C=[%f,%f]\n", v, buf, pC[0], pC[1]);
}
}
void* mxWrapGetP(const mxArray* a, const char* fmt, const char** e)
{
void* p = 0;
mxArray* ap;
if (mxGetClassID(a) == mxDOUBLE_CLASS &&
mxGetM(a)*mxGetN(a) == 1 && *mxGetPr(a) == 0)
return p;
if (mxIsChar(a)) {
char pbuf[128];
mxGetString(a, pbuf, sizeof(pbuf));
sscanf(pbuf, fmt, &p);
}
#ifdef R2008OO
else if (ap = mxGetProperty(a, 0, "mwptr")) {
return mxWrapGetP(ap, fmt, e);
}
#endif
if (p == 0)
*e = "Invalid pointer";
return p;
}
int32 CVICALLBACK callbackWrapper(TaskHandle taskHandle, int32 eventInfo, void *callbackData)
{
CallbackData *cbData = (CallbackData*)callbackData;
mxArray *rhs[3];
rhs[1] = cbData->taskObjHandle;
rhs[2] = cbData->eventData;
#if DEBUG_MEX
mexPrintf("in CBWrap\n");
#endif
int32 retval = 0;
size_t cachedNCB = cbData->numCallbacks; // veej wants deletion of a task object during a DONE callback to work
const char *cachedCBT = cbData->callbackType;
for (size_t i=0;i<cachedNCB;i++){
#if DEBUG_MEX
mexPrintf("in CBWrap, calling %d\n",i);
#endif
//mException = mexCallMATLABWithTrap(0,NULL,0,0,cbData->callbackFuncNames[i]); //TODO -- pass arguments!
rhs[0] = cbData->callbackFuncHandles[i];
mxArray *mException = mexCallMATLABWithTrap(0,NULL,3,rhs,"feval"); //TODO -- pass arguments!
if (!mException) {
continue;
} else {
char *errorString = (char*)mxCalloc(256,sizeof(char));
mxGetString(mxGetProperty(mException,0,"message"),errorString,MAXCALLBACKNAMELENGTH);
mexPrintf("ERROR in %s callback of Task object: %s\n",cachedCBT,errorString);
mxFree(errorString);
retval = 1;
break;
}
}
return retval;
}
mxArray* myGetProperty(const mxArray* pobj, const char* propname)
{
mxArray* pm = mxGetProperty(pobj,0,propname);
if (!pm) mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","ControlUtil is trying to load object property '%s', but failed.", propname);
return pm;
}
//Gateway routine
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Read input arguments
char outputFormat[10];
char outputVarName[MAXVARNAMESIZE];
int outputVarSampsPerChan;
double timeout;
int numSampsPerChan;
bool outputData; //Indicates whether to return an outputData argument
mxClassID outputDataClass;
uInt32 bufSize;
bool writeDigitalLines;
uInt32 bytesPerChan=0;
TaskHandle taskID, *taskIDPtr;
int32 status;
//Get TaskHandle
taskIDPtr = (TaskHandle*)mxGetData(mxGetProperty(prhs[0],0, "taskID"));
taskID = *taskIDPtr;
//Determine if this is a buffered read operation
status = DAQmxGetBufInputBufSize(taskID, &bufSize);
if (status)
handleDAQmxError(status, "DAQmxGetBufInputBufSize");
//Handle input arguments
if ((nrhs < 2) || mxIsEmpty(prhs[1]) || mxIsInf(mxGetScalar(prhs[1])))
{
if (bufSize==0)
numSampsPerChan = 1;
else
numSampsPerChan = DAQmx_Val_Auto;
}
else
numSampsPerChan = (int) mxGetScalar(prhs[1]);
if ((nrhs < 3) || mxIsEmpty(prhs[2]))
{
//Automatic determination of read type
bool isLineBased = (bool) mxGetScalar(mxGetProperty(prhs[0],0,"isLineBasedDigital"));
if ((bufSize==0) && isLineBased) //This is a non-buffered, line-based Task: return data as a double array
outputDataClass = mxDOUBLE_CLASS;
else
{
status = DAQmxGetReadDigitalLinesBytesPerChan(taskID,&bytesPerChan); //This actually returns the number of bytes required to represent one sample of Channel data
if (status)
handleDAQmxError(status, "DAQmxGetReadDigitalLinesBytesPerChan");
if (bytesPerChan <= 8)
outputDataClass = mxUINT8_CLASS;
else if (bytesPerChan <= 16)
outputDataClass = mxUINT16_CLASS;
else if (bytesPerChan <= 32)
outputDataClass = mxUINT32_CLASS;
else
mexErrMsgTxt("It is not currently possible to read integer values from Task with greater than 32 lines per sample value");
}
}
else
{
mxGetString(prhs[2], outputFormat, 10);
if (_strcmpi(outputFormat,"uint8"))
outputDataClass = mxUINT8_CLASS;
else if (_strcmpi(outputFormat,"uint16"))
outputDataClass = mxUINT16_CLASS;
else if (_strcmpi(outputFormat,"uint32"))
outputDataClass = mxUINT32_CLASS;
else if (_strcmpi(outputFormat,"double"))
outputDataClass = mxDOUBLE_CLASS;
else if (_strcmpi(outputFormat,"logical"))
outputDataClass = mxLOGICAL_CLASS;
else
mexErrMsgTxt("The specified 'outputFormat' value (case-sensitive) is not recognized.");
}
if ((outputDataClass == mxDOUBLE_CLASS) || (outputDataClass == mxLOGICAL_CLASS))
{
writeDigitalLines = true;
if (bytesPerChan == 0)
{
status = DAQmxGetReadDigitalLinesBytesPerChan(taskID,&bytesPerChan); //This actually returns the number of bytes required to represent one sample of Channel data
if (status)
handleDAQmxError(status, "DAQmxGetReadDigitalLinesBytesPerChan");
}
}
else
writeDigitalLines = false;
if ((nrhs < 4) || mxIsEmpty(prhs[3]) || mxIsInf(mxGetScalar(prhs[3])))
timeout = DAQmx_Val_WaitInfinitely;
else
timeout = mxGetScalar(prhs[3]);
if ((nrhs < 5) || mxIsEmpty(prhs[4])) //OutputVarSizeOrName argument
{
outputData = true;
outputVarSampsPerChan = numSampsPerChan; //If value is DAQmx_Val_Auto, then the # of samples available will be queried before allocting array
}
else
{
outputData = mxIsNumeric(prhs[4]);
if (outputData)
{
if (nlhs < 2)
mexErrMsgTxt("There must be two output arguments specified if a preallocated MATLAB variable is not specified");
outputVarSampsPerChan = (int) mxGetScalar(prhs[4]);
}
else
mxGetString(prhs[4], outputVarName, MAXVARNAMESIZE);
}
//Determin # of output channels
uInt32 numChannels;
DAQmxGetReadNumChans(taskID, &numChannels); //Reflects number of channels in Task, or the number of channels specified by 'ReadChannelsToRead' property
//Determine output buffer/size (creating if needed)
mxArray *outputDataBuf, *outputDataBufTrue;
void *outputDataPtr;
//float64 *outputDataPtr;
if (outputData)
{
mwSize numRows;
if (outputVarSampsPerChan == DAQmx_Val_Auto)
{
status = DAQmxGetReadAvailSampPerChan(taskID, (uInt32 *)&outputVarSampsPerChan);
if (status)
{
handleDAQmxError(status, "DAQmxGetReadAvailSampPerChan");
return;
}
}
if (writeDigitalLines)
numRows = (mwSize) (outputVarSampsPerChan * bytesPerChan);
else
numRows = (mwSize) outputVarSampsPerChan;
if (outputDataClass == mxDOUBLE_CLASS)
{
outputDataBuf = mxCreateNumericMatrix(numRows,numChannels,mxUINT8_CLASS,mxREAL);
outputDataBufTrue = mxCreateDoubleMatrix(numRows,numChannels,mxREAL);
}
else
outputDataBuf = mxCreateNumericMatrix(numRows,numChannels,outputDataClass,mxREAL);
}
else //I don't believe this is working
{
outputDataBuf = mexGetVariable("caller", outputVarName);
outputVarSampsPerChan = mxGetM(outputDataBuf);
//TODO: Add check to ensure WS variable is of correct class
}
outputDataPtr = mxGetData(outputDataBuf);
//Read data
int32 numSampsRead;
int32 numBytesPerSamp;
switch (outputDataClass)
{
case mxUINT8_CLASS:
status = DAQmxReadDigitalU8(taskID, numSampsPerChan, timeout, fillMode, (uInt8*) outputDataPtr, outputVarSampsPerChan * numChannels, &numSampsRead, NULL);
break;
case mxUINT16_CLASS:
status = DAQmxReadDigitalU16(taskID, numSampsPerChan, timeout, fillMode, (uInt16*) outputDataPtr, outputVarSampsPerChan * numChannels, &numSampsRead, NULL);
break;
case mxUINT32_CLASS:
status = DAQmxReadDigitalU32(taskID, numSampsPerChan, timeout, fillMode, (uInt32*) outputDataPtr, outputVarSampsPerChan * numChannels, &numSampsRead, NULL);
break;
case mxLOGICAL_CLASS:
case mxDOUBLE_CLASS:
status = DAQmxReadDigitalLines(taskID, numSampsPerChan, timeout, fillMode, (uInt8*) outputDataPtr, outputVarSampsPerChan * numChannels * bytesPerChan, &numSampsRead, &numBytesPerSamp, NULL);
break;
default:
mexErrMsgTxt("There must be two output arguments specified if a preallocated MATLAB variable is not specified");
}
//Return output data
if (!status)
{
//mexPrintf("Successfully read %d samples of data\n", numSampsRead);
if (outputData)
{
if (nlhs > 0)
{
if (outputDataClass == mxDOUBLE_CLASS)
{
//Convert logical data to double type
double *outputDataTruePtr = mxGetPr(outputDataBufTrue);
for (size_t i=0;i < mxGetNumberOfElements(outputDataBuf);i++)
*(outputDataTruePtr+i) = (double) *((uInt8 *)outputDataPtr+i);
mxDestroyArray(outputDataBuf);
plhs[0] = outputDataBufTrue;
}
else
plhs[0] = outputDataBuf;
}
else
mxDestroyArray(outputDataBuf); //If you don't read out, all the reading was done for naught
}
else //I don't believe this is working
{
mexErrMsgTxt("invalid branch");
mexPutVariable("caller", outputVarName, outputDataBuf);
if (nlhs > 0) //Return empty value for output data
plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
}
if (nlhs > 1) //Return number of samples actually read
{
double *sampsReadOutput;
plhs[1] = mxCreateDoubleScalar(0);
sampsReadOutput = mxGetPr(plhs[1]);
*sampsReadOutput = (double)numSampsRead;
}
}
else //Read failed
handleDAQmxError(status, mexFunctionName());
}
void mexFunction( const int nlhs, mxArray *plhs[], const int nrhs, const mxArray *prhs[]) {
/* Check for proper number of arguments. */
if (nrhs != 1) {
mexErrMsgIdAndTxt( "MATLAB:Graph:IsConnected:invalidNumInputs",
"One input required.");
} else if (nlhs > 1) {
mexErrMsgIdAndTxt( "MATLAB:Graph:IsConnected:invalidNumOutputs",
"At most one output is allowed.");
}
const mxArray *numberOfNodesArray = mxGetProperty(prhs[0], 0, "numberOfNodes");
if (!mxIsNumeric(numberOfNodesArray) || mxGetNumberOfElements(numberOfNodesArray) != 1) {
mexErrMsgIdAndTxt( "MATLAB:Graph:IsConnected:invalidNumberOfNodes", "Number of nodes is not numeric scalar.");
}
const int numberOfNodes = mxGetScalar(numberOfNodesArray);
if (numberOfNodes < 1) {
mexErrMsgIdAndTxt( "MATLAB:Graph:IsConnected:emptyGraph",
"Graph is empty.");
}
const mxArray *pAdjacencyMatrix = mxGetProperty(prhs[0], 0, "pAdjacencyMatrix");
if (!mxIsLogical(pAdjacencyMatrix) || mxGetN(pAdjacencyMatrix) != numberOfNodes || mxGetM(pAdjacencyMatrix) != numberOfNodes) {
mexErrMsgIdAndTxt("MATLAB:Graph:IsConnected:invalidAdjacencyMatrix", "Adjacency matrix is not logical square matrix");
}
const bool *adjacencyMatrix = mxGetLogicals(pAdjacencyMatrix);
const mxArray *pIsDirectedArray = mxGetProperty(prhs[0], 0, "pIsDirected");
if (!mxIsLogicalScalar(pIsDirectedArray)) {
mexErrMsgIdAndTxt( "MATLAB:Graph:IsConnected:invalidIsDirected", "Direction is not logical scalar");
}
const bool isDirected = mxIsLogicalScalarTrue(pIsDirectedArray);
bool *reachable = new bool[numberOfNodes];
if (mxIsSparse(pAdjacencyMatrix)) {
// Sparse adjacency matrix
std::vector<std::vector<mwIndex> > graph(numberOfNodes), rgraph(numberOfNodes);
const mwIndex* row = mxGetIr(pAdjacencyMatrix);
const mwIndex* col = mxGetJc(pAdjacencyMatrix);
const size_t nz = col[mxGetN(pAdjacencyMatrix)];
mwIndex c = 0;
for(size_t i = 0; i<nz; ++i) {
while(col[c+1]<=i) ++c;
if(adjacencyMatrix[i]) {
graph[row[i]].push_back(c);
rgraph[c].push_back(row[i]);
}
}
memset(reachable, false, numberOfNodes * sizeof(bool));
reachable[0] = true;
int nReachable = 1;
std::queue<int> nodeQueue;
nodeQueue.push(0);
while(!nodeQueue.empty() && nReachable < numberOfNodes) {
const int i = nodeQueue.front(); nodeQueue.pop();
std::vector<mwIndex>::const_iterator it = graph[i].begin();
for (; it != graph[i].end(); ++it) {
if(!reachable[*it]) {
reachable[*it] = true;
nReachable++;
nodeQueue.push(*it);
}
}
}
if (!isDirected || nReachable != numberOfNodes) {
plhs[0] = mxCreateLogicalScalar(nReachable == numberOfNodes);
} else {
memset(reachable, false, numberOfNodes * sizeof(bool));
reachable[0] = true;
int nReachable2 = 1;
nodeQueue = std::queue<int>(); // Clear queue
nodeQueue.push(0);
while(!nodeQueue.empty() && nReachable2 < numberOfNodes) {
const int i = nodeQueue.front(); nodeQueue.pop();
std::vector<mwIndex>::const_iterator it = rgraph[i].begin();
for (; it != rgraph[i].end(); ++it) {
if(!reachable[*it]) {
mexPrintf("Reachable 2 is %lu\n", *it);
reachable[*it] = true;
nReachable2++;
nodeQueue.push(*it);
}
}
}
plhs[0] = mxCreateLogicalScalar(nReachable2 == numberOfNodes);
}
} else {
// Full adjacency matrix
memset(reachable, false, numberOfNodes * sizeof(bool));
reachable[0] = true;
int nReachable = 1;
std::queue<int> nodeQueue;
nodeQueue.push(0);
while(!nodeQueue.empty() && nReachable < numberOfNodes) {
const int i = nodeQueue.front(); nodeQueue.pop();
for (int j=0; j<numberOfNodes; ++j) {
if(adjacencyMatrix[i*numberOfNodes+j] && !reachable[j]) {
reachable[j] = true;
nReachable++;
nodeQueue.push(j);
}
}
}
if (!isDirected || nReachable != numberOfNodes) {
plhs[0] = mxCreateLogicalScalar(nReachable == numberOfNodes);
} else {
memset(reachable, false, numberOfNodes * sizeof(bool));
reachable[0] = true;
int nReachable2 = 1;
nodeQueue = std::queue<int>(); // Clear queue
nodeQueue.push(0);
while(!nodeQueue.empty() && nReachable2 < numberOfNodes) {
const int i = nodeQueue.front(); nodeQueue.pop();
for (int j=0; j<numberOfNodes; ++j) {
if(adjacencyMatrix[j*numberOfNodes+i] && !reachable[j]) {
reachable[j] = true;
nReachable2++;
nodeQueue.push(j);
}
}
}
plhs[0] = mxCreateLogicalScalar(nReachable2 == numberOfNodes);
}
}
delete [] reachable;
}
/**
* @brief This file implements an interface to add a function to an opengm model in MatLab.
*
* This routine accepts a matrix containing the data for the function and creates a explicit function from it.
*
* @param[in] nlhs number of output arguments expected from MatLab.
* (needs to be 1).
* @param[out] plhs pointer to the mxArrays containing the results.
* @param[in] nrhs number of input arguments provided from MatLab.
* (needs to be 2)
* @param[in] prhs pointer to the mxArrays containing the input data provided by
* matlab. prhs[0] needs to contain the handle for the model. prhs[1] needs to contain the data for the explicit function.
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//check if data is in correct format
if(nrhs != 2) {
mexErrMsgTxt("Wrong number of input variables specified (two needed)\n");
}
if(nlhs != 1) {
mexErrMsgTxt("Wrong number of output variables specified (one needed)\n");
}
// get model out of handle
typedef opengm::interface::MatlabModelType::GmType GmType;
GmType& gm = opengm::interface::handle<GmType>::getObject(prhs[0]);
// get shape
mxArray* shapeIn = mxGetProperty(prhs[1], 0, "shape");
size_t numDimensions = mxGetNumberOfElements(shapeIn);
std::vector<GmType::IndexType> shape;
shape.resize(numDimensions);
typedef opengm::interface::helper::copyValue<GmType::ValueType, std::vector<GmType::IndexType>::iterator> copyFunctor;
typedef opengm::interface::helper::forAllValues<copyFunctor> copyValues;
typedef opengm::interface::helper::getDataFromMXArray<copyValues> coppyShape;
copyFunctor variableAdder(shape.begin());
copyValues functor(variableAdder);
coppyShape()(functor, shapeIn);
// create function corresponding to function type
GmType::FunctionIdentifier functionID;
if(mxIsClass(prhs[1], "openGMExplicitFunction")) {
// create function
opengm::interface::MatlabModelType::ExplicitFunction function(shape.begin(), shape.end());
mxArray* functionValues = mxGetProperty(prhs[1], 0, "functionValues");
typedef opengm::interface::helper::copyValue<GmType::ValueType, opengm::ExplicitFunction<GmType::ValueType>::iterator> copyFunctor;
typedef opengm::interface::helper::forAllValues<copyFunctor> copyValues;
typedef opengm::interface::helper::getDataFromMXArray<copyValues> coppyExplicitFunctionValues;
copyFunctor variableAdder(function.begin());
copyValues functor(variableAdder);
coppyExplicitFunctionValues()(functor, functionValues);
// add function to model
functionID = gm.addFunction(function);
} else if(mxIsClass(prhs[1], "openGMPottsFunction")) {
// get values for PottsFunction
GmType::ValueType valueEqual;
GmType::ValueType valueNotEqual;
typedef opengm::interface::helper::copyValue<GmType::ValueType> copyFunctor;
typedef opengm::interface::helper::forFirstValue<copyFunctor> copyValue;
typedef opengm::interface::helper::getDataFromMXArray<copyValue> coppyPottsFunctionValues;
{
copyFunctor copy(&valueEqual);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "valueEqual"));
}
{
copyFunctor copy(&valueNotEqual);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "valueNotEqual"));
}
// create function
opengm::interface::MatlabModelType::PottsFunction function(shape[0], shape[1], valueEqual, valueNotEqual);
// add function to model
functionID = gm.addFunction(function);
} else if(mxIsClass(prhs[1], "openGMPottsNFunction")) {
// get values for PottsNFunction
GmType::ValueType valueEqual;
GmType::ValueType valueNotEqual;
typedef opengm::interface::helper::copyValue<GmType::ValueType> copyFunctor;
typedef opengm::interface::helper::forFirstValue<copyFunctor> copyValue;
typedef opengm::interface::helper::getDataFromMXArray<copyValue> coppyPottsNFunctionValues;
{
copyFunctor copy(&valueEqual);
copyValue functor(copy);
coppyPottsNFunctionValues()(functor, mxGetProperty(prhs[1], 0, "valueEqual"));
}
{
copyFunctor copy(&valueNotEqual);
copyValue functor(copy);
coppyPottsNFunctionValues()(functor, mxGetProperty(prhs[1], 0, "valueNotEqual"));
}
// create function
opengm::interface::MatlabModelType::PottsNFunction function(shape.begin(), shape.end(), valueEqual, valueNotEqual);
// add function to model
functionID = gm.addFunction(function);
} else if(mxIsClass(prhs[1], "openGMPottsGFunction")) {
// copy values
mxArray* functionValues = mxGetProperty(prhs[1], 0, "values");
std::vector<GmType::ValueType> values(mxGetNumberOfElements(functionValues));
typedef opengm::interface::helper::copyValue<GmType::ValueType, std::vector<GmType::ValueType>::iterator> copyFunctor;
typedef opengm::interface::helper::forAllValues<copyFunctor> copyValues;
typedef opengm::interface::helper::getDataFromMXArray<copyValues> coppyPottsGValues;
copyFunctor valuesAdder(values.begin());
copyValues functor(valuesAdder);
coppyPottsGValues()(functor, functionValues);
// create function
opengm::interface::MatlabModelType::PottsGFunction function(shape.begin(), shape.end(), values.begin());
// add function to model
functionID = gm.addFunction(function);
} else if(mxIsClass(prhs[1], "openGMTruncatedSquaredDifferenceFunction")) {
// get values for TruncatedSquaredDifferenceFunction
GmType::ValueType truncation;
GmType::ValueType weight;
typedef opengm::interface::helper::copyValue<GmType::ValueType> copyFunctor;
typedef opengm::interface::helper::forFirstValue<copyFunctor> copyValue;
typedef opengm::interface::helper::getDataFromMXArray<copyValue> coppyPottsFunctionValues;
{
copyFunctor copy(&truncation);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "truncation"));
}
{
copyFunctor copy(&weight);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "weight"));
}
// create function
opengm::interface::MatlabModelType::TruncatedSquaredDifferenceFunction function(shape[0], shape[1], truncation, weight);
// add function to model
functionID = gm.addFunction(function);
} else if(mxIsClass(prhs[1], "openGMTruncatedAbsoluteDifferenceFunction")) {
// get values for PottsFunction
GmType::ValueType truncation;
GmType::ValueType weight;
typedef opengm::interface::helper::copyValue<GmType::ValueType> copyFunctor;
typedef opengm::interface::helper::forFirstValue<copyFunctor> copyValue;
typedef opengm::interface::helper::getDataFromMXArray<copyValue> coppyPottsFunctionValues;
{
copyFunctor copy(&truncation);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "truncation"));
}
{
copyFunctor copy(&weight);
copyValue functor(copy);
coppyPottsFunctionValues()(functor, mxGetProperty(prhs[1], 0, "weight"));
}
// create function
opengm::interface::MatlabModelType::TruncatedAbsoluteDifferenceFunction function(shape[0], shape[1], truncation, weight);
// add function to model
functionID = gm.addFunction(function);
} else {
mexErrMsgTxt("Unsupported function class!");
}
// return function id
const size_t maxSize = sizeof(GmType::FunctionIdentifier::FunctionIndexType) > sizeof(GmType::FunctionIdentifier::FunctionTypeIndexType) ? sizeof(GmType::FunctionIdentifier::FunctionIndexType) : sizeof(GmType::FunctionIdentifier::FunctionTypeIndexType);
if(maxSize > 8) {
mexErrMsgTxt("Unsupported size of opengm IndexType!");
} else if(maxSize > 4) {
plhs[0] = mxCreateNumericMatrix(1, 2, mxUINT64_CLASS, mxREAL);
reinterpret_cast<uint64_T*>(mxGetData(plhs[0]))[0] = static_cast<uint64_T>(functionID.getFunctionIndex());
reinterpret_cast<uint64_T*>(mxGetData(plhs[0]))[1] = static_cast<uint64_T>(functionID.getFunctionType());
} else if(maxSize > 2) {
plhs[0] = mxCreateNumericMatrix(1, 2, mxUINT32_CLASS, mxREAL);
reinterpret_cast<uint32_T*>(mxGetData(plhs[0]))[0] = static_cast<uint32_T>(functionID.getFunctionIndex());
reinterpret_cast<uint32_T*>(mxGetData(plhs[0]))[1] = static_cast<uint32_T>(functionID.getFunctionType());
} else if(maxSize > 1) {
plhs[0] = mxCreateNumericMatrix(1, 2, mxUINT16_CLASS, mxREAL);
reinterpret_cast<uint16_T*>(mxGetData(plhs[0]))[0] = static_cast<uint16_T>(functionID.getFunctionIndex());
reinterpret_cast<uint16_T*>(mxGetData(plhs[0]))[1] = static_cast<uint16_T>(functionID.getFunctionType());
} else {
plhs[0] = mxCreateNumericMatrix(1, 2, mxUINT8_CLASS, mxREAL);
reinterpret_cast<uint8_T*>(mxGetData(plhs[0]))[0] = static_cast<uint8_T>(functionID.getFunctionIndex());
reinterpret_cast<uint8_T*>(mxGetData(plhs[0]))[1] = static_cast<uint8_T>(functionID.getFunctionType());
}
}
// #############################################################################################################
void parseModel()
{ /*fills out the global parameters*/
int i;
t0=*(mxGetPr(mxGetProperty(Model,0,"t0")));
T=*(mxGetPr(mxGetProperty(Model,0,"T")));
ngridm=*(mxGetPr(mxGetProperty(Model,0,"ngridm")));
ngridmax=*(mxGetPr(mxGetProperty(Model,0,"ngridmax")));
nthrhmax=*(mxGetPr(mxGetProperty(Model,0,"nthrhmax")));
ny=*(mxGetPr(mxGetProperty(Model,0,"ny")));
nd=*(mxGetPr(mxGetProperty(Model,0,"nd")));
nnd=*(mxGetPr(mxGetProperty(Model,0,"nnd")));
nst=*(mxGetPr(mxGetProperty(Model,0,"nst")));
nnst=*(mxGetPr(mxGetProperty(Model,0,"nnst")));
mmax=*(mxGetPr(mxGetProperty(Model,0,"mmax")));
a0=*(mxGetPr(mxGetProperty(Model,0,"a0")));
stm=(double *) mxGetPr(mxGetProperty(Model,0,"stm"));//dimentions of state variables + multiplicators
optim_UasD=(int) mxIsLogicalScalarTrue(mxGetField(mxGetProperty(Model,0,"optim"),0,"optim_UasD"));
optim_MUnoD=(int) mxIsLogicalScalarTrue(mxGetField(mxGetProperty(Model,0,"optim"),0,"optim_MUnoD"));
optim_UnoD=(int) mxIsLogicalScalarTrue(mxGetField(mxGetProperty(Model,0,"optim"),0,"optim_UnoD"));
optim_TRPRnoSH=(int) mxIsLogicalScalarTrue(mxGetField(mxGetProperty(Model,0,"optim"),0,"optim_TRPRnoSH"));
states=(double *) mxGetPr(mxGetProperty(Model,0,"states"));
decisions=(double *) mxGetPr(mxGetProperty(Model,0,"decisions"));
//initialize pointers to continuous variable grids (modelspec.c)
loadcontinuousgrid();
//initialize byval to default
byval=0;
//initialize err
*((int*)err)=0;
}
//Gateway routine
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//General vars
char errMsg[512];
//Read input arguments
float64 timeout;
bool writeDigitalLines;
uInt32 bytesPerChan;
int numSampsPerChan;
bool32 autoStart;
int32 status;
TaskHandle taskID, *taskIDPtr;
//Get TaskHandle
taskIDPtr = (TaskHandle*)mxGetData(mxGetProperty(prhs[0],0, "taskID"));
taskID = *taskIDPtr;
mxClassID writeDataClassID = mxGetClassID(prhs[1]);
if ((writeDataClassID == mxLOGICAL_CLASS) || (writeDataClassID == mxDOUBLE_CLASS))
writeDigitalLines = true;
else
writeDigitalLines = false;
if (writeDigitalLines)
{
status = DAQmxGetWriteDigitalLinesBytesPerChan(taskID,&bytesPerChan); //This actually returns the number of bytes required to represent one sample of Channel data
if (status)
handleDAQmxError(status,"DAQmxGetWriteDigitalLinesBytesPerChan");
}
if ((nrhs < 3) || mxIsEmpty(prhs[2]))
timeout = DAQmx_Val_WaitInfinitely;
else
{
timeout = (float64) mxGetScalar(prhs[2]);
if (mxIsInf(timeout) || (timeout < 0))
timeout = DAQmx_Val_WaitInfinitely;
}
if ((nrhs < 4) || mxIsEmpty(prhs[3]))
{
if (writeDigitalLines)
autoStart = true;
else
autoStart = false;
}
else
autoStart = (bool32) mxGetScalar(prhs[3]);
mwSize numRows = mxGetM(prhs[1]);
if ((nrhs < 5) || mxIsEmpty(prhs[4]))
if (writeDigitalLines)
{
numSampsPerChan = numRows / bytesPerChan;
}
else
numSampsPerChan = numRows;
else
numSampsPerChan = (int) mxGetScalar(prhs[4]);
//Verify correct input length
//Write data
int32 sampsWritten;
switch (writeDataClassID)
{
case mxUINT32_CLASS:
status = DAQmxWriteDigitalU32(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt32*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
case mxUINT16_CLASS:
status = DAQmxWriteDigitalU16(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt16*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
case mxUINT8_CLASS:
status = DAQmxWriteDigitalU8(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt8*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
case mxDOUBLE_CLASS:
case mxLOGICAL_CLASS:
{
if (numRows < (numSampsPerChan * bytesPerChan))
mexErrMsgTxt("Supplied writeData argument must have at least (numSampsPerChan x numBytesPerChannel) rows.");
else if (writeDataClassID == mxLOGICAL_CLASS)
status = DAQmxWriteDigitalLines(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt8*) mxGetData(prhs[1]), &sampsWritten, NULL);
else //mxDOUBLE_CLASS
{
//Convert DOUBLE data to LOGICAL values
double *writeDataRaw = mxGetPr(prhs[1]);
mwSize numElements = mxGetNumberOfElements(prhs[1]);
uInt8 *writeData = (uInt8 *)mxCalloc(numElements,sizeof(uInt8));
for (unsigned int i=0;i<numElements;i++)
{
if (writeDataRaw[i] != 0)
writeData[i] = 1;
}
status = DAQmxWriteDigitalLines(taskID, numSampsPerChan, autoStart, timeout, dataLayout, writeData, &sampsWritten, NULL);
mxFree(writeData);
}
}
break;
default:
sprintf_s(errMsg,"Class of supplied writeData argument (%s) is not valid", mxGetClassName(prhs[1]));
mexErrMsgTxt(errMsg);
}
//Handle output arguments
if (nlhs > 0) {
plhs[0] = mxCreateDoubleScalar(0);
double *sampsPerChanWritten = mxGetPr(plhs[0]);
if (!status)
{
//mexPrintf("Successfully wrote %d samples of data\n", sampsWritten);
*sampsPerChanWritten = (double)sampsWritten;
}
else //Write failed
handleDAQmxError(status, mexFunctionName());
}
}
//Gateway routine
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//General vars
char errMsg[512];
//Read input arguments
float64 timeout;
//bool dataIsLogicalOrDouble;
// For unsigned int data (8, 16, or 32-bit) we call DAQmxWriteDigitalU<whatever>() to
// write the data to the buffer. For logical or double data, we call DAQmxWriteDigitalLines().
// For the first, the several channel settings have to be "packed" into a single unsigned int.
// For the second, each channel is set individually.
// Note that a "channel" in this context is a thing you set up with a single call to the
// DAQmxCreateDOChan() function.
// That is, a channel can consist of more than one TTL line.
// This var is set to true iff the data is logical or double.
uInt32 maxLinesPerChannel;
int32 numSampsPerChan; // The number of time points to output, aka the number of "scans"
bool32 autoStart;
int32 status;
TaskHandle taskID, *taskIDPtr;
//Get TaskHandle
taskIDPtr = (TaskHandle*)mxGetData(mxGetProperty(prhs[0],0, "taskID"));
taskID = *taskIDPtr;
// Get type and dimensions of data array
mxClassID writeDataClassID = mxGetClassID(prhs[1]);
bool dataIsLogicalOrDouble = ((writeDataClassID == mxLOGICAL_CLASS) || (writeDataClassID == mxDOUBLE_CLASS)) ;
mwSize numRows = mxGetM(prhs[1]);
mwSize numCols = mxGetN(prhs[1]);
// Determine the timeout
if ((nrhs < 3) || mxIsEmpty(prhs[2]))
timeout = DAQmx_Val_WaitInfinitely;
else
{
timeout = (float64) mxGetScalar(prhs[2]);
if (mxIsInf(timeout) || (timeout < 0))
timeout = DAQmx_Val_WaitInfinitely;
}
// Determine whether to start automatically or not
if ((nrhs < 4) || mxIsEmpty(prhs[3]))
{
int32 sampleTimingType;
status = DAQmxGetSampTimingType(taskID, &sampleTimingType) ;
if (status)
handleDAQmxError(status,"DAQmxSetSampTimingType");
if ( sampleTimingType == DAQmx_Val_OnDemand )
{
// The task is using on-demand timing, so we want the
// new values to be immediately output. (And trying to call
// DAQmxWriteDigitalLines() for an on-demand task, with autoStart set to false,
// will error anyway.
autoStart = (bool32) true;
}
else
{
autoStart = (bool32) false;
}
}
else
{
autoStart = (bool32) mxGetScalar(prhs[3]);
}
// Determine the number of scans (time points) to write out
if ((nrhs < 5) || mxIsEmpty(prhs[4]))
{
if (dataIsLogicalOrDouble)
{
status = DAQmxGetWriteDigitalLinesBytesPerChan(taskID,&maxLinesPerChannel);
// maxLinesPerChannel is maximum number of lines per channel.
// Each DO "channel" can consist of multiple DO lines. (A channel is the thing created with
// a call to DAQmxCreateDOChan().) So this returns the maximum number of lines per channel,
// across all the channels in the task. When you call DAQmxWriteDigitalLines(), the data must consist
// of (number of scans) x (number of channels) x maxLinesPerChannel uint8 elements. If a particular channel has fewer lines
// than the max number, the extra ones are ignored.
if (status)
handleDAQmxError(status,"DAQmxGetWriteDigitalLinesBytesPerChan");
numSampsPerChan = (int32) (numRows/maxLinesPerChannel); // this will do an implicit floor
}
else
numSampsPerChan = (int32) numRows;
}
else
{
numSampsPerChan = (int32) mxGetScalar(prhs[4]);
}
// Verify that the data contains the right number of columns
uInt32 numberOfChannels;
status = DAQmxGetTaskNumChans(taskID, &numberOfChannels) ;
if (status)
handleDAQmxError(status,"DAQmxGetTaskNumChans");
if (numCols != numberOfChannels )
{
mexErrMsgTxt("Supplied writeData argument must have as many columns as there are channels in the task.");
}
// Verify that the data contains enough rows that we won't read off the end of it.
// Note that for logical or double data, the different lines for each channel must be stored in the *rows*.
// So for each time point, there's a maxLinesPerChannel x nChannels submatrix for that time point.
int numberOfRowsNeededPerScan = (dataIsLogicalOrDouble ? maxLinesPerChannel : 1) ;
int minNumberOfRowsNeeded = numberOfRowsNeededPerScan * numSampsPerChan ;
if (numRows < minNumberOfRowsNeeded )
{
mexErrMsgTxt("Supplied writeData argument does not have enough rows.");
}
//Write data
int32 scansWritten;
switch (writeDataClassID)
{
case mxUINT32_CLASS:
status = DAQmxWriteDigitalU32(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt32*) mxGetData(prhs[1]), &scansWritten, NULL);
break;
case mxUINT16_CLASS:
status = DAQmxWriteDigitalU16(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt16*) mxGetData(prhs[1]), &scansWritten, NULL);
break;
case mxUINT8_CLASS:
status = DAQmxWriteDigitalU8(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt8*) mxGetData(prhs[1]), &scansWritten, NULL);
break;
case mxLOGICAL_CLASS:
status = DAQmxWriteDigitalLines(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt8*) mxGetData(prhs[1]), &scansWritten, NULL);
break;
case mxDOUBLE_CLASS:
{
//Convert DOUBLE data to LOGICAL values
double *writeDataRaw = mxGetPr(prhs[1]);
mwSize numElements = mxGetNumberOfElements(prhs[1]);
uInt8 *writeData = (uInt8 *)mxCalloc(numElements,sizeof(uInt8));
for (unsigned int i=0;i<numElements;i++)
{
if (writeDataRaw[i] != 0)
writeData[i] = 1;
}
status = DAQmxWriteDigitalLines(taskID, numSampsPerChan, autoStart, timeout, dataLayout, writeData, &scansWritten, NULL);
mxFree(writeData);
}
break;
default:
sprintf_s(errMsg,"Class of supplied writeData argument (%s) is not valid", mxGetClassName(prhs[1]));
mexErrMsgTxt(errMsg);
}
// If an error occured at some point during writing, deal with that
if (status)
handleDAQmxError(status, mexFunctionName());
// Handle output arguments, if needed
if (nlhs > 0)
{
plhs[0] = mxCreateDoubleScalar(0);
double * sampsPerChanWritten = mxGetPr(plhs[0]);
*sampsPerChanWritten = (double)scansWritten ;
}
}
/**
* MEX function entry point.
*/
void
mexFunction(
int nlhs,
mxArray *plhs[],
int nrhs,
const mxArray *prhs[]
) {
double *q, *qd, *qdd;
double *tau;
unsigned int m,n;
int j, njoints, p, nq;
double *fext = NULL;
double *grav = NULL;
Robot robot;
mxArray *link0;
mxArray *mx_robot;
mxArray *mx_links;
static int first_time = 0;
/*
fprintf(stderr, "Fast RNE: (c) Peter Corke 2002-2011\n");
*/
if ( !mxIsClass(ROBOT_IN, "SerialLink") )
mexErrMsgTxt("frne: first argument is not a robot structure\n");
mx_robot = (mxArray *)ROBOT_IN;
njoints = mstruct_getint(mx_robot, 0, "n");
/***********************************************************************
* Handle the different calling formats.
* Setup pointers to q, qd and qdd inputs
***********************************************************************/
switch (nrhs) {
case 2:
/*
* TAU = RNE(ROBOT, [Q QD QDD])
*/
if (NUMCOLS(A1_IN) != 3 * njoints)
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
break;
case 3:
/*
* TAU = RNE(ROBOT, [Q QD QDD], GRAV)
*/
if (NUMCOLS(A1_IN) != (3 * njoints))
mexErrMsgTxt("frne: too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
break;
case 4:
/*
* TAU = RNE(ROBOT, Q, QD, QDD)
* TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
*/
if (NUMCOLS(A1_IN) == (3 * njoints)) {
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A2_IN);
if (NUMELS(A3_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A3_IN);
} else {
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
}
break;
case 5: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
break;
}
case 6: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
if (NUMELS(A5_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A5_IN);
break;
}
default:
mexErrMsgTxt("frne: wrong number of arguments.");
}
/*
* fill out the robot structure
*/
robot.njoints = njoints;
if (grav)
robot.gravity = (Vect *)grav;
else
robot.gravity = (Vect *)mxGetPr( mxGetProperty(mx_robot, (mwIndex)0, "gravity") );
robot.dhtype = mstruct_getint(mx_robot, 0, "mdh");
/* build link structure */
robot.links = (Link *)mxCalloc((mwSize) njoints, (mwSize) sizeof(Link));
/***********************************************************************
* Now we have to get pointers to data spread all across a cell-array
* of Matlab structures.
*
* Matlab structure elements can be found by name (slow) or by number (fast).
* We assume that since the link structures are all created by the same
* constructor, the index number for each element will be the same for all
* links. However we make no assumption about the numbers themselves.
***********************************************************************/
/* get pointer to the first link structure */
link0 = mxGetProperty(mx_robot, (mwIndex) 0, "links");
if (link0 == NULL)
mexErrMsgTxt("couldnt find element link in robot object");
/*
* Elements of the link structure are:
*
* alpha:
* A:
* theta:
* D:
* offset:
* sigma:
* mdh:
* m:
* r:
* I:
* Jm:
* G:
* B:
* Tc:
*/
if (first_time == 0) {
mexPrintf("Fast RNE: (c) Peter Corke 2002-2012\n");
first_time = 1;
}
/*
* copy data from the Link objects into the local links structure
* to save function calls later
*/
for (j=0; j<njoints; j++) {
Link *l = &robot.links[j];
mxArray *links = mxGetProperty(mx_robot, (mwIndex) 0, "links"); // links array
l->alpha = mxGetScalar( mxGetProperty(links, (mwIndex) j, "alpha") );
l->A = mxGetScalar( mxGetProperty(links, (mwIndex) j, "a") );
l->theta = mxGetScalar( mxGetProperty(links, (mwIndex) j, "theta") );
l->D = mxGetScalar( mxGetProperty(links, (mwIndex) j, "d") );
l->sigma = mxGetScalar( mxGetProperty(links, (mwIndex) j, "sigma") );
l->offset = mxGetScalar( mxGetProperty(links, (mwIndex) j, "offset") );
l->m = mxGetScalar( mxGetProperty(links, (mwIndex) j, "m") );
l->rbar = (Vect *)mxGetPr( mxGetProperty(links, (mwIndex) j, "r") );
l->I = mxGetPr( mxGetProperty(links, (mwIndex) j, "I") );
l->Jm = mxGetScalar( mxGetProperty(links, (mwIndex) j, "Jm") );
l->G = mxGetScalar( mxGetProperty(links, (mwIndex) j, "G") );
l->B = mxGetScalar( mxGetProperty(links, (mwIndex) j, "B") );
l->Tc = mxGetPr( mxGetProperty(links, (mwIndex) j, "Tc") );
}
/* Create a matrix for the return argument */
TAU_OUT = mxCreateDoubleMatrix((mwSize) nq, (mwSize) njoints, mxREAL);
tau = mxGetPr(TAU_OUT);
#define MEL(x,R,C) (x[(R)+(C)*nq])
/* for each point in the input trajectory */
for (p=0; p<nq; p++) {
/*
* update all position dependent variables
*/
for (j = 0; j < njoints; j++) {
Link *l = &robot.links[j];
switch (l->sigma) {
case REVOLUTE:
rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype);
break;
case PRISMATIC:
rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype);
break;
}
#ifdef DEBUG
rot_print("R", &l->R);
vect_print("p*", &l->r);
#endif
}
newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq);
}
mxFree(robot.links);
}
// [outputData, sampsPerChanRead] = readAnalogData(task, numSampsPerChan, outputFormat, timeout, outputVarSizeOrName)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Read input arguments
// Get the task handle
mxArray *mxTaskID = mxGetProperty(prhs[0],0,"taskID");
mxClassID clsID = mxGetClassID(mxTaskID);
localAssert(clsID==TASKHANDLE_MXCLASS);
TaskHandle *taskIDPtr = (TaskHandle*)mxGetData(mxTaskID);
TaskHandle taskID = *taskIDPtr;
//Determine if this is a buffered read operation
uInt32 bufSize = 0;
int32 status = DAQmxGetBufInputBufSize(taskID,&bufSize);
if (status) {
handleDAQmxError(status,"DAQmxGetBufInputBufSize");
}
// Handle input arguments
int32 numSampsPerChan = 0; // this does take negative vals in the case of DAQmx_Val_Auto
if ((nrhs < 2) || mxIsEmpty(prhs[1]) || mxIsInf(mxGetScalar(prhs[1]))) {
if (bufSize==0)
numSampsPerChan = 1;
else
numSampsPerChan = DAQmx_Val_Auto;
} else {
numSampsPerChan = (int) mxGetScalar(prhs[1]);
}
char outputFormat[10];
if ((nrhs < 3) || mxIsEmpty(prhs[2]))
strcpy_s(outputFormat,"scaled");
else
mxGetString(prhs[2], outputFormat, 10);
double timeout;
if ((nrhs < 4) || mxIsEmpty(prhs[3]) || mxIsInf(mxGetScalar(prhs[3])))
timeout = DAQmx_Val_WaitInfinitely;
else
timeout = mxGetScalar(prhs[3]);
bool outputData; //Indicates whether to return an outputData argument
int outputVarSampsPerChan = 0; // this CAN take negative values in case of DAQmx_Val_Auto
char outputVarName[MAXVARNAMESIZE];
if ((nrhs < 5) || mxIsEmpty(prhs[4])) {
outputData = true;
outputVarSampsPerChan = numSampsPerChan; //If value is DAQmx_Val_Auto, then the # of samples available will be queried before allocting array
} else {
outputData = mxIsNumeric(prhs[4]);
if (outputData) {
if (nlhs < 2) {
mexErrMsgTxt("There must be two output arguments specified if a preallocated MATLAB variable is not specified");
}
outputVarSampsPerChan = (int) mxGetScalar(prhs[4]);
} else {
mxGetString(prhs[4],outputVarName,MAXVARNAMESIZE);
}
}
//Determine output data type
mxClassID outputDataClass;
mxArray *mxRawDataArrayAI = mxGetProperty(prhs[0],0,"rawDataArrayAI"); //Stored in MCOS Task object as an empty array of the desired class!
mxClassID rawDataClass = mxGetClassID(mxRawDataArrayAI);
char errorMessage[30];
if (!_strcmpi(outputFormat,"scaled"))
outputDataClass = mxDOUBLE_CLASS;
else if (!_strcmpi(outputFormat,"native"))
outputDataClass = rawDataClass;
else {
sprintf_s(errorMessage,"Unrecognized output format: %s\n",outputFormat);
mexErrMsgTxt(errorMessage);
}
//Determine # of output channels
uInt32 numChannels;
DAQmxGetReadNumChans(taskID,&numChannels); //Reflects number of channels in Task, or the number of channels specified by 'ReadChannelsToRead' property
//Determine output buffer/size (creating if needed)
mxArray *mxOutputDataBuf = NULL;
if (outputData) {
if (outputVarSampsPerChan == DAQmx_Val_Auto) {
uInt32 buf = 0;
status = DAQmxGetReadAvailSampPerChan(taskID,&buf);
if (status) {
handleDAQmxError(status, mexFunctionName());
}
outputVarSampsPerChan = buf;
}
//localAssert(outputVarSampsPerChan >= 0);
localAssert(outputVarSampsPerChan > 0);
mxOutputDataBuf = mxCreateNumericMatrix(outputVarSampsPerChan,numChannels,outputDataClass,mxREAL);
} else {
localAssert(false);
////I don't believe this is working
//mxOutputDataBuf = mexGetVariable("caller", outputVarName);
//outputVarSampsPerChan = mxGetM(mxOutputDataBuf);
////TODO: Add check to ensure WS variable is of correct class
}
void* outputDataPtr = mxGetData(mxOutputDataBuf);
localAssert(outputDataPtr!=NULL);
uInt32 arraySizeInSamps = outputVarSampsPerChan * numChannels;
localAssert(mxGetNumberOfElements(mxOutputDataBuf)==(size_t)arraySizeInSamps);
int32 numSampsPerChanRead;
if (outputDataClass == mxDOUBLE_CLASS) //'scaled'
// float64 should be double
status = DAQmxReadAnalogF64(taskID,numSampsPerChan,timeout,fillMode,
(float64*) outputDataPtr, arraySizeInSamps, &numSampsPerChanRead, NULL);
else { //'raw'
switch (outputDataClass)
{
case mxINT16_CLASS:
status = DAQmxReadBinaryI16(taskID, numSampsPerChan, timeout, fillMode, (int16*) outputDataPtr, arraySizeInSamps, &numSampsPerChanRead, NULL);
break;
case mxINT32_CLASS:
status = DAQmxReadBinaryI32(taskID, numSampsPerChan, timeout, fillMode, (int32*) outputDataPtr, arraySizeInSamps, &numSampsPerChanRead, NULL);
break;
case mxUINT16_CLASS:
status = DAQmxReadBinaryU16(taskID, numSampsPerChan, timeout, fillMode, (uInt16*) outputDataPtr, arraySizeInSamps, &numSampsPerChanRead, NULL);
break;
case mxUINT32_CLASS:
status = DAQmxReadBinaryU32(taskID, numSampsPerChan, timeout, fillMode, (uInt32*) outputDataPtr, arraySizeInSamps, &numSampsPerChanRead, NULL);
break;
}
}
//Return output data
if (!status)
{
//mexPrintf("Successfully read %d samples of data\n", numSampsRead);
if (outputData) {
if (nlhs > 0)
plhs[0] = mxOutputDataBuf;
else
mxDestroyArray(mxOutputDataBuf); //If you don't read out, all the reading was done for naught
} else {
//I don't believe this is working
localAssert(false);
//mexPutVariable("caller", outputVarName, mxOutputDataBuf);
//
//if (nlhs >= 0) //Return empty value for output data
// plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
}
if (nlhs>1) { //Return number of samples actually read
plhs[1] = mxCreateDoubleScalar(0.0);
double *sampsReadOutput = mxGetPr(plhs[1]);
*sampsReadOutput = (double)numSampsPerChanRead;
}
} else { //Read failed
handleDAQmxError(status, mexFunctionName());
}
}
//Gateway routine
//sampsPerChanWritten = writeAnalogData(task, writeData, timeout, autoStart, numSampsPerChan)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//General vars
char errMsg[512];
//Read input arguments
float64 timeout;
int numSampsPerChan;
bool32 autoStart;
TaskHandle taskID, *taskIDPtr;
//Get TaskHandle
taskIDPtr = (TaskHandle*)mxGetData(mxGetProperty(prhs[0],0, "taskID"));
taskID = *taskIDPtr;
if ((nrhs < 3) || mxIsEmpty(prhs[2]))
timeout = 10.0;
else
{
timeout = (float64) mxGetScalar(prhs[2]);
if (mxIsInf(timeout))
timeout = DAQmx_Val_WaitInfinitely;
}
if ((nrhs < 4) || mxIsEmpty(prhs[3])) {
int32 sampTimingType = 0;
int32 status = DAQmxGetSampTimingType(taskID,&sampTimingType);
if (status!=0) {
mexErrMsgTxt("Failed to get sample timing type from DAQmx.");
}
autoStart = (sampTimingType==DAQmx_Val_OnDemand);
}
else
autoStart = (bool32) mxGetScalar(prhs[3]);
size_t numRows = mxGetM(prhs[1]);
if ((nrhs < 5) || mxIsEmpty(prhs[4]))
numSampsPerChan = numRows;
else
numSampsPerChan = (int) mxGetScalar(prhs[4]);
//Verify correct input length
//Write data
int32 sampsWritten;
int32 status;
switch (mxGetClassID(prhs[1]))
{
case mxUINT16_CLASS:
status = DAQmxWriteBinaryU16(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (uInt16*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
case mxINT16_CLASS:
status = DAQmxWriteBinaryI16(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (int16*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
case mxDOUBLE_CLASS:
status = DAQmxWriteAnalogF64(taskID, numSampsPerChan, autoStart, timeout, dataLayout, (float64*) mxGetData(prhs[1]), &sampsWritten, NULL);
break;
default:
sprintf_s(errMsg,"Class of supplied writeData argument (%s) is not valid", mxGetClassName(prhs[1]));
mexErrMsgTxt(errMsg);
}
//Handle output arguments and errors
if (!status)
{
if (nlhs > 0) {
plhs[0] = mxCreateDoubleScalar(0);
double *sampsPerChanWritten = mxGetPr(plhs[0]);
}
//mexPrintf("Successfully wrote %d samples of data\n", sampsWritten);
}
else //Write failed
handleDAQmxError(status, mexFunctionName());
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 3)
{
mexErrMsgTxt("kernelCore: not enough parameters!");
}
if (!mxIsDouble(prhs[0]) || !mxIsDouble(prhs[1]))
{
mexErrMsgTxt("kernelCore: Invalid parameters!");
}
mat x = armaGetPr(prhs[0]);
cube scale = armaGetCubePr(prhs[1]);
Kernel *kernel = convertMat2Ptr<Kernel>(mxGetProperty(prhs[2], 0, "objectHandle"));
if (nlhs == 1)
{
vec f;
if (THREAD_NUMBER > x.n_rows)
{
KernelModules::kernelCore(f, x, scale, *kernel);
}
else
{
KernelModules::kernelCore_fast_parallel(f, x, scale, *kernel);
}
if (plhs[0] == NULL)
{
plhs[0] = mxCreateDoubleMatrix(f.n_rows, f.n_cols, mxREAL);
}
armaSetPr(plhs[0], f);
}
if (nlhs == 2)
{
vec f;
mat g;
if (THREAD_NUMBER > x.n_rows)
{
KernelModules::kernelCore(f, g, x, scale, *kernel);
}
else
{
KernelModules::kernelCore_fast_parallel(f, g, x, scale, *kernel);
}
if (plhs[0] == NULL)
{
plhs[0] = mxCreateDoubleMatrix(f.n_rows, f.n_cols, mxREAL);
}
if (plhs[1] == NULL)
{
plhs[1] = mxCreateDoubleMatrix(g.n_rows, g.n_cols, mxREAL);
}
armaSetPr(plhs[0], f);
armaSetPr(plhs[1], g);
}
if (nlhs == 3)
{
vec f;
mat g;
cube H;
if (THREAD_NUMBER > x.n_rows)
{
KernelModules::kernelCore(f, g, H, x, scale, *kernel);
}
else
{
KernelModules::kernelCore_fast_parallel(f, g, H, x, scale, *kernel);
}
if (plhs[0] == NULL)
{
plhs[0] = mxCreateDoubleMatrix(f.n_rows, f.n_cols, mxREAL);
}
if (plhs[1] == NULL)
{
plhs[1] = mxCreateDoubleMatrix(g.n_rows, g.n_cols, mxREAL);
}
if (plhs[2] == NULL)
{
mwSize dim[3];
dim[0] = H.n_rows;
dim[1] = H.n_cols;
dim[2] = H.n_slices;
plhs[2] = mxCreateNumericArray(3, dim, mxDOUBLE_CLASS, mxREAL);
}
armaSetPr(plhs[0], f);
armaSetPr(plhs[1], g);
armaSetCubePr(plhs[2], H);
}
}
