本文整理汇总了C++中VectorXd::topRows方法的典型用法代码示例。如果您正苦于以下问题：C++ VectorXd::topRows方法的具体用法？C++ VectorXd::topRows怎么用？C++ VectorXd::topRows使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类VectorXd的用法示例。
在下文中一共展示了VectorXd::topRows方法的5个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: operator
VectorXd operator()(const VectorXd& x) const {
m_config->SetDOFValues(toDblVec(x.topRows(m_nDof)));
OR::Vector ptWorldA, ptWorldB, nWorldA, nWorldB;
CalcWorldPoints(x, ptWorldA, ptWorldB);
return toVector3d(ptWorldA - ptWorldB);
}示例2: operator
VectorXd LinearizedControlError::operator()(const VectorXd& a) const {
Vector6d x1 = a.topRows(6);
Vector6d x2 = a.middleRows(6,6);
double phi = a(12), Delta = a(13), curvature = a(14);
double phi_ref = cfg0->phi, Delta_ref = cfg0->Delta, curvature_ref = cfg0->curvature;
MatrixXd F = MatrixXd::Zero(6,6);
F.topLeftCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
F.topRightCorner(3, 3) = -rotMat(Vector3d(0, 0, Delta_ref));
F.bottomRightCorner(3, 3) = -rotMat(Vector3d(Delta_ref*curvature_ref, 0, phi_ref));
MatrixXd G = MatrixXd::Zero(6,6);
G(2, 0) = 1;
G(3, 2) = 1;
G(5, 1) = 1;
MatrixXd A = F.exp();
MatrixXd halfF = 0.5*F;
MatrixXd B = (1/6.0) * (G + 4.0*(halfF.exp()*G) + A*G);
Vector3d u;
u << Delta - Delta_ref, phi - phi_ref, Delta*curvature - Delta_ref*curvature_ref;
return x2 - A * x1 - B * u;
}示例3: Plot
void FaceContactConstraint::Plot(const DblVec& xvec, OR::EnvironmentBase& env, std::vector<OR::GraphHandlePtr>& handles) {
FaceContactErrorCalculator* calc = static_cast<FaceContactErrorCalculator*>(f_.get());
OR::Vector ptA, ptB;
VectorXd x = getVec(xvec, vars_);
calc->m_config->SetDOFValues(toDblVec(x.topRows(calc->m_nDof)));
calc->CalcWorldPoints(x, ptA, ptB);
handles.push_back(env.drawarrow(ptA, ptB, .001, OR::Vector(1, 0, 1, 1)));
}示例4: mexFunction
//.........这里部分代码省略.........
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies = myGetField(prhs[desired_support_argid],"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPr(mxBodies);
mxArray* mxContactPts = myGetField(prhs[desired_support_argid],"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
mxArray* mxContactSurfaces = myGetField(prhs[desired_support_argid],"contact_surfaces");
if (!mxContactSurfaces) mexErrMsgTxt("couldn't get contact surfaces");
double* pContactSurfaces = mxGetPr(mxContactSurfaces);
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
int nc = mxGetNumberOfElements(mxBodyContactPts);
if (nc<1) continue;
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
pr = mxGetPr(mxBodyContactPts);
for (j=0; j<nc; j++) {
se.contact_pt_inds.insert((int)pr[j]-1);
}
se.contact_surface = (int) pContactSurfaces[i]-1;
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
pdata->r->HandC(q,qd,(MatrixXd*)NULL,pdata->H,pdata->C,(MatrixXd*)NULL,(MatrixXd*)NULL,(MatrixXd*)NULL);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (pdata->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
Map<VectorXd> qdvec(qd,nq);
示例5: setupAndSolveQP
//.........这里部分代码省略.........
// mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0,
// desired_body_accelerations[i].body_vdot(0),
// desired_body_accelerations[i].body_vdot(1),
// desired_body_accelerations[i].body_vdot(2),
// desired_body_accelerations[i].body_vdot(3),
// desired_body_accelerations[i].body_vdot(4),
// desired_body_accelerations[i].body_vdot(5),
// weight);
// mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0,
}
int n_body_accel_eq_constraints = 0;
for (int i=0; i < desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0)
n_body_accel_eq_constraints++;
}
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input);
vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height);
int num_active_contact_pts=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
num_active_contact_pts += iter->contact_pts.size();
}
pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
MatrixXd B,JB,Jp,Jpdot,normals;