void BodyNode::fitWorldTransformParentJointImpl(
const Eigen::Isometry3d& _target, bool _jointLimit)
{
Joint* parentJoint = getParentJoint();
size_t dof = parentJoint->getNumGenCoords();
if (dof == 0)
return;
optimizer::Problem prob(dof);
// Use the current joint configuration as initial guess
prob.setInitialGuess(parentJoint->getConfigs());
// Objective function
TransformObjFunc obj(this, _target, mSkeleton);
prob.setObjective(&obj);
// Joint limit
if (_jointLimit)
{
prob.setLowerBounds(parentJoint->getConfigsMin());
prob.setUpperBounds(parentJoint->getConfigsMax());
}
// Solve with gradient-free local minima algorithm
optimizer::NloptSolver solver(&prob, NLOPT_LN_BOBYQA);
solver.solve();
// Set optimal configuration of the parent joint
Eigen::VectorXd jointQ = prob.getOptimalSolution();
parentJoint->setConfigs(jointQ, true, true, true);
}
