예제 #1
0
controllerErrorCode ArmController::setHandCartesianPosLinear(double x, double y, double z, double roll, double pitch, double yaw, double timestep, double stepSize)
{

  double pos;
  //Set up controller
  setMode(CONTROLLER_POSITION);
  this->timeStep = timestep;
  this->stepSize = stepSize;

  //Calculate end position
  KDL::Vector endPos(x,y,z);
  KDL::Rotation endRot;
  endRot = endRot.RPY(roll,pitch,yaw);
  endFrame.p = endPos;
  endFrame.M = endRot;  

  //Get current location
  KDL::JntArray q = KDL::JntArray(pr2_kin.nJnts);

  //Get joint positions
  for(int i=0;i<ARM_MAX_JOINTS;i++){   
    armJointControllers[i].getPosAct(&pos);
    q(i) = pos;
  }

  KDL::Frame f;
  pr2_kin.FK(q,f);

  startPosition = f.p; //Record Starting location
  KDL::Vector move = endPos-startPosition; //Vector to ending position

  double dist = move.Norm(); //Distance to move
  moveDirection = move/dist; //Normalize movement vector
    
  rotInterpolator.SetStartEnd(f.M, endRot);
  double total_angle = rotInterpolator.Angle();

  nSteps = (int)(dist/stepSize);
  if(nSteps==0){
    std::cout<<"ArmController.cpp: Error:: number of steps calculated to be 0"<<std::endl;
    return CONTROLLER_COMPUTATION_ERROR;
  } 
  angleStep = total_angle/nSteps;
  lastTime= getTime(); //Record first time marker
  stepIndex = 0; //Reset step index

  linearInterpolation = true;
  return CONTROLLER_ALL_OK;
}
예제 #2
0
controllerErrorCode
ArmController::setHandCartesianPos(double x, double y, double z, double roll, double pitch, double yaw)
{	
  linearInterpolation = false;
  
  //Define position and rotation
  KDL::Vector position(x,y,z);
  KDL::Rotation rotation;
  rotation = rotation.RPY(roll,pitch,yaw);
  
  cmdPos[0] = x;
  cmdPos[1] = y;
  cmdPos[2] = z;
  cmdPos[3] = roll;
  cmdPos[4] = pitch;
  cmdPos[5] = yaw;

  //Create frame based on position and rotation
  KDL::Frame f;
  f.p = position;
  f.M = rotation;
    
  return commandCartesianPos(f);
 }