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; } }
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); }
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; } }
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; }
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; } }
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; }