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); }