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