コード例 #1
0
ファイル: RLG.cpp プロジェクト: HargisJ/KrisLibrary
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
ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim
string IKCommandInterface::SpaceballEvent(const RigidTransform& T)
{
  if(currentLink >= 0) {
    RigidTransform Tlink;
    Config q;
    GetEndConfig(q);
    Robot* robot=GetRobot();
    robot->UpdateConfig(q);
    Tlink = robot->links[currentLink].T_World;

    IKGoal goal;
    goal.link = currentLink;
    goal.localPosition.setZero();
    RigidTransform Tgoal = Tlink*T;
    goal.SetFixedRotation(Tgoal.R);
    goal.SetFixedPosition(Tgoal.t);

    vector<IKGoal> problem(1,goal);
    int iters=100;
    bool res=SolveIK(*robot,problem,1e-3,iters,0);

    command = robot->q;
    sendCommand = true;
  }
  return "";
}
コード例 #3
0
ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim
string IKCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{ 
  if(drag) {
    if(currentLink >= 0) {
      //alter current desired configuration
      Config q;
      GetEndConfig(q);
      Robot* robot=GetRobot();

      robot->UpdateConfig(q);
      Vector3 wp = currentDestination;
      Vector3 ofs;
      Vector3 vv;
      viewport->getViewVector(vv);
      Real d = (wp-viewport->position()).dot(vv);
      viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
      currentDestination = wp+ofs;

      IKGoal goal;
      goal.link = currentLink;
      goal.localPosition = currentPoint;
      goal.SetFixedPosition(currentDestination);

      vector<IKGoal> problem(1,goal);
      int iters=100;
      bool res=SolveIK(*robot,problem,1e-3,iters,0);

      command = robot->q;
      sendCommand = true;
    }
    stringstream ss;
    ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
    return ss.str();
  }
  else {
    Ray3D ray;
    GetClickRay(mx,my,ray);

    Config q;
    GetCurConfig(q);
    Robot* robot=GetRobot();
    robot->UpdateConfig(q);

    int link;
    Vector3 localPos;
    RobotInfo* rob=world->ClickRobot(ray,link,localPos);
    if(rob) {
      currentLink = link;
      currentPoint = localPos;
      currentDestination = robot->links[link].T_World*localPos;
      world->robots[0].view.SetGrey();
      world->robots[0].view.colors[currentLink].set(1,1,0);
    }
    else {
      world->robots[0].view.SetGrey();
      currentLink = -1;
    }
    return "";
  }
}
コード例 #4
0
ファイル: robotik.cpp プロジェクト: ol3577600/Klampt
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;
  }
}
コード例 #5
0
ファイル: AnalyticIK.cpp プロジェクト: HargisJ/KrisLibrary
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);
}
コード例 #6
0
ファイル: InputProcessor.cpp プロジェクト: RGrant92/Klampt
PlannerObjectiveBase* PredictiveExtrapolationInputProcessor::MakeObjective(Robot* robot)
{
  if(!move) return NULL;
  assert(currentLink >= 0);

  if(tracking) {
    CartesianTrackingObjective* userGoal = new CartesianTrackingObjective(robot);
    userGoal->link = currentLink;
    userGoal->localPosition = currentPoint;
    userGoal->positions.push_back(currentDestination);
    userGoal->times.push_back(0.0);
    userGoal->weights.push_back(1.0);
    Real dt = 0.25;
    Real decay = Exp(-weightDecay*dt/predictionOffset);
    Real w=1.0;
    while(w > 0.01) {
      //integral of weightdecay exp(-weightdecay*t) from 0 to dt is
      //(1-exp(-weightdecay*dt))
      if(w*decay <= 0.01) break;
      userGoal->times.push_back(userGoal->times.back()+dt);
      userGoal->positions.push_back(currentDestination+(1.0-Exp(-userGoal->times.back()*speedDecay))/speedDecay*sumVelocity);
      userGoal->weights.push_back(w*(1-decay));
      w*=decay;
    }
    //weightdecay integral[0 to inf] w*exp(-weightdecay*t) = w
    userGoal->endPosWeight = w;
    lastObjective = userGoal;
    return userGoal;
  }
  else {
    IKGoal goal;
    goal.link = currentLink;
    goal.localPosition = currentPoint;
    goal.SetFixedPosition(currentDestination);
    CartesianObjective* userGoal = new CartesianObjective(robot);
    userGoal->ikGoal = goal;
    //predict where goal will be at the split time
    userGoal->ikGoal.endPosition += (currentTime+predictionOffset-currentInputTime)*sumVelocity;
    lastObjective = userGoal;
    return userGoal;
  }
}
コード例 #7
0
Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal)
{
  int m=IKGoal::NumDims(goal.posConstraint);
  int n=IKGoal::NumDims(goal.rotConstraint);
  Real poserr[3],orierr[3];
  if(goal.destLink < 0)
     goal.GetError(robot.links[goal.link].T_World,poserr,orierr);
  else {
    RigidTransform Trel;
    Trel.mulInverseB(robot.links[goal.link].T_World,robot.links[goal.destLink].T_World);
    goal.GetError(Trel,poserr,orierr);
  }

  Real emax=0.0;
  for(int i=0;i<m;i++)
    emax = Max(emax,Abs(poserr[i]));
  for(int i=0;i<n;i++)
    emax = Max(emax,Abs(orierr[i]));
  return emax;
}
コード例 #8
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;
}
コード例 #9
0
ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim
string PlannerCommandInterface::SpaceballEvent(const RigidTransform& T)
{
  if(currentLink >= 0) {
    RigidTransform Tlink;
    Config q;
    GetEndConfig(q);
    Robot* robot=GetRobot();
    robot->UpdateConfig(q);
    Tlink = robot->links[currentLink].T_World;

    IKGoal goal;
    goal.link = currentLink;
    goal.localPosition.setZero();
    goal.SetFixedPosition(Tlink.t+T.t);
    goal.SetFixedRotation(Tlink.R*T.R);

    if(planner) {
      CartesianObjective* obj=new CartesianObjective(robot);
      obj->ikGoal = goal;
      if(planner) planner->Reset(obj);
    }
  }
  return "";
}
コード例 #10
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;
  }
}
コード例 #11
0
void EvalIKError(const IKGoal& g,const RigidTransform& T,Real* poserr,Real* orierr)
{
  g.GetError(T,poserr,orierr);
}
コード例 #12
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;
}
コード例 #13
0
void TestRLG() {
  int n=3;
  //create random robot
  RobotKinematics3D chain;
  MakePlanarChain(chain,n);
  chain.UpdateFrames();
  IKGoal ikgoal;
  ikgoal.link=n-1;
  ikgoal.SetFixedPosition(Vector3(2,1,0));
  ikgoal.localPosition.set(1,0,0);
  JointStructure jschain(chain);
  jschain.Init();
  jschain.SolveRootBounds();
  cout<<"Initial bounds:"<<endl;
  for(int i=0;i<n;i++) {
    const WorkspaceBound& b=jschain.bounds[i];
    cout<<i<<" is "<<b<<endl;
  }
  getchar();
  jschain.IntersectWorkspaceBounds(ikgoal);
  cout<<"Bounds:"<<endl;
  for(int i=0;i<n;i++) {
    const WorkspaceBound& b=jschain.bounds[i];
    cout<<i<<" is "<<b<<endl;
  }
  getchar();

  vector<IKGoal> ik;
  ik.push_back(ikgoal);
  RLG rlg(chain,jschain);
  for(int i=0;i<100;i++) {
    rlg.SampleFrom(0);
    //set q(n-1) to minimize the distance between tgt and end effector
    Frame3D Tn=chain.links[n-2].T_World*chain.links[n-1].T0_Parent;
    Vector3 pLocal;
    Tn.mulPointInverse(ik[0].endPosition,pLocal);
    //try to align (1,0,0) with pLocal => q(n-1) = atan2(pLocal.y,pLocal.x)
    chain.q(n-1) = Clamp(Atan2(pLocal.y,pLocal.x),chain.qMin(n-1),chain.qMax(n-1));
    Frame3D Tloc;
    chain.links[n-1].GetLocalTransform(chain.q(n-1),Tloc);
    chain.links[n-1].T_World = Tn*Tloc;

    cout<<"Q is "<<VectorPrinter(chain.q)<<endl;
    if(!chain.InJointLimits(chain.q)) {
      cout<<"Chain out of joint limits!"<<endl;
      abort();
    }
    for(int k=0;k<chain.q.n;k++) {
      cout<<k<<": "<<chain.links[k].T_World.t<<endl;
      Frame3D tp;
      Frame3D tloc,tk;
      if(chain.parents[k]!=-1) {
	tp=chain.links[chain.parents[k]].T_World;
	chain.links[k].GetLocalTransform(chain.q(k),tloc);
	tk = tp*chain.links[k].T0_Parent*tloc;
	Assert(tk.R.isEqual(chain.links[k].T_World.R,1e-3));
	Assert(tk.t.isEqual(chain.links[k].T_World.t,1e-3));
      }
    }
    Vector3 p;
    chain.GetWorldPosition(ikgoal.localPosition,ikgoal.link,p);
    cout<<n<<": "<<p<<endl;
    cout<<"Goal distance "<<p.distance(ikgoal.endPosition)<<endl;
    getchar();
  }
}
コード例 #14
0
ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim
string PlannerCommandInterface::MouseInputEvent(int mx,int my,bool drag) 
{ 
  if(drag) {
    printf("Dragging event, dragging status %d\n",(int)dragging);
    if(!dragging) {
      //first click
      Ray3D ray;
      GetClickRay(mousex,mousey,ray);
      
      Config q;
      GetCurConfig(q);
      Robot* robot=GetRobot();
      robot->UpdateConfig(q);
      int link;
      Vector3 localPos;
      RobotInfo* rob=world->ClickRobot(ray,link,localPos);
      if(rob) {
	currentLink = link;
	currentPoint = localPos;
	currentDestination = robot->links[link].T_World*localPos;
	world->robots[0].view.SetGrey();
	world->robots[0].view.colors[currentLink].set(1,1,0);
      }
      else {
	world->robots[0].view.SetGrey();
	currentLink = -1;
      }
      dragging = true;
    }
    if(currentLink >= 0) {
      //alter current desired configuration
      Config q;
      GetEndConfig(q);

      Robot* robot=GetRobot();
      robot->UpdateConfig(q);
      Vector3 wp = currentDestination;
      Vector3 ofs;
      Vector3 vv;
      viewport->getViewVector(vv);
      Real d = (wp-viewport->position()).dot(vv);
      viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
      currentDestination = wp + ofs;

      IKGoal goal;
      goal.link = currentLink;
      goal.localPosition = currentPoint;
      goal.SetFixedPosition(currentDestination);

      if(planner) {
	CartesianObjective* obj=new CartesianObjective(robot);
	obj->ikGoal = goal;
	planner->Reset(obj);
      }
    }
    stringstream ss;
    ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
    return ss.str();
  }
  else {
    printf("Stop dragging event, dragging status %d\n",(int)dragging);
    dragging = false;
    mousex=mx;
    mousey=my;
    return "";
  }
}