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