本文整理汇总了C++中Vector6d::data方法的典型用法代码示例。如果您正苦于以下问题：C++ Vector6d::data方法的具体用法？C++ Vector6d::data怎么用？C++ Vector6d::data使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d的用法示例。
在下文中一共展示了Vector6d::data方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: mexFunction
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int error;
if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct QPControllerData* pdata;
mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct QPControllerData;
// get control object properties
const mxArray* pobj = prhs[1];
pm = myGetProperty(pobj,"slack_limit");
pdata->slack_limit = mxGetScalar(pm);
pm = myGetProperty(pobj,"W_kdot");
assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
memcpy(pdata->W_kdot.data(),mxGetPr(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));
pm= myGetProperty(pobj,"w_grf");
pdata->w_grf = mxGetScalar(pm);
pm= myGetProperty(pobj,"w_slack");
pdata->w_slack = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_ang");
pdata->Kp_ang = mxGetScalar(pm);
pm = myGetProperty(pobj,"Kp_accel");
pdata->Kp_accel = mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_inputs");
pdata->n_body_accel_inputs = (int) mxGetScalar(pm);
pm= myGetProperty(pobj,"n_body_accel_bounds");
pdata->n_body_accel_bounds = (int) mxGetScalar(pm);
mxArray* body_accel_bounds = myGetProperty(pobj,"body_accel_bounds");
Vector6d vecbound;
for (int i=0; i<pdata->n_body_accel_bounds; i++) {
pdata->accel_bound_body_idx.push_back((int) mxGetScalar(mxGetField(body_accel_bounds,i,"body_idx"))-1);
pm = mxGetField(body_accel_bounds,i,"min_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPr(pm),sizeof(double)*6);
pdata->min_body_acceleration.push_back(vecbound);
pm = mxGetField(body_accel_bounds,i,"max_acceleration");
assert(mxGetM(pm)==6); assert(mxGetN(pm)==1);
memcpy(vecbound.data(),mxGetPr(pm),sizeof(double)*6);
pdata->max_body_acceleration.push_back(vecbound);
}
pm = myGetProperty(pobj,"body_accel_input_weights");
pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
memcpy(pdata->body_accel_input_weights.data(),mxGetPr(pm),sizeof(double)*pdata->n_body_accel_inputs);
pdata->n_body_accel_eq_constraints = 0;
for (int i=0; i<pdata->n_body_accel_inputs; i++) {
if (pdata->body_accel_input_weights(i) < 0)
pdata->n_body_accel_eq_constraints++;
}
// get robot mex model ptr
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
memcpy(pdata->B.data(),mxGetPr(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));
int nq = pdata->r->num_dof, nu = pdata->B.cols();
pm = myGetProperty(pobj,"w_qdd");
pdata->w_qdd.resize(nq);
memcpy(pdata->w_qdd.data(),mxGetPr(pm),sizeof(double)*nq);
pdata->umin.resize(nu);
pdata->umax.resize(nu);
memcpy(pdata->umin.data(),mxGetPr(prhs[4]),sizeof(double)*nu);
memcpy(pdata->umax.data(),mxGetPr(prhs[5]),sizeof(double)*nu);
pdata->B_act.resize(nu,nu);
pdata->B_act = pdata->B.bottomRows(nu);
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
mexErrMsgIdAndTxt("Drake:QPControllermex:BadInputs","the seventh argument should be the map ptr");
memcpy(&pdata->map_ptr,mxGetPr(prhs[6]),sizeof(pdata->map_ptr));
// pdata->map_ptr = NULL;
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
// create gurobi environment
//.........这里部分代码省略.........