C++ (Cpp) IKGoal::SetFixedRotationの例

コード例 #1

ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim

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

コード例 #2

ファイル: userinterface.cpp プロジェクト: bbgw/RobotSim

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