Real IKObjective::TerminalCost(Real tend,const Vector& qend,const Vector& dqend)
{
Assert(ikGoal.link >= 0 && ikGoal.link < robot->q.n);
Assert(ikGoal.link >= 0 && ikGoal.link < (int)robot->links.size());
Assert(qend.n == robot->q.n);
robot->UpdateConfig(qend);
Vector3 poserr,orierr;
EvalIKError(ikGoal,robot->links[ikGoal.link].T_World,&poserr.x,&orierr.x);
return posCoeff*poserr.norm() + oriCoeff*orierr.norm();
}
bool ConstraintChecker::HasContact(const Robot& robot,const Stance& stance,Real dist)
{
Vector res;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const IKGoal& g=i->second.ikConstraint;
res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint));
EvalIKError(g,robot.links[i->first].T_World,res);
if(res.maxAbsElement() > dist) return false;
}
return true;
}
Real ConstraintChecker::ContactDistance(const Robot& robot,const Stance& stance)
{
Real maxErr = 0;
Vector res;
for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) {
const IKGoal& g=i->second.ikConstraint;
res.resize(IKGoal::NumDims(g.posConstraint)+IKGoal::NumDims(g.rotConstraint));
EvalIKError(g,robot.links[i->first].T_World,res);
Real m=res.maxAbsElement();
if(m > maxErr) maxErr = m;
}
return maxErr;
}