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);
}
