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 "";
}
void EvaluateMultiPath(Robot& robot,const MultiPath& path,Real t,Config& q,Real xtol,Real contactol,int numIKIters)
{
RobotCSpace space(robot);;
RobotGeodesicManifold manifold(robot);
GeneralizedCubicBezierCurve curve(&space,&manifold);
Real duration,param;
int seg=path.Evaluate(t,curve,duration,param,MultiPath::InterpLinear);
if(seg < 0) seg = 0;
if(seg >= path.sections.size()) seg = (int)path.sections.size()-1;
curve.Eval(param,q);
//solve for constraints
bool solveIK = false;
if(!path.settings.contains("resolution"))
solveIK = true;
else {
Real res = path.settings.as<Real>("resolution");
if(res > xtol) solveIK=true;
}
if(solveIK) {
vector<IKGoal> ik;
path.GetIKProblem(ik,seg);
if(!ik.empty()) {
swap(q,robot.q);
robot.UpdateFrames();
int iters=numIKIters;
bool res=SolveIK(robot,ik,contactol,iters,0);
if(!res) printf("Warning, couldn't solve IK problem at sec %d, time %g\n",seg,t);
swap(q,robot.q);
}
}
}
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 "";
}
}
