void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
Assert(goal.posConstraint==IKGoal::PosFixed);
switch(goal.rotConstraint) {
case IKGoal::RotFixed:
goal.GetFixedGoalTransform(T);
break;
case IKGoal::RotTwoAxis:
FatalError("Can't have 2 axis terms rotation!");
break;
case IKGoal::RotAxis:
{
Real theta = Rand()*TwoPi;
goal.GetEdgeGoalTransform(theta,T);
}
break;
case IKGoal::RotNone:
{
QuaternionRotation q;
RandRotation(q);
q.getMatrix(T.R);
T.t = goal.endPosition - T.R*goal.localPosition;
}
break;
}
}
string IKCommandInterface::SpaceballEvent(const RigidTransform& T)
{
if(currentLink >= 0) {
RigidTransform Tlink;
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Tlink = robot->links[currentLink].T_World;
IKGoal goal;
goal.link = currentLink;
goal.localPosition.setZero();
RigidTransform Tgoal = Tlink*T;
goal.SetFixedRotation(Tgoal.R);
goal.SetFixedPosition(Tgoal.t);
vector<IKGoal> problem(1,goal);
int iters=100;
bool res=SolveIK(*robot,problem,1e-3,iters,0);
command = robot->q;
sendCommand = true;
}
return "";
}
string IKCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp+ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
vector<IKGoal> problem(1,goal);
int iters=100;
bool res=SolveIK(*robot,problem,1e-3,iters,0);
command = robot->q;
sendCommand = true;
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
Ray3D ray;
GetClickRay(mx,my,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
return "";
}
}
void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
assert(goal.posConstraint == IKGoal::PosFixed);
if(goal.rotConstraint == IKGoal::RotFixed) {
goal.GetFixedGoalTransform(T);
}
else if (goal.rotConstraint == IKGoal::RotAxis) {
goal.GetEdgeGoalTransform(Rand(0,TwoPi),T);
}
else {
QuaternionRotation q=RandRotation();
q.getMatrix(T.R);
T.t = goal.endPosition - T.R*goal.localPosition;
}
}
bool AnalyticIKMap::Solve(RobotKinematics3D& robot,
const IKGoal& goal,
AnalyticIKSolution& solution) const
{
assert(goal.destLink == -1);
int numdofs = IKGoal::NumDims(goal.posConstraint)+IKGoal::NumDims(goal.rotConstraint);
assert(numdofs == 3 || numdofs == 6);
//find the appropriate solver
int link = goal.link;
int base = link;
for(int i=0;i+1<numdofs;i++) {
base = robot.parents[base];
}
std::pair<int,int> ind(base,goal.link);
std::map<std::pair<int,int>,SmartPointer<AnalyticalIKSolver> >::const_iterator solver = solvers.find(ind);
if(solver == solvers.end())
return false;
RigidTransform Trel,Tworld,TbaseInv;
if(numdofs == 6)
goal.GetFixedGoalTransform(Tworld);
else {
Tworld.R.setIdentity();
Tworld.t = goal.endPosition;
}
TbaseInv.setInverse(robot.links[robot.parents[base]].T_World*robot.links[base].T0_Parent);
Trel = TbaseInv * Tworld;
return solver->second->Solve(base,goal.link,Trel,solution);
}
PlannerObjectiveBase* PredictiveExtrapolationInputProcessor::MakeObjective(Robot* robot)
{
if(!move) return NULL;
assert(currentLink >= 0);
if(tracking) {
CartesianTrackingObjective* userGoal = new CartesianTrackingObjective(robot);
userGoal->link = currentLink;
userGoal->localPosition = currentPoint;
userGoal->positions.push_back(currentDestination);
userGoal->times.push_back(0.0);
userGoal->weights.push_back(1.0);
Real dt = 0.25;
Real decay = Exp(-weightDecay*dt/predictionOffset);
Real w=1.0;
while(w > 0.01) {
//integral of weightdecay exp(-weightdecay*t) from 0 to dt is
//(1-exp(-weightdecay*dt))
if(w*decay <= 0.01) break;
userGoal->times.push_back(userGoal->times.back()+dt);
userGoal->positions.push_back(currentDestination+(1.0-Exp(-userGoal->times.back()*speedDecay))/speedDecay*sumVelocity);
userGoal->weights.push_back(w*(1-decay));
w*=decay;
}
//weightdecay integral[0 to inf] w*exp(-weightdecay*t) = w
userGoal->endPosWeight = w;
lastObjective = userGoal;
return userGoal;
}
else {
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
CartesianObjective* userGoal = new CartesianObjective(robot);
userGoal->ikGoal = goal;
//predict where goal will be at the split time
userGoal->ikGoal.endPosition += (currentTime+predictionOffset-currentInputTime)*sumVelocity;
lastObjective = userGoal;
return userGoal;
}
}
Real RobotIKError(const RobotKinematics3D& robot,const IKGoal& goal)
{
int m=IKGoal::NumDims(goal.posConstraint);
int n=IKGoal::NumDims(goal.rotConstraint);
Real poserr[3],orierr[3];
if(goal.destLink < 0)
goal.GetError(robot.links[goal.link].T_World,poserr,orierr);
else {
RigidTransform Trel;
Trel.mulInverseB(robot.links[goal.link].T_World,robot.links[goal.destLink].T_World);
goal.GetError(Trel,poserr,orierr);
}
Real emax=0.0;
for(int i=0;i<m;i++)
emax = Max(emax,Abs(poserr[i]));
for(int i=0;i<n;i++)
emax = Max(emax,Abs(orierr[i]));
return emax;
}
int ikcmp (const IKGoal& a,const IKGoal& b)
{
Assert(a.posConstraint==IKGoal::PosFixed && b.posConstraint==IKGoal::PosFixed);
int cmp;
if(int(a.rotConstraint) < int(b.rotConstraint)) return -1;
if(int(a.rotConstraint) > int(b.rotConstraint)) return 1;
if(a.rotConstraint == IKGoal::RotFixed) {
//only the transform matters
RigidTransform Ta,Tb;
a.GetFixedGoalTransform(Ta);
b.GetFixedGoalTransform(Tb);
if((cmp=vec3cmp_fuzzy(Ta.t,Tb.t))!=0) return cmp;
if((cmp=mat3cmp_fuzzy(Ta.R,Tb.R))!=0) return cmp;
}
else {
if((cmp=vec3cmp_fuzzy(a.endPosition,b.endPosition))!=0) return cmp;
if((cmp=vec3cmp_fuzzy(a.localPosition,b.localPosition))!=0) return cmp;
if(a.rotConstraint != IKGoal::RotNone) {
if((cmp=vec3cmp_fuzzy(a.endRotation,b.endRotation))!=0) return cmp;
if((cmp=vec3cmp_fuzzy(a.localAxis,b.localAxis))!=0) return cmp;
}
}
return 0;
}
string PlannerCommandInterface::SpaceballEvent(const RigidTransform& T)
{
if(currentLink >= 0) {
RigidTransform Tlink;
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Tlink = robot->links[currentLink].T_World;
IKGoal goal;
goal.link = currentLink;
goal.localPosition.setZero();
goal.SetFixedPosition(Tlink.t+T.t);
goal.SetFixedRotation(Tlink.R*T.R);
if(planner) {
CartesianObjective* obj=new CartesianObjective(robot);
obj->ikGoal = goal;
if(planner) planner->Reset(obj);
}
}
return "";
}
void GetConstraintPoints(const IKGoal& g,vector<Vector3>& lp,vector<Vector3>& wp)
{
Assert(g.posConstraint == IKGoal::PosFixed);
switch(g.rotConstraint) {
case IKGoal::RotNone:
lp.resize(1);
wp.resize(1);
lp[0] = g.localPosition;
wp[0] = g.endPosition;
break;
case IKGoal::RotAxis:
lp.resize(2);
wp.resize(2);
lp[0] = g.localPosition;
wp[0] = g.endPosition;
lp[1] = g.localPosition + g.localAxis;
wp[1] = g.endPosition + g.endRotation;
break;
case IKGoal::RotFixed:
lp.resize(3);
wp.resize(3);
lp[0] = g.localPosition;
wp[0] = g.endPosition;
{
RigidTransform T;
g.GetFixedGoalTransform(T);
lp[1] = g.localPosition;
lp[1].x += 1.0;
wp[1] = T * lp[1];
lp[2] = g.localPosition;
lp[2].y += 1.0;
wp[1] = T * lp[2];
}
break;
default:
fprintf(stderr,"Two-axis rotations not supported\n");
break;
}
}
void EvalIKError(const IKGoal& g,const RigidTransform& T,Real* poserr,Real* orierr)
{
g.GetError(T,poserr,orierr);
}
bool IntersectGoals(const IKGoal& a,const IKGoal& b,IKGoal& c,Real tolerance)
{
Assert(a.link == b.link);
Assert(a.destLink == b.destLink);
//fixed constraints get copied over if compatible
if(b.posConstraint == IKGoal::PosFixed && b.rotConstraint == IKGoal::RotFixed) {
RigidTransform T;
b.GetFixedGoalTransform(T);
Real poserr[3],roterr[3];
a.GetError(T,poserr,roterr);
for(int j=0;j<IKGoal::NumDims(a.posConstraint);j++)
if(poserr[j] > tolerance) return false;
for(int j=0;j<IKGoal::NumDims(a.rotConstraint);j++)
if(roterr[j] > tolerance) return false;
c = b;
return true;
}
if(a.posConstraint == IKGoal::PosFixed && a.rotConstraint == IKGoal::RotFixed) {
return IntersectGoals(b,a,c,tolerance);
}
//empty goals
if(a.posConstraint == IKGoal::PosNone && a.rotConstraint == IKGoal::RotNone) {
c = b;
return true;
}
if(b.posConstraint == IKGoal::PosNone && b.rotConstraint == IKGoal::RotNone) {
c = a;
return true;
}
c.link = a.link;
c.destLink = a.destLink;
if(a.posConstraint == IKGoal::PosFixed && b.posConstraint == IKGoal::PosFixed) {
//a fixes certain points and b fixes certain other points -- merge them together
vector<Vector3> aplocal,apworld,bplocal,bpworld;
GetConstraintPoints(a,aplocal,apworld);
GetConstraintPoints(b,bplocal,bpworld);
aplocal.insert(aplocal.end(),bplocal.begin(),bplocal.end());
apworld.insert(apworld.end(),bpworld.begin(),bpworld.end());
c.SetFromPoints(aplocal,apworld,tolerance);
return true;
}
//handle rotation constraints first
if(a.rotConstraint == IKGoal::RotNone) {
c.rotConstraint = b.rotConstraint;
c.localAxis = b.localAxis;
c.endRotation = b.endRotation;
}
else if(b.rotConstraint == IKGoal::RotNone) {
c.rotConstraint = a.rotConstraint;
c.localAxis = a.localAxis;
c.endRotation = a.endRotation;
}
else if(a.rotConstraint == IKGoal::RotFixed) {
c.rotConstraint = a.rotConstraint;
c.endRotation = a.endRotation;
//now determine compatibility
if(b.rotConstraint == IKGoal::RotFixed) {
if(!a.endRotation.isEqual(b.endRotation,tolerance)) return false;
}
else if(b.rotConstraint == IKGoal::RotAxis) {
MomentRotation m(a.endRotation);
Matrix3 R;
m.getMatrix(R);
if(!b.endRotation.isEqual(R*b.localAxis)) return false;
}
else {
fprintf(stderr,"Two-axis rotations not supported\n");
return false;
}
}
else if(b.rotConstraint == IKGoal::RotFixed) {
c.rotConstraint = b.rotConstraint;
c.endRotation = b.endRotation;
//now determine compatibility
if(a.rotConstraint == IKGoal::RotFixed) {
if(!a.endRotation.isEqual(b.endRotation,tolerance)) return false;
}
else if(a.rotConstraint == IKGoal::RotAxis) {
MomentRotation m(b.endRotation);
Matrix3 R;
m.getMatrix(R);
if(!a.endRotation.isEqual(R*a.localAxis,tolerance)) return false;
}
else {
fprintf(stderr,"Two-axis rotations not supported\n");
return false;
}
}
else if(a.rotConstraint == IKGoal::RotTwoAxis || b.rotConstraint == IKGoal::RotTwoAxis) {
fprintf(stderr,"Two-axis rotations not supported\n");
return false;
}
else {
Assert(a.rotConstraint == IKGoal::RotAxis && b.rotConstraint == IKGoal::RotAxis);
//if compatible, set a single axis constraint
//otherwise, set the proper rotation matrix
if((a.localAxis.isEqual(b.localAxis,tolerance) && a.endRotation.isEqual(b.endRotation,tolerance)) ||
(a.localAxis.isEqual(-b.localAxis,tolerance) && a.endRotation.isEqual(-b.endRotation,tolerance))) {
c.rotConstraint = IKGoal::RotAxis;
c.localAxis = a.localAxis;
c.endRotation = a.endRotation;
}
else {
//TODO find a rotation matrix that maps both axes to their directions
fprintf(stderr,"TODO: intersect two axis rotations\n");
return false;
}
}
//c's rotation is now chosen.
//now deal with the positions, at least one of which must be non-fixed
if(a.posConstraint == IKGoal::PosNone) {
//copy b's position constraint
c.posConstraint = b.posConstraint;
c.localPosition = b.localPosition;
c.endPosition = b.endPosition;
c.direction = b.direction;
return true;
}
else if(b.posConstraint == IKGoal::PosNone) {
//copy a's position constraint
c.posConstraint = a.posConstraint;
c.localPosition = a.localPosition;
c.endPosition = a.endPosition;
c.direction = a.direction;
return true;
}
if(c.rotConstraint == IKGoal::RotFixed) {
MomentRotation m(c.endRotation);
Matrix3 R;
m.getMatrix(R);
//check compatibility of fixed directions
if(a.posConstraint == IKGoal::PosFixed) {
c.posConstraint = a.posConstraint;
c.localPosition = a.localPosition;
c.endPosition = a.endPosition;
RigidTransform T;
c.GetFixedGoalTransform(T);
if(b.posConstraint == IKGoal::PosLinear) {
if(!cross(b.direction,T*b.localPosition-b.endPosition).isZero(tolerance)) return false;
}
else if(b.posConstraint == IKGoal::PosPlanar) {
if(!FuzzyZero(b.direction.dot(T*b.localPosition-b.endPosition),tolerance)) return false;
}
return true;
}
else if(b.posConstraint == IKGoal::PosFixed) {
c.posConstraint = b.posConstraint;
c.localPosition = b.localPosition;
c.endPosition = b.endPosition;
RigidTransform T;
c.GetFixedGoalTransform(T);
if(a.posConstraint == IKGoal::PosLinear) {
if(!cross(a.direction,T*a.localPosition-a.endPosition).isZero(tolerance)) return false;
}
else if(a.posConstraint == IKGoal::PosPlanar) {
if(!FuzzyZero(a.direction.dot(T*a.localPosition-a.endPosition),tolerance)) return false;
}
return true;
}
else {
//fixed rotation, linear or planar positions
fprintf(stderr,"TODO: merging linear or planar position constraints\n");
return false;
}
}
//check equality
if(a.posConstraint == b.posConstraint) {
if(a.localPosition.isEqual(b.localPosition,tolerance) &&
a.endPosition.isEqual(b.endPosition,tolerance) &&
a.direction.isEqual(b.direction,tolerance)) {
c.posConstraint = a.posConstraint;
c.localPosition = a.localPosition;
c.endPosition = a.endPosition;
return true;
}
}
//non-fixed rotation, linear or planar positions
fprintf(stderr,"TODO: merging linear or planar position constraints with non-fixed rotations\n");
return false;
}
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();
}
}
string PlannerCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
printf("Dragging event, dragging status %d\n",(int)dragging);
if(!dragging) {
//first click
Ray3D ray;
GetClickRay(mousex,mousey,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
dragging = true;
}
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp + ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
if(planner) {
CartesianObjective* obj=new CartesianObjective(robot);
obj->ikGoal = goal;
planner->Reset(obj);
}
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
printf("Stop dragging event, dragging status %d\n",(int)dragging);
dragging = false;
mousex=mx;
mousey=my;
return "";
}
}
