Real MaxLinkParentDistance(const RobotKinematics3D& robot,int link)
{
if(robot.links[link].type == RobotLink3D::Revolute) return robot.links[link].T0_Parent.t.norm();
else {
Vector3 p1 = robot.links[link].T0_Parent.t + robot.qMin(link)*robot.links[link].w;
Vector3 p2 = robot.links[link].T0_Parent.t + robot.qMax(link)*robot.links[link].w;
return Max(p1.norm(),p2.norm());
}
}
void GetPassiveChainDOFs(const RobotKinematics3D& robot,int link,int numTerms,ArrayMapping& passiveDofs)
{
passiveDofs.mapping.resize(numTerms);
int d = link;
Assert(d >= 0 && d < (int)robot.parents.size());
for(int i=0;i<numTerms;i++) {
if(robot.qMin(d) != robot.qMax(d))
passiveDofs.mapping[i]=d;
else
i--;
d = robot.parents[d];
}
}
Real MaxLinkSiblingDistance(const RobotKinematics3D& robot,int link1,int link2)
{
if(robot.links[link1].type == RobotLink3D::Revolute) {
if(robot.links[link2].type == RobotLink3D::Revolute)
return robot.links[link1].T0_Parent.t.distance(robot.links[link2].T0_Parent.t);
else {
Vector3 p1 = robot.links[link2].T0_Parent.t + robot.qMin(link2)*robot.links[link2].w;
Vector3 p2 = robot.links[link2].T0_Parent.t + robot.qMax(link2)*robot.links[link2].w;
return Max(robot.links[link1].T0_Parent.t.distance(p1),robot.links[link1].T0_Parent.t.distance(p2));
}
}
else {
if(robot.links[link2].type == RobotLink3D::Revolute) {
Vector3 p1 = robot.links[link1].T0_Parent.t + robot.qMin(link1)*robot.links[link1].w;
Vector3 p2 = robot.links[link1].T0_Parent.t + robot.qMax(link1)*robot.links[link1].w;
return Max(robot.links[link2].T0_Parent.t.distance(p1),robot.links[link2].T0_Parent.t.distance(p2));
}
else {
Vector3 p1 = robot.links[link1].T0_Parent.t + robot.qMin(link1)*robot.links[link1].w;
Vector3 p2 = robot.links[link1].T0_Parent.t + robot.qMax(link1)*robot.links[link1].w;
Vector3 p3 = robot.links[link2].T0_Parent.t + robot.qMin(link2)*robot.links[link2].w;
Vector3 p4 = robot.links[link2].T0_Parent.t + robot.qMax(link2)*robot.links[link2].w;
return Sqrt(Max(Max(p1.distanceSquared(p3),p1.distanceSquared(p4)),Max(p2.distanceSquared(p3),p2.distanceSquared(p4))));
}
}
}
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());
}
void MakePlanarChain(RobotKinematics3D& robot,int n,Real d=One)
{
robot.Initialize(n);
for(int i=0;i<n;i++) {
robot.parents[i]=i-1;
robot.links[i].T0_Parent.setIdentity();
if(i > 0) robot.links[i].T0_Parent.t.x=d;
//if(RandBool())
if(i%2 == 0)
robot.links[i].SetRotationJoint(Vector3(0,0,1));
else
robot.links[i].SetRotationJoint(Vector3(0,1,0));
robot.links[i].mass=One;
robot.links[i].com.set(Half*d,0,0);
robot.links[i].inertia.setZero();
//for rod of negligible thickness
robot.links[i].inertia(1,1)=robot.links[i].inertia(2,2)=1.0/12.0*Sqr(d);
robot.qMin(i)=-Pi*0.75;
robot.qMax(i)=Pi*0.75;
}
}
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];
}
}
}
}
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);
}
}
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();
}
}
