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);
}
}
void TestRLG() {
int n=3;
//create random robot
RobotKinematics3D chain;
MakePlanarChain(chain,n);
chain.UpdateFrames();
IKGoal ikgoal;
ikgoal.link=n-1;
ikgoal.SetFixedPosition(Vector3(2,1,0));
ikgoal.localPosition.set(1,0,0);
JointStructure jschain(chain);
jschain.Init();
jschain.SolveRootBounds();
cout<<"Initial bounds:"<<endl;
for(int i=0;i<n;i++) {
const WorkspaceBound& b=jschain.bounds[i];
cout<<i<<" is "<<b<<endl;
}
getchar();
jschain.IntersectWorkspaceBounds(ikgoal);
cout<<"Bounds:"<<endl;
for(int i=0;i<n;i++) {
const WorkspaceBound& b=jschain.bounds[i];
cout<<i<<" is "<<b<<endl;
}
getchar();
vector<IKGoal> ik;
ik.push_back(ikgoal);
RLG rlg(chain,jschain);
for(int i=0;i<100;i++) {
rlg.SampleFrom(0);
//set q(n-1) to minimize the distance between tgt and end effector
Frame3D Tn=chain.links[n-2].T_World*chain.links[n-1].T0_Parent;
Vector3 pLocal;
Tn.mulPointInverse(ik[0].endPosition,pLocal);
//try to align (1,0,0) with pLocal => q(n-1) = atan2(pLocal.y,pLocal.x)
chain.q(n-1) = Clamp(Atan2(pLocal.y,pLocal.x),chain.qMin(n-1),chain.qMax(n-1));
Frame3D Tloc;
chain.links[n-1].GetLocalTransform(chain.q(n-1),Tloc);
chain.links[n-1].T_World = Tn*Tloc;
cout<<"Q is "<<VectorPrinter(chain.q)<<endl;
if(!chain.InJointLimits(chain.q)) {
cout<<"Chain out of joint limits!"<<endl;
abort();
}
for(int k=0;k<chain.q.n;k++) {
cout<<k<<": "<<chain.links[k].T_World.t<<endl;
Frame3D tp;
Frame3D tloc,tk;
if(chain.parents[k]!=-1) {
tp=chain.links[chain.parents[k]].T_World;
chain.links[k].GetLocalTransform(chain.q(k),tloc);
tk = tp*chain.links[k].T0_Parent*tloc;
Assert(tk.R.isEqual(chain.links[k].T_World.R,1e-3));
Assert(tk.t.isEqual(chain.links[k].T_World.t,1e-3));
}
}
Vector3 p;
chain.GetWorldPosition(ikgoal.localPosition,ikgoal.link,p);
cout<<n<<": "<<p<<endl;
cout<<"Goal distance "<<p.distance(ikgoal.endPosition)<<endl;
getchar();
}
}
