示例#1
0
void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
  Assert(goal.posConstraint==IKGoal::PosFixed);
  switch(goal.rotConstraint) {
  case IKGoal::RotFixed:
    goal.GetFixedGoalTransform(T);
    break;
  case IKGoal::RotTwoAxis:
    FatalError("Can't have 2 axis terms rotation!");
    break;
  case IKGoal::RotAxis:
    {
      Real theta = Rand()*TwoPi;
      goal.GetEdgeGoalTransform(theta,T);
    }
    break;
  case IKGoal::RotNone:
    {
      QuaternionRotation q;
      RandRotation(q);
      q.getMatrix(T.R);
      T.t = goal.endPosition - T.R*goal.localPosition;
    }
    break;
  }
}
示例#2
0
bool AnalyticIKMap::Solve(RobotKinematics3D& robot,
	   const IKGoal& goal,
	   AnalyticIKSolution& solution) const 
{
  assert(goal.destLink == -1);
  int numdofs = IKGoal::NumDims(goal.posConstraint)+IKGoal::NumDims(goal.rotConstraint);
  assert(numdofs == 3 || numdofs == 6);
  //find the appropriate solver
  int link = goal.link;
  int base = link;
  for(int i=0;i+1<numdofs;i++) {
    base = robot.parents[base];
  }
  std::pair<int,int> ind(base,goal.link);
  std::map<std::pair<int,int>,SmartPointer<AnalyticalIKSolver> >::const_iterator solver = solvers.find(ind);
  if(solver == solvers.end())
    return false;

  RigidTransform Trel,Tworld,TbaseInv;
  if(numdofs == 6)
    goal.GetFixedGoalTransform(Tworld);
  else {
    Tworld.R.setIdentity();
    Tworld.t = goal.endPosition;
  }
  TbaseInv.setInverse(robot.links[robot.parents[base]].T_World*robot.links[base].T0_Parent);
  Trel = TbaseInv * Tworld;
  return solver->second->Solve(base,goal.link,Trel,solution);
}
示例#3
0
void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
  assert(goal.posConstraint == IKGoal::PosFixed);
  if(goal.rotConstraint == IKGoal::RotFixed) {
    goal.GetFixedGoalTransform(T);
  }
  else if (goal.rotConstraint == IKGoal::RotAxis) {
    goal.GetEdgeGoalTransform(Rand(0,TwoPi),T);
  }
  else {
    QuaternionRotation q=RandRotation();
    q.getMatrix(T.R);
    T.t = goal.endPosition - T.R*goal.localPosition;
  }
}
示例#4
0
文件: Hold.cpp 项目: bbgw/RobotSim
int ikcmp (const IKGoal& a,const IKGoal& b)
{
  Assert(a.posConstraint==IKGoal::PosFixed && b.posConstraint==IKGoal::PosFixed);
  int cmp;
  if(int(a.rotConstraint) < int(b.rotConstraint)) return -1;
  if(int(a.rotConstraint) > int(b.rotConstraint)) return 1;
  if(a.rotConstraint == IKGoal::RotFixed) {
    //only the transform matters
    RigidTransform Ta,Tb;
    a.GetFixedGoalTransform(Ta);
    b.GetFixedGoalTransform(Tb);
    if((cmp=vec3cmp_fuzzy(Ta.t,Tb.t))!=0) return cmp;
    if((cmp=mat3cmp_fuzzy(Ta.R,Tb.R))!=0) return cmp;
  }
  else {
    if((cmp=vec3cmp_fuzzy(a.endPosition,b.endPosition))!=0) return cmp;
    if((cmp=vec3cmp_fuzzy(a.localPosition,b.localPosition))!=0) return cmp;
    if(a.rotConstraint != IKGoal::RotNone) {
      if((cmp=vec3cmp_fuzzy(a.endRotation,b.endRotation))!=0) return cmp;
      if((cmp=vec3cmp_fuzzy(a.localAxis,b.localAxis))!=0) return cmp;
    }
  }
  return 0;
}
示例#5
0
void GetConstraintPoints(const IKGoal& g,vector<Vector3>& lp,vector<Vector3>& wp)
{
  Assert(g.posConstraint == IKGoal::PosFixed);
  switch(g.rotConstraint) {
  case IKGoal::RotNone:
    lp.resize(1);
    wp.resize(1);
    lp[0] = g.localPosition;
    wp[0] = g.endPosition;
    break;
  case IKGoal::RotAxis:
    lp.resize(2);
    wp.resize(2);
    lp[0] = g.localPosition;
    wp[0] = g.endPosition;
    lp[1] = g.localPosition + g.localAxis;
    wp[1] = g.endPosition + g.endRotation;
    break;
  case IKGoal::RotFixed:
    lp.resize(3);
    wp.resize(3);
    lp[0] = g.localPosition;
    wp[0] = g.endPosition;
    {
      RigidTransform T;
      g.GetFixedGoalTransform(T);
      lp[1] = g.localPosition;
      lp[1].x += 1.0;
      wp[1] = T * lp[1];
      lp[2] = g.localPosition;
      lp[2].y += 1.0;
      wp[1] = T * lp[2];
    }
    break;
  default:
    fprintf(stderr,"Two-axis rotations not supported\n");
    break;
  }
}
示例#6
0
bool IntersectGoals(const IKGoal& a,const IKGoal& b,IKGoal& c,Real tolerance)
{
  Assert(a.link == b.link);
  Assert(a.destLink == b.destLink);
  //fixed constraints get copied over if compatible
  if(b.posConstraint == IKGoal::PosFixed && b.rotConstraint == IKGoal::RotFixed) {
    RigidTransform T;
    b.GetFixedGoalTransform(T);
    Real poserr[3],roterr[3];
    a.GetError(T,poserr,roterr);
    for(int j=0;j<IKGoal::NumDims(a.posConstraint);j++)
      if(poserr[j] > tolerance) return false;
    for(int j=0;j<IKGoal::NumDims(a.rotConstraint);j++)
      if(roterr[j] > tolerance) return false;
    c = b;
    return true;
  }
  if(a.posConstraint == IKGoal::PosFixed && a.rotConstraint == IKGoal::RotFixed) {
    return IntersectGoals(b,a,c,tolerance);
  }
  //empty goals
  if(a.posConstraint == IKGoal::PosNone && a.rotConstraint == IKGoal::RotNone) {
    c = b;
    return true;
  }
  if(b.posConstraint == IKGoal::PosNone && b.rotConstraint == IKGoal::RotNone) {
    c = a;
    return true;
  }

  c.link = a.link;
  c.destLink = a.destLink;

  if(a.posConstraint == IKGoal::PosFixed && b.posConstraint == IKGoal::PosFixed) {
    //a fixes certain points and b fixes certain other points -- merge them together
    vector<Vector3> aplocal,apworld,bplocal,bpworld;
    GetConstraintPoints(a,aplocal,apworld);
    GetConstraintPoints(b,bplocal,bpworld);
    aplocal.insert(aplocal.end(),bplocal.begin(),bplocal.end());
    apworld.insert(apworld.end(),bpworld.begin(),bpworld.end());
    c.SetFromPoints(aplocal,apworld,tolerance);
    return true;
  }

  //handle rotation constraints first
  if(a.rotConstraint == IKGoal::RotNone) {
    c.rotConstraint = b.rotConstraint;
    c.localAxis = b.localAxis;
    c.endRotation = b.endRotation;
  }
  else if(b.rotConstraint == IKGoal::RotNone) {
    c.rotConstraint = a.rotConstraint;
    c.localAxis = a.localAxis;
    c.endRotation = a.endRotation;
  }
  else if(a.rotConstraint == IKGoal::RotFixed) {
    c.rotConstraint = a.rotConstraint;
    c.endRotation = a.endRotation;
    //now determine compatibility
    if(b.rotConstraint == IKGoal::RotFixed) {
      if(!a.endRotation.isEqual(b.endRotation,tolerance)) return false;
    }
    else if(b.rotConstraint == IKGoal::RotAxis) {
      MomentRotation m(a.endRotation);
      Matrix3 R;
      m.getMatrix(R);
      if(!b.endRotation.isEqual(R*b.localAxis)) return false;
    }
    else {
      fprintf(stderr,"Two-axis rotations not supported\n");
      return false;
    }
  }
  else if(b.rotConstraint == IKGoal::RotFixed) {
    c.rotConstraint = b.rotConstraint;
    c.endRotation = b.endRotation;
    //now determine compatibility
    if(a.rotConstraint == IKGoal::RotFixed) {
      if(!a.endRotation.isEqual(b.endRotation,tolerance)) return false;
    }
    else if(a.rotConstraint == IKGoal::RotAxis) {
      MomentRotation m(b.endRotation);
      Matrix3 R;
      m.getMatrix(R);
      if(!a.endRotation.isEqual(R*a.localAxis,tolerance)) return false;
    }
    else {
      fprintf(stderr,"Two-axis rotations not supported\n");
      return false;
    }
  }
  else if(a.rotConstraint == IKGoal::RotTwoAxis || b.rotConstraint == IKGoal::RotTwoAxis) {
    fprintf(stderr,"Two-axis rotations not supported\n");
    return false;
  }
  else {
    Assert(a.rotConstraint == IKGoal::RotAxis && b.rotConstraint == IKGoal::RotAxis);
    //if compatible, set a single axis constraint
    //otherwise, set the proper rotation matrix
    if((a.localAxis.isEqual(b.localAxis,tolerance) && a.endRotation.isEqual(b.endRotation,tolerance)) || 
       (a.localAxis.isEqual(-b.localAxis,tolerance) && a.endRotation.isEqual(-b.endRotation,tolerance))) {
      c.rotConstraint = IKGoal::RotAxis;
      c.localAxis = a.localAxis;
      c.endRotation = a.endRotation;
    }
    else {
      //TODO find a rotation matrix that maps both axes to their directions
      fprintf(stderr,"TODO: intersect two axis rotations\n");
      return false;
    }
  }

  //c's rotation is now chosen.
  //now deal with the positions, at least one of which must be non-fixed
  if(a.posConstraint == IKGoal::PosNone) {
    //copy b's position constraint 
    c.posConstraint = b.posConstraint;
    c.localPosition = b.localPosition;
    c.endPosition = b.endPosition;
    c.direction = b.direction;
    return true;
  }
  else if(b.posConstraint == IKGoal::PosNone) {
    //copy a's position constraint 
    c.posConstraint = a.posConstraint;
    c.localPosition = a.localPosition;
    c.endPosition = a.endPosition;
    c.direction = a.direction;
    return true;
  }
  if(c.rotConstraint == IKGoal::RotFixed) {
    MomentRotation m(c.endRotation);
    Matrix3 R;
    m.getMatrix(R);
    //check compatibility of fixed directions
    if(a.posConstraint == IKGoal::PosFixed) {
      c.posConstraint = a.posConstraint;
      c.localPosition = a.localPosition;
      c.endPosition = a.endPosition;
      RigidTransform T;
      c.GetFixedGoalTransform(T);
      if(b.posConstraint == IKGoal::PosLinear) {
	if(!cross(b.direction,T*b.localPosition-b.endPosition).isZero(tolerance)) return false;
      }
      else if(b.posConstraint == IKGoal::PosPlanar) {
	if(!FuzzyZero(b.direction.dot(T*b.localPosition-b.endPosition),tolerance)) return false;
      }
      return true;
    }
    else if(b.posConstraint == IKGoal::PosFixed) {
      c.posConstraint = b.posConstraint;
      c.localPosition = b.localPosition;
      c.endPosition = b.endPosition;
      RigidTransform T;
      c.GetFixedGoalTransform(T);
      if(a.posConstraint == IKGoal::PosLinear) {
	if(!cross(a.direction,T*a.localPosition-a.endPosition).isZero(tolerance)) return false;
      }
      else if(a.posConstraint == IKGoal::PosPlanar) {
	if(!FuzzyZero(a.direction.dot(T*a.localPosition-a.endPosition),tolerance)) return false;
      }
      return true;
    }
    else {
      //fixed rotation, linear or planar positions
      fprintf(stderr,"TODO: merging linear or planar position constraints\n");
      return false;
    }
  }
  //check equality
  if(a.posConstraint == b.posConstraint) {
    if(a.localPosition.isEqual(b.localPosition,tolerance) &&
       a.endPosition.isEqual(b.endPosition,tolerance) &&
       a.direction.isEqual(b.direction,tolerance)) {
      c.posConstraint = a.posConstraint;
      c.localPosition = a.localPosition;
      c.endPosition = a.endPosition;
      return true;
    }
  }
  //non-fixed rotation, linear or planar positions
  fprintf(stderr,"TODO: merging linear or planar position constraints with non-fixed rotations\n");
  return false;
}