C++ (Cpp) Updater::tanAcc示例

void testTaskNumDiff(const std::vector<rbd::MultiBody>& mbs,
	const std::vector<rbd::MultiBodyConfig>& mbcs,
	Task& task, Updater updater, Tester tester,
	int nrIter=100, double diffStep=1e-6, double tol=1e-4)
{
	using namespace Eigen;
	using namespace sva;
	using namespace rbd;

	std::vector<MultiBodyConfig> mbcsPost(mbcs), mbcsCur(mbcs);

	std::vector<Eigen::VectorXd> q(mbs.size());
	std::vector<Eigen::VectorXd> alpha(mbs.size());
	std::vector<Eigen::VectorXd> alphaD(mbs.size());

	for(int i = 0; i < nrIter; ++i)
	{
		for(std::size_t r = 0; r < mbs.size(); ++r)
		{
			const rbd::MultiBody& mb = mbs[r];
			const rbd::MultiBodyConfig& mbc = mbcs[r];
			rbd::MultiBodyConfig& mbcPost = mbcsPost[r];
			rbd::MultiBodyConfig& mbcCur = mbcsCur[r];

			q[r].setRandom(mb.nrParams());
			alpha[r].setRandom(mb.nrDof());
			alphaD[r].setRandom(mb.nrDof());

			mbcCur = mbc;
			vectorToParam(q[r], mbcCur.q);
			vectorToParam(alpha[r], mbcCur.alpha);
			vectorToParam(alphaD[r], mbcCur.alphaD);

			mbcPost = mbcCur;

			eulerIntegration(mb, mbcPost, diffStep);

			forwardKinematics(mb, mbcCur);
			forwardKinematics(mb, mbcPost);
			forwardVelocity(mb, mbcCur);
			forwardVelocity(mb, mbcPost);
		}

		updater(task, mbs, mbcsCur);
		VectorXd evalCur = task.eval();
		VectorXd speedCur = -task.speed();
		VectorXd accCur = -task.normalAcc();
		accCur -= updater.tanAcc(task, alphaD);

		updater(task, mbs, mbcsPost);
		VectorXd evalPost = task.eval();
		VectorXd speedPost = -task.speed();

		VectorXd speedDiff = (evalPost - evalCur)/diffStep;
		VectorXd accDiff = (speedPost - speedCur)/diffStep;

		tester(speedCur, accCur, speedDiff, accDiff, tol);
	}
}