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