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