本文整理汇总了C++中eigen::VectorXf::head方法的典型用法代码示例。如果您正苦于以下问题：C++ VectorXf::head方法的具体用法？C++ VectorXf::head怎么用？C++ VectorXf::head使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::VectorXf的用法示例。
在下文中一共展示了VectorXf::head方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: hist
void
shiftHistogram (const Eigen::VectorXf &hist, Eigen::VectorXf &hist_shifted, bool direction)
{
int bins = hist.rows();
hist_shifted = Eigen::VectorXf::Zero(bins);
if(direction){ //shift right
hist_shifted.tail(bins - 1) = hist.head(bins-1);
hist_shifted(bins-1) += hist(bins-1);
}
else { // shift left
hist_shifted.head(bins - 1) = hist.tail(bins-1);
hist_shifted(0) += hist(0);
}
}示例2: updateJointInfo
void SimDynamicsWindow::updateJointInfo()
{
//std::stringstream info;
std::string info;
int n = UI.comboBoxRobotNode->currentIndex();
QString qMin("0");
QString qMax("0");
QString qName("Name: <not set>");
QString qJV("Joint value: 0");
QString qTarget("Joint target: 0");
QString qVel("Joint velocity: 0");
QString qVelTarget("Joint velocity target: 0");
QString qGP("GlobalPose (simox): 0/0/0");
QString qVisu("VISU (simox): 0/0/0");
QString qCom("COM (bullet): 0/0/0");
QString tmp;
RobotNodeRevolutePtr rn;
if (n >= 0 && n < (int)robotNodes.size())
{
rn = robotNodes[n];
}
SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
SimDynamics::BulletObjectPtr bulletRN = boost::dynamic_pointer_cast<SimDynamics::BulletObject>(dynRN);
if (bulletRN)
{
// cout << "FORCE: " << bulletRN->getRigidBody()->getTotalForce()[0] << ", " << bulletRN->getRigidBody()->getTotalForce()[1] << ", " << bulletRN->getRigidBody()->getTotalForce()[2] << endl;
// cout << "TORQUE: " << bulletRN->getRigidBody()->getTotalTorque()[0] << ", " << bulletRN->getRigidBody()->getTotalTorque()[1] << ", " << bulletRN->getRigidBody()->getTotalTorque()[2] << endl;
// cout << "getLinearVelocity: " << bulletRN->getRigidBody()->getLinearVelocity()[0] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[1] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[2] << endl;
// cout << "getAngularVelocity: " << bulletRN->getRigidBody()->getAngularVelocity()[0] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[1] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[2] << endl;
}
BulletRobotPtr bulletRobot = boost::dynamic_pointer_cast<SimDynamics::BulletRobot>(dynamicsRobot);
if (rn && bulletRobot && bulletRobot->hasLink(rn))
{
BulletRobot::LinkInfo linkInfo = bulletRobot->getLink(rn);
linkInfo.nodeA->showCoordinateSystem(true);
linkInfo.nodeB->showCoordinateSystem(true);
bulletRobot->enableForceTorqueFeedback(linkInfo);
Eigen::VectorXf ftA = bulletRobot->getForceTorqueFeedbackA(linkInfo);
Eigen::VectorXf ftB = bulletRobot->getForceTorqueFeedbackB(linkInfo);
std::stringstream streamFTA;
streamFTA << "ForceTorqueA: " << std::fixed;
streamFTA.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTA << ftA[i] << ",";
}
UI.label_ForceTorqueA->setText(QString::fromStdString(streamFTA.str()));
std::stringstream streamFTB;
streamFTB << "ForceTorqueB: " << std::fixed;
streamFTB.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTB << ftB[i] << ",";
}
UI.label_ForceTorqueB->setText(QString::fromStdString(streamFTB.str()));
// cout << "ForceTorqueA:" << endl;
// MathTools::print(ftA);
// cout << "ForceTorqueB:" << endl;
// MathTools::print(ftB);
forceSep->removeAllChildren();
SoUnits* u = new SoUnits();
u->units = SoUnits::MILLIMETERS;
forceSep->addChild(u);
Eigen::Vector3f n = ftA.head(3);
//n = linkInfo.nodeA->toGlobalCoordinateSystemVec(n);
float l = ftA.head(3).norm();
float w = 5.0f;
SoSeparator* forceA = new SoSeparator;
SoSeparator* arrowForceA = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());
/*
cout << "FORCE_A: " << linkInfo.dynNode1->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEA: " << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[2] << endl;
cout << "FORCE_B: " << linkInfo.dynNode2->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEB: " << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[2] << endl;
*/
// show as nodeA local coords system
/*
Eigen::Matrix4f comCoord = linkInfo.nodeA->getGlobalPose();
Eigen::Matrix4f comLocal = Eigen::Matrix4f::Identity();
comLocal.block(0,3,3,1) = linkInfo.nodeA->getCoMLocal();
//.........这里部分代码省略.........