本文整理汇总了C++中Vector6d::normalized方法的典型用法代码示例。如果您正苦于以下问题：C++ Vector6d::normalized方法的具体用法？C++ Vector6d::normalized怎么用？C++ Vector6d::normalized使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d的用法示例。
在下文中一共展示了Vector6d::normalized方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: moveFoot
/* ********************************************************************************************* */
void moveFoot(const Eigen::VectorXd& dx, bool left, Vector6d& dq) {
// Get the jacobian
Eigen::MatrixXd J = hubo->getBodyNode(left ? "leftFoot" : "rightFoot")
->getWorldJacobian().bottomRightCorner<6,6>();
Eigen::MatrixXd temp = J.topRightCorner<3,6>();
J.topRightCorner<3,6>() = J.bottomRightCorner<3,6>();
J.bottomRightCorner<3,6>() = temp;
for(size_t i = 0; i < 6; i++) J(i,i) += 0.005;
if(dbg) std::cout << "J= [\n" << J << "];\n";
// Compute the inverse
Eigen::MatrixXd JInv;
JInv = J;
aa_la_inv(6, JInv.data());
// Compute joint space velocity
if(dbg) cout << "dxRightLeg: " << dx.transpose() << endl;
dq = (JInv * dx);
if(dq.norm() > max_step_size) dq = dq.normalized() * max_step_size;
if(dbg) cout << "dqRightLeg: " << dq.transpose() << endl;
}