本文整理汇总了C++中Vector6d::dot方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector6d::dot方法的具体用法?C++ Vector6d::dot怎么用?C++ Vector6d::dot使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d
的用法示例。
在下文中一共展示了Vector6d::dot方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: jacobianTransposeIK_chain
rk_result_t Robot::jacobianTransposeIK_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 state;
Vector6d err;
VectorXd delta(jointValues.size());
Vector6d gamma;
double alpha;
aagoal = target.rotation();
double Tscale = 3; // TODO: Put these as a class member in the constructor
double Rscale = 0;
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
size_t iterations = 0;
do {
values(jointIndices, jointValues);
jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);
pose = joint(jointIndices.back()).respectToRobot()*finalTF;
aastate = pose.rotation();
state << pose.translation(), aastate.axis()*aastate.angle();
err << (target.translation()-pose.translation()).normalized()*Tscale,
(aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis()).normalized()*Rscale;
gamma = J*J.transpose()*err;
alpha = err.dot(gamma)/gamma.norm();
delta = alpha*J.transpose()*err;
jointValues += delta;
iterations++;
std::cout << iterations << " | Norm:" << delta.norm()
// << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
<< " | " << (target.translation() - pose.translation()).norm()
<< "\tErr: " << (target.translation()-pose.translation()).transpose() << std::endl;
} while(err.norm() > tolerance && iterations < maxIterations);
}