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