constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/matlab_mex/cam_getJacobian.cpp
Go to the documentation of this file.
00001 #include "mex.h"
00002 #include "ConstraintApp_MB.h"
00003 
00017 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
00018 {
00019   if ( nrhs != 4 ) {
00020     mexErrMsgTxt("requires exactly 4 parameters: (ptr, state, ids, method)"); 
00021   }
00022 
00023   if ( nlhs != 1 ) {
00024     mexErrMsgTxt("returns exactly 1 parameters"); 
00025   }
00026 
00027   ConstraintApp_MB* app(*(ConstraintApp_MB**)mxGetData(prhs[0]));
00028 
00029   int stateSize = app->GetStateSize();
00030   if ( mxGetM(prhs[1])*mxGetN(prhs[1]) != stateSize ) {
00031     mexErrMsgTxt("state must be Sx1, where S is the size of the state");
00032   }
00033 
00034   int idSize = mxGetNumberOfElements(prhs[2]);
00035 
00036   std::vector<std::string> ids(idSize);
00037   const mxArray* idCellArray = prhs[2];
00038   for ( int i = 0; i < idSize; i++ ) {
00039     const mxArray* string_array = mxGetCell(idCellArray, i);
00040     int buflen = mxGetNumberOfElements(string_array) + 1;
00041     char* str = new char[buflen];
00042     mxGetString(string_array, str, buflen);
00043     //mexPrintf("cam_getJacobian: id[%i]=[%s]\n", i, str);
00044     ids[i] = std::string(str);
00045     delete str;
00046   }
00047 
00048   double method = mxGetScalar(prhs[3]);
00049 
00050   double* s((double*)mxGetData(prhs[1]));
00051   ConstraintApp_MB::StateVector state(stateSize);
00052   for ( int i = 0; i < stateSize; i++ ) {
00053     state[i] = s[i];
00054   }
00055 
00056   ConstraintApp_MB::Jacobian jacobian;
00057   if ( !app->GetJacobian(state, ids, jacobian, (int)method) ) {
00058     mexErrMsgTxt("error while computing jacobian");
00059   }
00060 
00061   plhs[0] = mxCreateDoubleMatrix(jacobian.rows(), jacobian.cols(), mxREAL);
00062   double* jac((double*)mxGetData(plhs[0]));
00063   memcpy(jac, jacobian.data(), jacobian.rows()*jacobian.cols()*sizeof(double));
00064 }
 All Classes Files Functions Variables Typedefs