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