void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, BodyNode::InverseKinematicsPolicy /*_policy*/, bool _jointVelLimit) { // TODO: Only IKP_PARENT_JOINT policy is supported now. 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->getGenVels()); // Objective function VelocityObjFunc obj(this, _targetAngVel, VelocityObjFunc::VT_ANGULAR, mSkeleton); prob.setObjective(&obj); // Joint limit if (_jointVelLimit) { prob.setLowerBounds(parentJoint->getGenVelsMin()); prob.setUpperBounds(parentJoint->getGenVelsMax()); } // 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 jointDQ = prob.getOptimalSolution(); parentJoint->setGenVels(jointDQ, true, true); }