void ComputeJointDistances(const RobotKinematics3D& robot,vector<vector<Real> >& dist)
{
vector<vector<int> > childList;
robot.GetChildList(childList);
dist.resize(robot.links.size());
for(size_t i=0;i<robot.links.size();i++)
dist[i].resize(robot.links.size(),Inf);
//floyd's algorithm
//initialization: edge distances
for(size_t i=0;i<robot.links.size();i++) {
dist[i][i] = 0;
if(robot.parents[i] >= 0) {
dist[i][robot.parents[i]] = dist[robot.parents[i]][i] = MaxLinkParentDistance(robot,i);
}
if(childList[i].size() >= 2) {
for(size_t j=1;j<childList[i].size();j++) {
for(size_t k=0;k<j;k++) {
Real d = MaxLinkSiblingDistance(robot,j,k);
if(d < dist[j][k]) dist[j][k] = dist[k][j] = d;
}
}
}
}
for(size_t k=0;k<robot.links.size();k++) {
//kth iteration: dist holds the lowest cost to go from i to j using only
//links 0...k-1.
//Consider adding vertex k. If it results in a shorter path, store it
for(size_t i=0;i<robot.links.size();i++) {
for(size_t j=0;j<robot.links.size();j++) {
if(dist[i][k] + dist[k][j] < dist[i][j])
dist[i][j] = dist[i][k] + dist[k][j];
}
}
}
}
void RobotKinematics3D::Subset(const RobotKinematics3D& robot,const vector<int>& subset)
{
Initialize(subset.size());
vector<int> linkMap(robot.links.size(),-1);
for(size_t i=0;i<subset.size();i++)
linkMap[subset[i]]=(int)i;
for(size_t i=0;i<subset.size();i++) {
links[i] = robot.links[subset[i]];
int j=robot.parents[subset[i]];
while(j != -1) {
if(linkMap[j] >= 0) break; //maps to a selected link
j = robot.parents[j];
}
//collapse the chain from linkMap[j] to sortedLinks[i]
if(j >= 0)
parents[i] = linkMap[j];
else
parents[i] = -1;
q(i) = robot.q(subset[i]);
qMin(i) = robot.qMin(subset[i]);
qMax(i) = robot.qMax(subset[i]);
}
//compute parent transformations by backtracking from nodes
for(size_t i=0;i<subset.size();i++) {
int j = robot.parents[subset[i]];
while(j != -1) {
if(linkMap[j] >= 0) break; //maps to a selected link
RigidTransform Tj;
robot.links[j].GetLocalTransform(robot.q(j),Tj);
//forward transform
links[i].T0_Parent = robot.links[j].T0_Parent * Tj * links[i].T0_Parent;
j = robot.parents[j];
}
}
//modify mass properties by propagating up from leaves
vector<vector<int> > children;
robot.GetChildList(children);
for(size_t i=0;i<subset.size();i++) {
//do we need to update? any child not included in selected subset
bool needUpdate = false;
for(size_t j=0;j<children[subset[i]].size();j++)
if(linkMap[children[subset[i]][j]] < 0) {
needUpdate = true;
break;
}
if(!needUpdate) continue;
//mass properties in world coordinates
Vector3 com(Zero);
Real mass(Zero);
Matrix3 inertia(Zero);
list<int> accumLinks;
queue<int> q;
//compute mass and com
q.push(subset[i]);
while(!q.empty()) {
int link = q.front(); q.pop();
if(linkMap[link] >= 0) continue; //it's in the remaining robot, break
accumLinks.push_back(link);
//descend to children
for(size_t j=0;j<children[link].size();j++)
q.push(children[link][j]);
}
for(list<int>::const_iterator j=accumLinks.begin();j!=accumLinks.end();j++) {
mass += robot.links[*j].mass;
com.madd(robot.links[*j].T_World*robot.links[*j].com,robot.links[*j].mass);
}
com /= mass;
for(list<int>::const_iterator j=accumLinks.begin();j!=accumLinks.end();j++) {
Matrix3 I;
InertiaMatrixAboutPoint(com,robot.links[*j].mass,robot.links[*j].T_World.R*robot.links[*j].inertia,com,I);
inertia += I;
}
links[i].mass = mass;
robot.links[subset[i]].T_World.mulInverse(com,links[i].com);
links[i].inertia.mulTransposeA(robot.links[subset[i]].T_World.R,inertia);
}
}
