void mexFunction(int nlhs,mxArray* plhs[], int nrhs, const mxArray * prhs[]) { if(nrhs!=3 || nlhs != 7) { mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Usage [type,num_cnst,cnst_val,dcnst_val,cnst_name,lb,ub] = testMultipleTimeKinCnstmex(kinCnst,q,t)"); } MultipleTimeKinematicConstraint* cnst = (MultipleTimeKinematicConstraint*) getDrakeMexPointer(prhs[0]); int n_breaks = mxGetNumberOfElements(prhs[2]); double* t_ptr = new double[n_breaks]; memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks); int nq = cnst->getRobotPointer()->num_positions; MatrixXd q(nq,n_breaks); if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks) { mexErrMsgIdAndTxt("Drake:testMultipleTimeKinCnstmex:BadInputs","Argument 2 must be of size nq*n_breaks"); } memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks); int type = cnst->getType(); int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks); VectorXd c(num_cnst); MatrixXd dc(num_cnst,nq*n_breaks); cnst->eval(t_ptr,n_breaks,q,c,dc); VectorXd lb(num_cnst); VectorXd ub(num_cnst); cnst->bounds(t_ptr,n_breaks,lb,ub); std::vector<std::string> cnst_names; cnst->name(t_ptr,n_breaks,cnst_names); int retvec_size; if(num_cnst == 0) { retvec_size = 0; } else { retvec_size = 1; } plhs[0] = mxCreateDoubleScalar((double) type); plhs[1] = mxCreateDoubleScalar((double) num_cnst); plhs[2] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL); memcpy(mxGetPrSafe(plhs[2]),c.data(),sizeof(double)*num_cnst); plhs[3] = mxCreateDoubleMatrix(num_cnst,nq*n_breaks,mxREAL); memcpy(mxGetPrSafe(plhs[3]),dc.data(),sizeof(double)*num_cnst*nq*n_breaks); int name_ndim = 1; mwSize name_dims[] = {(mwSize) num_cnst}; plhs[4] = mxCreateCellArray(name_ndim,name_dims); mxArray *name_ptr; for(int i = 0;i<num_cnst;i++) { name_ptr = mxCreateString(cnst_names[i].c_str()); mxSetCell(plhs[4],i,name_ptr); } plhs[5] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL); plhs[6] = mxCreateDoubleMatrix(num_cnst,retvec_size,mxREAL); memcpy(mxGetPrSafe(plhs[5]),lb.data(),sizeof(double)*num_cnst); memcpy(mxGetPrSafe(plhs[6]),ub.data(),sizeof(double)*num_cnst); delete[] t_ptr; }
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]); } }
DLL_EXPORT_SYM void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 3 || nlhs != 8) { mexErrMsgIdAndTxt( "Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs", "Usage [num_cnst, cnst_val, iAfun, jAvar, A, cnst_name, lb, ub] = " "testMultipleTimeLinearPostureConstraintmex(kinCnst, q, t)"); } MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*)getDrakeMexPointer(prhs[0]); int n_breaks = static_cast<int>(mxGetNumberOfElements(prhs[2])); double* t_ptr = new double[n_breaks]; memcpy(t_ptr, mxGetPrSafe(prhs[2]), sizeof(double) * n_breaks); int nq = cnst->getRobotPointer()->get_num_positions(); Eigen::MatrixXd q(nq, n_breaks); if (mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks) { mexErrMsgIdAndTxt( "Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs", "Argument 2 must be of size nq*n_breaks"); } memcpy(q.data(), mxGetPrSafe(prhs[1]), sizeof(double) * nq * n_breaks); int num_cnst = cnst->getNumConstraint(t_ptr, n_breaks); Eigen::VectorXd c(num_cnst); cnst->feval(t_ptr, n_breaks, q, c); Eigen::VectorXi iAfun; Eigen::VectorXi jAvar; Eigen::VectorXd A; cnst->geval(t_ptr, n_breaks, iAfun, jAvar, A); std::vector<std::string> cnst_names; cnst->name(t_ptr, n_breaks, cnst_names); Eigen::VectorXd lb(num_cnst); Eigen::VectorXd ub(num_cnst); cnst->bounds(t_ptr, n_breaks, lb, ub); Eigen::VectorXd iAfun_tmp(iAfun.size()); Eigen::VectorXd jAvar_tmp(jAvar.size()); for (int i = 0; i < iAfun.size(); i++) { iAfun_tmp(i) = (double)iAfun(i) + 1; jAvar_tmp(i) = (double)jAvar(i) + 1; } plhs[0] = mxCreateDoubleScalar((double)num_cnst); plhs[1] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL); memcpy(mxGetPrSafe(plhs[1]), c.data(), sizeof(double) * num_cnst); plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(), 1, mxREAL); memcpy(mxGetPrSafe(plhs[2]), iAfun_tmp.data(), sizeof(double) * iAfun_tmp.size()); plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(), 1, mxREAL); memcpy(mxGetPrSafe(plhs[3]), jAvar_tmp.data(), sizeof(double) * jAvar_tmp.size()); plhs[4] = mxCreateDoubleMatrix(A.size(), 1, mxREAL); memcpy(mxGetPrSafe(plhs[4]), A.data(), sizeof(double) * A.size()); int name_ndim = 1; mwSize name_dims[] = {(mwSize)num_cnst}; plhs[5] = mxCreateCellArray(name_ndim, name_dims); mxArray* name_ptr; for (int i = 0; i < num_cnst; i++) { name_ptr = mxCreateString(cnst_names[i].c_str()); mxSetCell(plhs[5], i, name_ptr); } plhs[6] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL); plhs[7] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL); memcpy(mxGetPrSafe(plhs[6]), lb.data(), sizeof(double) * num_cnst); memcpy(mxGetPrSafe(plhs[7]), ub.data(), sizeof(double) * num_cnst); delete[] t_ptr; }
/* * [type, num_constraint, constraint_val, dconstraint_val, constraint_name, lower_bound, upper_bound] * = testSingleTimeKinCnstmex(kinCnst_ptr, q, t) * @param kinCnst_ptr A pointer to a SingleTimeKinematicConstraint * object * @param q A nqx1 double vector * @param t A double scalar, the time to evaluate constraint * value, bounds and name. This is optional. * @retval type The type of the constraint * @retval num_constraint The number of constraint active at time t * @retval constraint_val The value of the constraint at time t * @retval dconstraint_val The gradient of the constraint w.r.t q at time t * @retval constraint_name The name of the constraint at time t * @retval lower_bound The lower bound of the constraint at time t * @retval upper_bound The upper bound of the constraint at time t * */ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if ((nrhs != 3 && nrhs != 2) || nlhs != 7) { mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs", "Usage [type, " "num_cnst, cnst_val, dcnst_val, cnst_name, lb, ub] = " "testSingleTimeKinKinCnstmex(kinCnst, q, t)"); } SingleTimeKinematicConstraint* cnst = (SingleTimeKinematicConstraint*)getDrakeMexPointer(prhs[0]); double* t_ptr; if (nrhs == 2) { t_ptr = nullptr; } else { size_t num_t = mxGetNumberOfElements(prhs[2]); if (num_t == 0) { t_ptr = nullptr; } if (num_t == 1) { t_ptr = mxGetPrSafe(prhs[2]); } if (num_t > 1) { mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs", "t must be either empty or a single number"); } } int type = cnst->getType(); int num_cnst = cnst->getNumConstraint(t_ptr); // mexPrintf("num_cnst = %d\n", num_cnst); int nq = cnst->getRobotPointer()->number_of_positions(); Eigen::Map<Eigen::VectorXd> q(mxGetPrSafe(prhs[1]), nq); KinematicsCache<double> cache = cnst->getRobotPointer()->doKinematics(q); Eigen::VectorXd c(num_cnst); Eigen::MatrixXd dc(num_cnst, nq); cnst->eval(t_ptr, cache, c, dc); // mexPrintf("get c, dc\n"); Eigen::VectorXd lb(num_cnst); Eigen::VectorXd ub(num_cnst); cnst->bounds(t_ptr, lb, ub); // mexPrintf("get lb, ub\n"); std::vector<std::string> cnst_names; cnst->name(t_ptr, cnst_names); // mexPrintf("get name\n"); int retvec_size; if (num_cnst == 0) { retvec_size = 0; } else { retvec_size = 1; } plhs[0] = mxCreateDoubleScalar((double)type); plhs[1] = mxCreateDoubleScalar((double)num_cnst); plhs[2] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL); memcpy(mxGetPrSafe(plhs[2]), c.data(), sizeof(double) * num_cnst); plhs[3] = mxCreateDoubleMatrix(num_cnst, nq, mxREAL); memcpy(mxGetPrSafe(plhs[3]), dc.data(), sizeof(double) * num_cnst * nq); int name_ndim = 1; mwSize name_dims[] = {(mwSize)num_cnst}; plhs[4] = mxCreateCellArray(name_ndim, name_dims); mxArray* name_ptr; for (int i = 0; i < num_cnst; i++) { name_ptr = mxCreateString(cnst_names[i].c_str()); mxSetCell(plhs[4], i, name_ptr); } plhs[5] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL); plhs[6] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL); memcpy(mxGetPrSafe(plhs[5]), lb.data(), sizeof(double) * num_cnst); memcpy(mxGetPrSafe(plhs[6]), ub.data(), sizeof(double) * num_cnst); }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { if (nrhs<1) mexErrMsgTxt("usage: ptr = bodyMotionControlmex(0,robot_obj,Kp,Kd,body_index); y=bodyMotionControlmex(ptr,x,body_pose_des,body_v_des,body_vdot_des)"); if (nlhs<1) mexErrMsgTxt("take at least one output... please."); struct BodyMotionControlData* pdata; if (mxGetScalar(prhs[0])==0) { // then construct the data object and return pdata = new struct BodyMotionControlData; // get robot mex model ptr if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1) mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the second argument should be the robot mex ptr"); memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r)); if (!mxIsNumeric(prhs[2]) || mxGetM(prhs[2])!=6 || mxGetN(prhs[2])!=1) mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the third argument should be Kp"); memcpy(&(pdata->Kp),mxGetPrSafe(prhs[2]),sizeof(pdata->Kp)); if (!mxIsNumeric(prhs[3]) || mxGetM(prhs[3])!=6 || mxGetN(prhs[3])!=1) mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the fourth argument should be Kd"); memcpy(&(pdata->Kd),mxGetPrSafe(prhs[3]),sizeof(pdata->Kd)); pdata->body_index = (int) mxGetScalar(prhs[4]) -1; mxClassID cid; if (sizeof(pdata)==4) cid = mxUINT32_CLASS; else if (sizeof(pdata)==8) cid = mxUINT64_CLASS; else mexErrMsgIdAndTxt("Drake:bodyMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??"); plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL); memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata)); return; } // first get the ptr back from matlab if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1) mexErrMsgIdAndTxt("DRC:bodyMotionControlmex:BadInputs","the first argument should be the ptr"); memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata)); int nq = pdata->r->num_positions; int nv = pdata->r->num_velocities; int narg = 1; Map<VectorXd> q(mxGetPrSafe(prhs[narg]), nq); Map<VectorXd> qd(mxGetPrSafe(prhs[narg++]) + nq, nv); assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1); Map< Vector6d > body_pose_des(mxGetPrSafe(prhs[narg++])); assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1); Map< Vector6d > body_v_des(mxGetPrSafe(prhs[narg++])); assert(mxGetM(prhs[narg])==6); assert(mxGetN(prhs[narg])==1); Map< Vector6d > body_vdot_des(mxGetPrSafe(prhs[narg++])); pdata->r->doKinematics(q, qd); // TODO: this must be updated to use quaternions/spatial velocity auto body_pose_gradientvar = pdata->r->forwardKin(Vector3d::Zero().eval(), pdata->body_index, 0, 1, 1); const auto& body_pose = body_pose_gradientvar.value(); const auto& J = body_pose_gradientvar.gradient().value(); Vector6d body_error; body_error.head<3>()= body_pose_des.head<3>()-body_pose.head<3>(); Vector3d error_rpy,pose_rpy,des_rpy; pose_rpy = body_pose.tail<3>(); des_rpy = body_pose_des.tail<3>(); angleDiff(pose_rpy,des_rpy,error_rpy); body_error.tail(3) = error_rpy; Vector6d body_vdot = (pdata->Kp.array()*body_error.array()).matrix() + (pdata->Kd.array()*(body_v_des-J*qd).array()).matrix() + body_vdot_des; plhs[0] = eigenToMatlab(body_vdot); }