本文整理汇总了C++中Vector6d::normalize方法的典型用法代码示例。如果您正苦于以下问题：C++ Vector6d::normalize方法的具体用法？C++ Vector6d::normalize怎么用？C++ Vector6d::normalize使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d的用法示例。
在下文中一共展示了Vector6d::normalize方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: pseudoinverseIK_chain
rk_result_t Robot::pseudoinverseIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues,
const Isometry3d &target, const Isometry3d &finalTF)
{
return RK_SOLVER_NOT_READY;
// FIXME: Make this solver work
vector<Linkage::Joint*> joints;
joints.resize(jointIndices.size());
// FIXME: Add in safety checks
for(int i=0; i<joints.size(); i++)
joints[i] = joints_[jointIndices[i]];
// ~~ Declarations ~~
MatrixXd J;
MatrixXd Jinv;
Isometry3d pose;
AngleAxisd aagoal;
AngleAxisd aastate;
Vector6d goal;
Vector6d state;
Vector6d err;
VectorXd delta(jointValues.size());
MatrixXd Jsub;
aagoal = target.rotation();
goal << target.translation(), aagoal.axis()*aagoal.angle();
tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily
maxIterations = 100; // TODO: Put this in the constructor so the user can set it arbitrarily
errorClamp = 0.25; // TODO: Put this in the constructor
deltaClamp = M_PI/4; // TODO: Put this in the constructor
size_t iterations = 0;
do {
values(jointIndices, jointValues);
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
Jsub = J.block(0,0,3,jointValues.size());
pinv(Jsub, Jinv);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
state << pose.translation(), aastate.axis()*aastate.angle();
err = goal-state;
for(int i=3; i<6; i++)
err[i] *= 0;
err.normalize();
Vector3d e = (target.translation() - pose.translation()).normalized()*0.005;
// delta = Jinv*err*0.1;
// clampMag(delta, deltaClamp);
VectorXd d = Jinv*e;
// jointValues += delta;
jointValues += d;
std::cout << iterations << " | Norm:" << delta.norm()
// << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
<< " | " << (target.translation() - pose.translation()).norm()
<< "\tErr: " << (goal-state).transpose() << std::endl;
iterations++;
} while(delta.norm() > tolerance && iterations < maxIterations);
}