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; } }
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 ""; }
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 ""; } }
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; } }
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); }
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; } }
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; }
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; }
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 ""; }
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; } }
void EvalIKError(const IKGoal& g,const RigidTransform& T,Real* poserr,Real* orierr) { g.GetError(T,poserr,orierr); }
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; }
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(); } }
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 ""; } }