Exemplo n.º 1
0
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();
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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;
}