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