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); }
void Skeleton::commitAnim() { for (int m = 0; m < _numJoints; ++m) { const Joint *parent = getParentJoint(&_joints[m]); if (parent) { _joints[m]._finalMatrix = parent->_finalMatrix * _joints[m]._finalMatrix; } } }
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); }