void GetDefaultIKDofs(const RobotKinematics3D& robot,const vector<IKGoal>& ik,ArrayMapping& m)
{
set<int> dofs;
for(size_t i=0;i<ik.size();i++) {
if(ik[i].destLink >= 0) { //take into account the branching structure
int lca=robot.LCA(ik[i].link,ik[i].destLink);
int d=ik[i].link;
Assert(d >= 0 && d < (int)robot.parents.size());
while(d!=lca) {
if(robot.qMin(d) != robot.qMax(d))
dofs.insert(d);
d = robot.parents[d];
}
d=ik[i].destLink;
Assert(d >= 0 && d < (int)robot.parents.size());
while(d!=lca) {
if(robot.qMin(d) != robot.qMax(d))
dofs.insert(d);
d = robot.parents[d];
}
}
else {
int d=ik[i].link;
Assert(d >= 0 && d < (int)robot.parents.size());
while(d>=0 && dofs.count(d)==0) {
if(robot.qMin(d) != robot.qMax(d))
dofs.insert(d);
d = robot.parents[d];
}
}
}
m.mapping.resize(dofs.size());
copy(dofs.begin(),dofs.end(),m.mapping.begin());
}
Real MaxJointDistance(const RobotKinematics3D& robot,int link1,int link2)
{
int p = robot.LCA(link1,link2);
if(p < 0) {
fprintf(stderr,"MaxJointDistance Error: joints don't have a common parent?\n");
Abort();
return Inf;
}
//if link1 or link2 is an ancestor of the other, don't include the
//length of the branch
Real d1=0,d2=0;
if(link1 != p) {
while(robot.parents[link1]!=p) {
d1 += MaxLinkParentDistance(robot,link1);
link1 = robot.parents[link1];
}
Assert(link1 != p);
}
if(link2 != p) {
while(robot.parents[link2]!=p) {
d2 += MaxLinkParentDistance(robot,link2);
link2 = robot.parents[link2];
}
Assert(link2 != p);
}
if(link1 == p) {
if(link2 == p) return 0; //same link
else
return d2 + MaxLinkParentDistance(robot,link2);
}
else {
if(link2 == p) //2 is an ancestor of 1
return d1 + MaxLinkParentDistance(robot,link1);
else
return d1 + d2 + MaxLinkSiblingDistance(robot,link1,link2);
}
}
