void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs<1) mexErrMsgTxt("usage: ptr = pelvisMotionControlmex(0,robot_obj,alpha,nominal_pelvis_height,Kp,Kd); y=pelvisMotionControlmex(ptr,x)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct PelvisMotionControlData* pdata;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct PelvisMotionControlData;
// get robot mex model ptr
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the second argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the third argument should be alpha");
memcpy(&(pdata->alpha),mxGetPr(prhs[2]),sizeof(pdata->alpha));
if (!mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[3])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fourth argument should be nominal_pelvis_height");
memcpy(&(pdata->nominal_pelvis_height),mxGetPr(prhs[3]),sizeof(pdata->nominal_pelvis_height));
if (!mxIsNumeric(prhs[4]) || mxGetM(prhs[4])!=6 || mxGetN(prhs[4])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the fifth argument should be Kp");
memcpy(&(pdata->Kp),mxGetPr(prhs[4]),sizeof(pdata->Kp));
if (!mxIsNumeric(prhs[5]) || mxGetM(prhs[5])!=6 || mxGetN(prhs[5])!=1)
mexErrMsgIdAndTxt("DRC:pelvisMotionControlmex:BadInputs","the sixth argument should be Kd");
memcpy(&(pdata->Kd),mxGetPr(prhs[5]),sizeof(pdata->Kd));
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:pelvisMotionControlmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
pdata->pelvis_height_previous = -1;
pdata->pelvis_body_index = pdata->r->findLinkInd("pelvis", 0);
pdata->rfoot_body_index = pdata->r->findLinkInd("r_foot", 0);
pdata->lfoot_body_index = pdata->r->findLinkInd("l_foot", 0);
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:pelvisMotionControlmex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
int nq = pdata->r->num_dof;
int narg = 1;
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
Map<VectorXd> qdvec(qd,nq);
double lfoot_yaw = mxGetScalar(prhs[narg++]);
double rfoot_yaw = mxGetScalar(prhs[narg++]);
pdata->r->doKinematics(q,false,qd);
// TODO: this must be updated to use quaternions/spatial velocity
Vector6d pelvis_pose,rfoot_pose,lfoot_pose;
MatrixXd Jpelvis = MatrixXd::Zero(6,pdata->r->num_dof);
Vector4d zero = Vector4d::Zero();
zero(3) = 1.0;
pdata->r->forwardKin(pdata->pelvis_body_index,zero,1,pelvis_pose);
pdata->r->forwardJac(pdata->pelvis_body_index,zero,1,Jpelvis);
pdata->r->forwardKin(pdata->rfoot_body_index,zero,1,rfoot_pose);
pdata->r->forwardKin(pdata->lfoot_body_index,zero,1,lfoot_pose);
if (pdata->pelvis_height_previous<0) {
pdata->pelvis_height_previous = pelvis_pose(2);
}
double min_foot_z = std::min(lfoot_pose(2),rfoot_pose(2));
double mean_foot_yaw = angleAverage(lfoot_yaw,rfoot_yaw);
double pelvis_height_desired = pdata->alpha*pdata->pelvis_height_previous + (1.0-pdata->alpha)*(min_foot_z + pdata->nominal_pelvis_height);
pdata->pelvis_height_previous = pelvis_height_desired;
Vector6d body_des;
double nan = std::numeric_limits<double>::quiet_NaN();
body_des << nan,nan,pelvis_height_desired,0,0,mean_foot_yaw;
Vector6d error;
error.head<3>()= body_des.head<3>()-pelvis_pose.head<3>();
Vector3d error_rpy,pose_rpy,des_rpy;
pose_rpy = pelvis_pose.tail<3>();
des_rpy = body_des.tail<3>();
angleDiff(pose_rpy,des_rpy,error_rpy);
error.tail(3) = error_rpy;
Vector6d body_vdot = (pdata->Kp.array()*error.array()).matrix() - (pdata->Kd.array()*(Jpelvis*qdvec).array()).matrix();
plhs[0] = eigenToMatlab(body_vdot);
}
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);
}
