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 "";
}
}
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;
}
}
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 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 "";
}
}
